spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.8.4 release

pull/20780/head
Vehicle Researcher 2021-05-15 01:20:48 +00:00
parent 747ff45da4
commit 6418d4a09a
345 changed files with 10464 additions and 5438 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -10,6 +10,7 @@ venv/
.vscode*
model2.png
a.out
.hypothesis
*.dylib
*.DSYM
@ -59,6 +60,7 @@ notebooks
xx
hyperthneed
panda_jungle
provisioning
.coverage*
coverage.xml

51
Jenkinsfile vendored
View File

@ -36,7 +36,7 @@ EOF"""
def phone_steps(String device_type, steps) {
lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) {
timeout(time: 60, unit: 'MINUTES') {
timeout(time: 90, unit: 'MINUTES') {
phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),)
steps.each { item ->
phone(device_ip, item[0], item[1])
@ -83,7 +83,6 @@ pipeline {
}
}
stages {
/*
@ -113,6 +112,10 @@ pipeline {
stage('On-device Tests') {
agent {
docker {
/*
filename 'Dockerfile.ondevice_ci'
args "--privileged -v /dev:/dev --shm-size=1G --user=root"
*/
image 'python:3.7.3'
args '--user=root'
}
@ -121,17 +124,12 @@ pipeline {
stages {
stage('parallel tests') {
parallel {
stage('Devel Build') {
environment {
CI_PUSH = "${env.BRANCH_NAME == 'master' ? 'master-ci' : ' '}"
}
stage('Devel Tests') {
steps {
phone_steps("eon-build", [
["build", "SCONS_CACHE=1 scons -j4"],
["test athena", "nosetests -s selfdrive/athena/tests/test_athenad_old.py"],
["build devel", "cd release && SCONS_CACHE=1 DEVEL_TEST=1 ./build_devel.sh"],
["test manager", "python selfdrive/manager/test/test_manager.py"],
["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
["build devel", "cd release && CI_PUSH=${env.CI_PUSH} ./build_devel.sh"],
["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"],
])
}
@ -150,6 +148,7 @@ pipeline {
steps {
phone_steps("eon", [
["build", "SCONS_CACHE=1 scons -j4"],
["test athena", "nosetests -s selfdrive/athena/tests/test_athenad_old.py"],
["test sounds", "nosetests -s selfdrive/test/test_sounds.py"],
["test boardd loopback", "nosetests -s selfdrive/boardd/tests/test_boardd_loopback.py"],
["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
@ -160,6 +159,27 @@ pipeline {
}
}
/*
stage('Power Consumption Tests') {
steps {
lock(resource: "", label: "c2-zookeeper", inversePrecedence: true, variable: 'device_ip', quantity: 1) {
timeout(time: 90, unit: 'MINUTES') {
sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device"
phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),)
phone(device_ip, "build", "SCONS_CACHE=1 scons -j4 && sync")
sh script: "/home/batman/tools/zookeeper/disable.py $device_ip", label: "turn off device"
sh script: "/home/batman/tools/zookeeper/enable_and_wait.py $device_ip 120", label: "turn on device"
sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 3", label: "idle power consumption after boot"
sh script: "/home/batman/tools/zookeeper/ignition.py 1", label: "go onroad"
sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 10", label: "onroad power consumption"
sh script: "/home/batman/tools/zookeeper/ignition.py 0", label: "go offroad"
sh script: "/home/batman/tools/zookeeper/check_consumption.py 60 2", label: "idle power consumption offroad"
}
}
}
}
*/
stage('Tici Build') {
environment {
R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}"
@ -169,6 +189,7 @@ pipeline {
["build", "SCONS_CACHE=1 scons -j16"],
["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"],
["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
//["build release3-staging", "cd release && PUSH=${env.R3_PUSH} ./build_release3.sh"],
])
}
@ -196,6 +217,18 @@ pipeline {
}
}
stage('Push master-ci') {
when {
branch 'master'
}
steps {
phone_steps("eon-build", [
["push devel", "cd release && CI_PUSH='master-ci' ./build_devel.sh"],
])
}
}
}
post {

39
README.md
View File

@ -66,7 +66,7 @@ Supported Cars
| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
| Acura | ILX 2016-19 | AcuraWatch Plus | openpilot | 25mph<sup>1</sup> | 25mph |
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>1</sup> | 12mph |
| Acura | RDX 2020-21 | All | Stock | 0mph | 3mph |
| Acura | RDX 2019-21 | All | Stock | 0mph | 3mph |
| Honda | Accord 2018-20 | All | Stock | 0mph | 3mph |
| Honda | Accord Hybrid 2018-20 | All | Stock | 0mph | 3mph |
| Honda | Civic Hatchback 2017-21 | Honda Sensing | Stock | 0mph | 12mph |
@ -86,18 +86,18 @@ Supported Cars
| Hyundai | Palisade 2020-21 | All | Stock | 0mph | 0mph |
| Hyundai | Sonata 2020-21 | All | Stock | 0mph | 0mph |
| Lexus | CT Hybrid 2017-18 | LSS | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | ES 2019-20 | All | openpilot | 0mph | 0mph |
| Lexus | ES Hybrid 2018 | LSS | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph |
| Lexus | ES 2019-21 | All | openpilot | 0mph | 0mph |
| Lexus | ES Hybrid 2017-18 | LSS | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | ES Hybrid 2019-21 | All | openpilot | 0mph | 0mph |
| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph |
| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph |
| Lexus | NX 2018 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | NX 2020 | All | openpilot | 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-21 | All | openpilot | 0mph | 0mph |
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Lexus | RX Hybrid 2020 | All | openpilot | 0mph | 0mph |
| Toyota | Avalon 2016-18, 2020-21 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
| Toyota | Avalon 2016-21 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
| Toyota | Camry 2018-20 | All | Stock | 0mph<sup>4</sup> | 0mph |
| Toyota | Camry 2021 | All | openpilot | 0mph | 0mph |
| Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph<sup>4</sup> | 0mph |
@ -112,6 +112,7 @@ Supported Cars
| Toyota | Highlander 2020-21 | All | openpilot | 0mph | 0mph |
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>3</sup>| 0mph | 0mph |
| Toyota | Highlander Hybrid 2020-21 | All | openpilot | 0mph | 0mph |
| Toyota | Mirai 2021 | 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 |
@ -132,7 +133,8 @@ Community Maintained Cars and Features
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
| Audi | A3 2015, 2017 | Prestige | Stock | 0mph | 0mph |
| Audi | A3 2014-17 | Prestige | Stock | 0mph | 0mph |
| Audi | A3 Sportback e-tron 2017-18 | Prestige | Stock | 0mph | 0mph |
| Buick | Regal 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Cadillac | ATS 2018<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chevrolet | Malibu 2017<sup>1</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
@ -157,45 +159,48 @@ Community Maintained Cars and Features
| Hyundai | Veloster 2019 | SCC + LKAS | Stock | 5mph | 0mph |
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
| Kia | Forte 2018-19, 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Forte 2018-2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Niro EV 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph |
| Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Seltos 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Sorento 2018 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Nissan | Altima 2020 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Leaf 2018-20 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Rogue 2018-19 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph |
| SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Kodiaq 2018 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Superb 2018 | Driver Assistance | Stock | 0mph | 0mph |
| Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph |
| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Forester 2019-20 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph |
| Volkswagen| e-Golf 2014, 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Atlas 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf 2015-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf Alltrack 2017-18 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf GTE 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf GTI 2018-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf R 2016-19 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Golf SportsVan 2016 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta 2018-21 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Passat 2016-17<sup>2</sup> | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Tiguan 2020 | 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 ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
<sup>2</sup>Only includes the MQB Passat sold outside of North America. The NMS Passat made in Chattanooga TN is not yet supported.
Although it's not upstream, there's a community of people getting openpilot to run on Tesla's [here](https://tinkla.us/)
Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`.
To promote a car from community maintained, it must meet a few requirements. We must own one from the brand, we must sell the harness for it, has full ISO26262 in both panda and openpilot, there must be a path forward for longitudinal control, it must have AEB still enabled, and it must support fingerprinting 2.0
Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
Installation Instructions
------
@ -327,7 +332,7 @@ Directory Structure
.
├── cereal # The messaging spec and libs used for all logs
├── common # Library like functionality we've developed here
├── installer/updater # Manages auto-updates of openpilot
├── installer/updater # Manages auto-updates of NEOS
├── opendbc # Files showing how to interpret data from cars
├── panda # Code used to communicate on CAN
├── phonelibs # Libraries used on NEOS devices

10
RELEASES.md
View File

@ -1,3 +1,13 @@
Version 0.8.4 (2021-05-17)
========================
* Delay controls start until system is ready
* Fuzzy car identification, enabled with Community Features toggle
* Localizer optimized for increased precision and less CPU usage
* Retuned lateral control to be more aggressive when model is confident
* Toyota Mirai 2021 support
* Lexus NX 300 2020 support thanks to goesreallyfast!
* Volkswagen Atlas 2018-19 support thanks to jyoung8607!
Version 0.8.3 (2021-04-01)
========================
* New model

71
SConstruct
View File

@ -13,6 +13,10 @@ AddOption('--test',
action='store_true',
help='build test files')
AddOption('--kaitai',
action='store_true',
help='Regenerate kaitai struct parsers')
AddOption('--asan',
action='store_true',
help='turn on ASAN')
@ -79,6 +83,9 @@ if arch == "aarch64" or arch == "larch64":
"#phonelibs/libyuv/larch64/lib",
"/usr/lib/aarch64-linux-gnu"
]
cpppath += [
"#selfdrive/camerad/include",
]
cflags = ["-DQCOM2", "-mcpu=cortex-a57"]
cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"]
rpath = ["/usr/local/lib"]
@ -101,17 +108,22 @@ else:
cpppath = []
if arch == "Darwin":
yuv_dir = "mac" if real_arch != "arm64" else "mac_arm64"
libpath = [
"#phonelibs/libyuv/mac/lib",
"#cereal",
"#selfdrive/common",
f"#phonelibs/libyuv/{yuv_dir}/lib",
"/usr/local/lib",
"/opt/homebrew/lib",
"/usr/local/opt/openssl/lib",
"/opt/homebrew/opt/openssl/lib",
"/System/Library/Frameworks/OpenGL.framework/Libraries",
]
cflags += ["-DGL_SILENCE_DEPRECATION"]
cxxflags += ["-DGL_SILENCE_DEPRECATION"]
cpppath += ["/usr/local/opt/openssl/include"]
cpppath += [
"/opt/homebrew/include",
"/usr/local/opt/openssl/include",
"/opt/homebrew/opt/openssl/include"
]
else:
libpath = [
"#phonelibs/snpe/x86_64-linux-clang",
@ -141,6 +153,10 @@ else:
ccflags = []
ldflags = []
# no --as-needed on mac linker
if arch != "Darwin":
ldflags += ["-Wl,--as-needed"]
# change pythonpath to this
lenv["PYTHONPATH"] = Dir("#").path
@ -162,7 +178,6 @@ env = Environment(
CPPPATH=cpppath + [
"#",
"#selfdrive",
"#phonelibs/catch2/include",
"#phonelibs/bzip2",
"#phonelibs/libyuv/include",
@ -177,14 +192,7 @@ env = Environment(
"#phonelibs/snpe/include",
"#phonelibs/nanovg",
"#phonelibs/qrcode",
"#selfdrive/boardd",
"#selfdrive/common",
"#selfdrive/camerad",
"#selfdrive/camerad/include",
"#selfdrive/loggerd/include",
"#selfdrive/modeld",
"#selfdrive/sensord",
"#selfdrive/ui",
"#phonelibs",
"#cereal",
"#cereal/messaging",
"#cereal/visionipc",
@ -270,7 +278,10 @@ if arch != "aarch64":
qt_libs = []
if arch == "Darwin":
qt_env['QTDIR'] = "/usr/local/opt/qt@5"
if real_arch == "arm64":
qt_env['QTDIR'] = "/opt/homebrew/opt/qt@5"
else:
qt_env['QTDIR'] = "/usr/local/opt/qt@5"
qt_dirs = [
os.path.join(qt_env['QTDIR'], "include"),
]
@ -326,12 +337,8 @@ if GetOption("clazy"):
qt_env['CXX'] = 'clazy'
qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
Export('qt_env')
# still needed for apks
zmq = 'zmq'
Export('env', 'arch', 'real_arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY')
Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY')
# cereal and messaging are shared with the system
SConscript(['cereal/SConscript'])
@ -356,6 +363,28 @@ else:
Export('common', 'gpucommon', 'visionipc')
# Build rednose library and ekf models
rednose_config = {
'generated_folder': '#selfdrive/locationd/models/generated',
'to_build': {
'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']),
'car': ('#selfdrive/locationd/models/car_kf.py', True, []),
},
}
if arch != "aarch64":
rednose_config['to_build'].update({
'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []),
'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []),
'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, []),
'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, []),
})
Export('rednose_config')
SConscript(['rednose/SConscript'])
# Build openpilot
@ -384,16 +413,12 @@ SConscript(['selfdrive/clocksd/SConscript'])
SConscript(['selfdrive/loggerd/SConscript'])
SConscript(['selfdrive/locationd/SConscript'])
SConscript(['selfdrive/locationd/models/SConscript'])
SConscript(['selfdrive/sensord/SConscript'])
SConscript(['selfdrive/ui/SConscript'])
if arch != "Darwin":
SConscript(['selfdrive/logcatd/SConscript'])
if real_arch == "x86_64":
SConscript(['tools/nui/SConscript'])
external_sconscript = GetOption('external_sconscript')
if external_sconscript:
SConscript([external_sconscript])

9
cereal/SConscript
View File

@ -1,4 +1,4 @@
Import('env', 'envCython', 'arch', 'zmq', 'QCOM_REPLAY')
Import('env', 'envCython', 'arch', 'QCOM_REPLAY')
import shutil
@ -40,13 +40,6 @@ messaging_objects = env.SharedObject([
messaging_lib = env.Library('messaging', messaging_objects)
Depends('messaging/impl_zmq.cc', services_h)
# note, this rebuilds the deps shared, zmq is statically linked to make APK happy
# TODO: get APK to load system zmq to remove the static link
if arch == "aarch64":
zmq_static = FindFile("libzmq.a", "/usr/lib")
shared_lib_shared_lib = [zmq_static, 'm', 'stdc++', "gnustl_shared", "kj", "capnp"]
env.SharedLibrary('messaging_shared', messaging_objects, LIBS=shared_lib_shared_lib)
env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[messaging_lib, 'zmq'])
Depends('messaging/bridge.cc', services_h)

9
cereal/car.capnp
View File

@ -53,7 +53,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
lowSpeedLockout @31;
plannerError @32;
debugAlert @34;
steerTempUnavailableMute @35;
steerTempUnavailableUserOverride @35;
resumeRequired @36;
preDriverDistracted @37;
promptDriverDistracted @38;
@ -64,7 +64,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
belowSteerSpeed @46;
lowBattery @48;
vehicleModelInvalid @50;
controlsFailed @51;
accFaulted @51;
sensorDataInvalid @52;
commIssue @53;
tooDistracted @54;
@ -89,6 +89,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
startupNoCar @76;
startupNoControl @77;
startupMaster @78;
startupFuzzyFingerprint @97;
fcw @79;
steerSaturated @80;
belowEngageSpeed @84;
@ -101,6 +102,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
gpsMalfunction @94;
processNotRunning @95;
dashcamMode @96;
controlsInitializing @98;
radarCanErrorDEPRECATED @15;
radarCommIssueDEPRECATED @67;
@ -351,12 +353,14 @@ struct CarControl {
struct CarParams {
carName @0 :Text;
carFingerprint @1 :Text;
fuzzyFingerprint @55 :Bool;
enableGasInterceptor @2 :Bool;
enableCruise @3 :Bool;
enableCamera @4 :Bool;
enableDsu @5 :Bool; # driving support unit
enableApgs @6 :Bool; # advanced parking guidance system
enableBsm @56 :Bool; # blind spot monitoring
minEnableSpeed @7 :Float32;
minSteerSpeed @8 :Float32;
@ -412,6 +416,7 @@ struct CarParams {
dashcamOnly @41: Bool;
transmissionType @43 :TransmissionType;
carFw @44 :List(CarFw);
radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard
communityFeature @46: Bool; # true if a community maintained feature is detected
fingerprintSource @49: FingerprintSource;

7
cereal/log.capnp
View File

@ -1070,6 +1070,7 @@ struct UbloxGnss {
aStatus @2 :AntennaSupervisorState;
aPower @3 :AntennaPowerStatus;
jamInd @4 :UInt8;
flags @5 :UInt8;
enum AntennaSupervisorState {
init @0;
@ -1192,9 +1193,11 @@ struct DriverMonitoringState @0xb83cda094a1da284 {
struct Boot {
wallTimeNanos @0 :UInt64;
lastKmsg @1 :Data;
lastPmsg @2 :Data;
pstore @4 :Map(Text, Data);
launchLog @3 :Text;
lastKmsgDEPRECATED @1 :Data;
lastPmsgDEPRECATED @2 :Data;
}
struct LiveParametersData {

44
cereal/messaging/__init__.py
View File

@ -1,9 +1,11 @@
# must be build with scons
from .messaging_pyx import Context, Poller, SubSocket, PubSocket # pylint: disable=no-name-in-module, import-error
from .messaging_pyx import MultiplePublishersError, MessagingError # pylint: disable=no-name-in-module, import-error
import os
import capnp
from typing import Optional, List, Union
from collections import deque
from cereal import log
from cereal.services import service_list
@ -11,6 +13,9 @@ from cereal.services import service_list
assert MultiplePublishersError
assert MessagingError
AVG_FREQ_HISTORY = 100
SIMULATION = "SIMULATION" in os.environ
# sec_since_boot is faster, but allow to run standalone too
try:
from common.realtime import sec_since_boot
@ -126,12 +131,14 @@ def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader:
class SubMaster():
def __init__(self, services: List[str], poll: Optional[List[str]] = None,
ignore_alive: Optional[List[str]] = None, addr:str ="127.0.0.1"):
ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None,
addr: str = "127.0.0.1"):
self.frame = -1
self.updated = {s: False for s in services}
self.rcv_time = {s: 0. for s in services}
self.rcv_frame = {s: 0 for s in services}
self.alive = {s: False for s in services}
self.recv_dts = {s: deque([0.0] * AVG_FREQ_HISTORY, maxlen=AVG_FREQ_HISTORY) for s in services}
self.sock = {}
self.freq = {}
self.data = {}
@ -142,10 +149,8 @@ class SubMaster():
self.non_polled_services = [s for s in services if poll is not None and
len(poll) and s not in poll]
if ignore_alive is not None:
self.ignore_alive = ignore_alive
else:
self.ignore_alive = []
self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq
self.ignore_alive = [] if ignore_alive is None else ignore_alive
for s in services:
if addr is not None:
@ -184,20 +189,34 @@ class SubMaster():
s = msg.which()
self.updated[s] = True
if self.rcv_time[s] > 1e-5 and self.freq[s] > 1e-5 and (s not in self.non_polled_services) \
and (s not in self.ignore_average_freq):
self.recv_dts[s].append(cur_time - self.rcv_time[s])
self.rcv_time[s] = cur_time
self.rcv_frame[s] = self.frame
self.data[s] = getattr(msg, s)
self.logMonoTime[s] = msg.logMonoTime
self.valid[s] = msg.valid
for s in self.data:
# arbitrary small number to avoid float comparison. If freq is 0, we can skip the check
if self.freq[s] > 1e-5:
# alive if delay is within 10x the expected frequency
self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s])
else:
if SIMULATION:
self.alive[s] = True
if not SIMULATION:
for s in self.data:
# arbitrary small number to avoid float comparison. If freq is 0, we can skip the check
if self.freq[s] > 1e-5:
# alive if delay is within 10x the expected frequency
self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s])
# alive if average frequency is higher than 90% of expected frequency
avg_dt = sum(self.recv_dts[s]) / AVG_FREQ_HISTORY
expected_dt = 1 / (self.freq[s] * 0.90)
self.alive[s] = self.alive[s] and (avg_dt < expected_dt)
else:
self.alive[s] = True
def all_alive(self, service_list=None) -> bool:
if service_list is None: # check all
service_list = self.alive.keys()
@ -223,3 +242,6 @@ class PubMaster():
if not isinstance(dat, bytes):
dat = dat.to_bytes()
self.sock[s].send(dat)
def all_readers_updated(self, s: str) -> bool:
return self.sock[s].all_readers_updated()

4
cereal/messaging/bridge.cc
View File

@ -8,8 +8,8 @@ typedef void (*sighandler_t)(int sig);
#include "services.h"
#include "impl_msgq.hpp"
#include "impl_zmq.hpp"
#include "impl_msgq.h"
#include "impl_zmq.h"
void sigpipe_handler(int sig) {
assert(sig == SIGPIPE);

6
cereal/messaging/impl_msgq.cc
View File

@ -6,7 +6,7 @@
#include <cerrno>
#include "services.h"
#include "impl_msgq.hpp"
#include "impl_msgq.h"
volatile sig_atomic_t msgq_do_exit = 0;
@ -196,6 +196,10 @@ int MSGQPubSocket::send(char *data, size_t size){
return msgq_msg_send(&msg, q);
}
bool MSGQPubSocket::all_readers_updated() {
return msgq_all_readers_updated(q);
}
MSGQPubSocket::~MSGQPubSocket(){
if (q != NULL){
msgq_close_queue(q);

5
cereal/messaging/impl_msgq.hpp → cereal/messaging/impl_msgq.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "messaging.hpp"
#include "msgq.hpp"
#include "messaging.h"
#include "msgq.h"
#include <zmq.h>
#include <string>
@ -48,6 +48,7 @@ public:
int connect(Context *context, std::string endpoint, bool check_endpoint=true);
int sendMessage(Message *message);
int send(char *data, size_t size);
bool all_readers_updated();
~MSGQPubSocket();
};

7
cereal/messaging/impl_zmq.cc
View File

@ -7,7 +7,7 @@
#include <zmq.h>
#include "services.h"
#include "impl_zmq.hpp"
#include "impl_zmq.h"
static int get_port(std::string endpoint) {
int port = -1;
@ -131,6 +131,11 @@ int ZMQPubSocket::send(char *data, size_t size){
return zmq_send(sock, data, size, ZMQ_DONTWAIT);
}
bool ZMQPubSocket::all_readers_updated() {
assert(false); // TODO not implemented
return false;
}
ZMQPubSocket::~ZMQPubSocket(){
zmq_close(sock);
}

3
cereal/messaging/impl_zmq.hpp → cereal/messaging/impl_zmq.h
View File

@ -1,5 +1,5 @@
#pragma once
#include "messaging.hpp"
#include "messaging.h"
#include <zmq.h>
#include <string>
@ -47,6 +47,7 @@ public:
int connect(Context *context, std::string endpoint, bool check_endpoint=true);
int sendMessage(Message *message);
int send(char *data, size_t size);
bool all_readers_updated();
~ZMQPubSocket();
};

6
cereal/messaging/messaging.cc
View File

@ -1,6 +1,6 @@
#include "messaging.hpp"
#include "impl_zmq.hpp"
#include "impl_msgq.hpp"
#include "messaging.h"
#include "impl_zmq.h"
#include "impl_msgq.h"
#ifdef __APPLE__
const bool MUST_USE_ZMQ = true;

7
cereal/messaging/messaging.hpp → cereal/messaging/messaging.h
View File

@ -48,6 +48,7 @@ public:
virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0;
virtual int sendMessage(Message *message) = 0;
virtual int send(char *data, size_t size) = 0;
virtual bool all_readers_updated() = 0;
static PubSocket * create();
static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true);
static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true);
@ -67,7 +68,8 @@ class SubMaster {
public:
SubMaster(const std::initializer_list<const char *> &service_list,
const char *address = nullptr, const std::initializer_list<const char *> &ignore_alive = {});
int update(int timeout = 1000);
void update(int timeout = 1000);
void update_msgs(uint64_t current_time, std::vector<std::pair<std::string, cereal::Event::Reader>> messages);
inline bool allAlive(const std::initializer_list<const char *> &service_list = {}) { return all_(service_list, false, true); }
inline bool allValid(const std::initializer_list<const char *> &service_list = {}) { return all_(service_list, true, false); }
inline bool allAliveAndValid(const std::initializer_list<const char *> &service_list = {}) { return all_(service_list, true, true); }
@ -76,7 +78,10 @@ public:
uint64_t frame = 0;
bool updated(const char *name) const;
bool alive(const char *name) const;
bool valid(const char *name) const;
uint64_t rcv_frame(const char *name) const;
uint64_t rcv_time(const char *name) const;
cereal::Event::Reader &operator[](const char *name);
private:

3
cereal/messaging/messaging.pxd
View File

@ -6,7 +6,7 @@ from libcpp.vector cimport vector
from libcpp cimport bool
cdef extern from "messaging.hpp":
cdef extern from "messaging.h":
cdef cppclass Context:
@staticmethod
Context * create()
@ -31,6 +31,7 @@ cdef extern from "messaging.hpp":
int connect(Context *, string)
int sendMessage(Message *)
int send(char *, size_t)
bool all_readers_updated()
cdef cppclass Poller:
@staticmethod

3
cereal/messaging/messaging_pyx.pyx
View File

@ -149,3 +149,6 @@ cdef class PubSocket:
raise MultiplePublishersError
else:
raise MessagingError
def all_readers_updated(self):
return self.socket.all_readers_updated()

12
cereal/messaging/msgq.cc
View File

@ -21,7 +21,7 @@
#include <stdio.h>
#include "msgq.hpp"
#include "msgq.h"
void sigusr2_handler(int signal) {
assert(signal == SIGUSR2);
@ -452,3 +452,13 @@ int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout){
return num;
}
bool msgq_all_readers_updated(msgq_queue_t *q) {
uint64_t num_readers = *q->num_readers;
for (uint64_t i = 0; i < num_readers; i++) {
if (*q->read_valids[i] && *q->write_pointer != *q->read_pointers[i]) {
return false;
}
}
return num_readers > 0;
}

2
cereal/messaging/msgq.hpp → cereal/messaging/msgq.h
View File

@ -64,3 +64,5 @@ int msgq_msg_send(msgq_msg_t *msg, msgq_queue_t *q);
int msgq_msg_recv(msgq_msg_t *msg, msgq_queue_t *q);
int msgq_msg_ready(msgq_queue_t * q);
int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout);
bool msgq_all_readers_updated(msgq_queue_t *q);

63
cereal/messaging/socketmaster.cc
View File

@ -1,7 +1,12 @@
#include <assert.h>
#include <time.h>
#include "messaging.hpp"
#include <assert.h>
#include <stdlib.h>
#include <string>
#include "services.h"
#include "messaging.h"
const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1");
static inline uint64_t nanos_since_boot() {
struct timespec t;
@ -35,7 +40,7 @@ struct SubMaster::SubMessage {
std::string name;
SubSocket *socket = nullptr;
int freq = 0;
bool updated = false, alive = false, valid = false, ignore_alive;
bool updated = false, alive = false, valid = true, ignore_alive;
uint64_t rcv_time = 0, rcv_frame = 0;
void *allocated_msg_reader = nullptr;
capnp::FlatArrayMessageReader *msg_reader = nullptr;
@ -53,6 +58,7 @@ SubMaster::SubMaster(const std::initializer_list<const char *> &service_list, co
assert(socket != 0);
poller_->registerSocket(socket);
SubMessage *m = new SubMessage{
.name = name,
.socket = socket,
.freq = serv->frequency,
.ignore_alive = inList(ignore_alive, name),
@ -62,37 +68,54 @@ SubMaster::SubMaster(const std::initializer_list<const char *> &service_list, co
}
}
int SubMaster::update(int timeout) {
if (++frame == UINT64_MAX) frame = 1;
void SubMaster::update(int timeout) {
for (auto &kv : messages_) kv.second->updated = false;
int updated = 0;
auto sockets = poller_->poll(timeout);
uint64_t current_time = nanos_since_boot();
std::vector<std::pair<std::string, cereal::Event::Reader>> messages;
for (auto s : sockets) {
Message *msg = s->receive(true);
if (msg == nullptr) continue;
SubMessage *m = messages_.at(s);
if (m->msg_reader) {
m->msg_reader->~FlatArrayMessageReader();
}
m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.align(msg));
delete msg;
m->event = m->msg_reader->getRoot<cereal::Event>();
messages.push_back({m->name, m->msg_reader->getRoot<cereal::Event>()});
}
update_msgs(current_time, messages);
}
void SubMaster::update_msgs(uint64_t current_time, std::vector<std::pair<std::string, cereal::Event::Reader>> messages){
if (++frame == UINT64_MAX) frame = 1;
for(auto &kv : messages) {
auto m_find = services_.find(kv.first);
if (m_find == services_.end()){
continue;
}
SubMessage *m = m_find->second;
m->event = kv.second;
m->updated = true;
m->rcv_time = current_time;
m->rcv_frame = frame;
m->valid = m->event.getValid();
++updated;
if (SIMULATION) m->alive = true;
}
for (auto &kv : messages_) {
SubMessage *m = kv.second;
m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq));
if (!SIMULATION) {
for (auto &kv : messages_) {
SubMessage *m = kv.second;
m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq));
}
}
return updated;
}
bool SubMaster::all_(const std::initializer_list<const char *> &service_list, bool valid, bool alive) {
@ -100,7 +123,7 @@ bool SubMaster::all_(const std::initializer_list<const char *> &service_list, bo
for (auto &kv : messages_) {
SubMessage *m = kv.second;
if (service_list.size() == 0 || inList(service_list, m->name.c_str())) {
found += (!valid || m->valid) && (!alive || (m->alive && !m->ignore_alive));
found += (!valid || m->valid) && (!alive || (m->alive || m->ignore_alive));
}
}
return service_list.size() == 0 ? found == messages_.size() : found == service_list.size();
@ -123,10 +146,22 @@ bool SubMaster::updated(const char *name) const {
return services_.at(name)->updated;
}
bool SubMaster::alive(const char *name) const {
return services_.at(name)->alive;
}
bool SubMaster::valid(const char *name) const {
return services_.at(name)->valid;
}
uint64_t SubMaster::rcv_frame(const char *name) const {
return services_.at(name)->rcv_frame;
}
uint64_t SubMaster::rcv_time(const char *name) const {
return services_.at(name)->rcv_time;
}
cereal::Event::Reader &SubMaster::operator[](const char *name) {
return services_.at(name)->event;
};

105
cereal/services.py 100755 → 100644
View File

@ -3,6 +3,14 @@ import os
from typing import Optional
EON = os.path.isfile('/EON')
RESERVED_PORT = 8022 # sshd
STARTING_PORT = 8001
def new_port(port: int):
port += STARTING_PORT
return port + 1 if port >= RESERVED_PORT else port
class Service:
def __init__(self, port: int, should_log: bool, frequency: float, decimation: Optional[int] = None):
@ -11,55 +19,58 @@ class Service:
self.frequency = frequency
self.decimation = decimation
service_list = {
"roadCameraState": Service(8002, True, 20., 1),
"sensorEvents": Service(8003, True, 100., 100),
"gpsNMEA": Service(8004, True, 9.),
"deviceState": Service(8005, True, 2., 1),
"can": Service(8006, True, 100.),
"controlsState": Service(8007, True, 100., 100),
"features": Service(8010, True, 0.),
"pandaState": Service(8011, True, 2., 1),
"radarState": Service(8012, True, 20., 5),
"roadEncodeIdx": Service(8015, True, 20., 1),
"liveTracks": Service(8016, True, 20.),
"sendcan": Service(8017, True, 100.),
"logMessage": Service(8018, True, 0.),
"liveCalibration": Service(8019, True, 4., 4),
"androidLog": Service(8020, True, 0., 1),
"carState": Service(8021, True, 100., 10),
"carControl": Service(8023, True, 100., 10),
"longitudinalPlan": Service(8024, True, 20., 2),
"liveLocation": Service(8025, True, 0., 1),
"procLog": Service(8031, True, 0.5),
"gpsLocationExternal": Service(8032, True, 10., 1),
"ubloxGnss": Service(8033, True, 10.),
"clocks": Service(8034, True, 1., 1),
"liveMpc": Service(8035, False, 20.),
"liveLongitudinalMpc": Service(8036, False, 20.),
"ubloxRaw": Service(8042, True, 20.),
"liveLocationKalman": Service(8054, True, 20., 2),
"uiLayoutState": Service(8060, True, 0.),
"liveParameters": Service(8064, True, 20., 2),
"cameraOdometry": Service(8066, True, 20., 5),
"lateralPlan": Service(8067, True, 20., 2),
"thumbnail": Service(8069, True, 0.2, 1),
"carEvents": Service(8070, True, 1., 1),
"carParams": Service(8071, True, 0.02, 1),
"driverCameraState": Service(8072, True, 10. if EON else 20., 1),
"driverEncodeIdx": Service(8061, True, 10. if EON else 20., 1),
"driverState": Service(8063, True, 10. if EON else 20., 1),
"driverMonitoringState": Service(8073, True, 10. if EON else 20., 1),
"offroadLayout": Service(8074, False, 0.),
"wideRoadEncodeIdx": Service(8075, True, 20., 1),
"wideRoadCameraState": Service(8076, True, 20., 1),
"modelV2": Service(8077, True, 20., 20),
"managerState": Service(8078, True, 2., 1),
"testModel": Service(8040, False, 0.),
"testLiveLocation": Service(8045, False, 0.),
"testJoystick": Service(8056, False, 0.),
services = {
"roadCameraState": (True, 20., 1), # should_log, frequency, decimation (optional)
"sensorEvents": (True, 100., 100),
"gpsNMEA": (True, 9.),
"deviceState": (True, 2., 1),
"can": (True, 100.),
"controlsState": (True, 100., 100),
"features": (True, 0.),
"pandaState": (True, 2., 1),
"radarState": (True, 20., 5),
"roadEncodeIdx": (True, 20., 1),
"liveTracks": (True, 20.),
"sendcan": (True, 100.),
"logMessage": (True, 0.),
"liveCalibration": (True, 4., 4),
"androidLog": (True, 0., 1),
"carState": (True, 100., 10),
"carControl": (True, 100., 10),
"longitudinalPlan": (True, 20., 2),
"liveLocation": (True, 0., 1),
"procLog": (True, 0.5),
"gpsLocationExternal": (True, 10., 1),
"ubloxGnss": (True, 10.),
"clocks": (True, 1., 1),
"liveMpc": (False, 20.),
"liveLongitudinalMpc": (False, 20.),
"ubloxRaw": (True, 20.),
"liveLocationKalman": (True, 20., 2),
"uiLayoutState": (True, 0.),
"liveParameters": (True, 20., 2),
"cameraOdometry": (True, 20., 5),
"lateralPlan": (True, 20., 2),
"thumbnail": (True, 0.2, 1),
"carEvents": (True, 1., 1),
"carParams": (True, 0.02, 1),
"driverCameraState": (True, 10. if EON else 20., 1),
"driverEncodeIdx": (True, 10. if EON else 20., 1),
"driverState": (True, 10. if EON else 20., 1),
"driverMonitoringState": (True, 10. if EON else 20., 1),
"offroadLayout": (False, 0.),
"wideRoadEncodeIdx": (True, 20., 1),
"wideRoadCameraState": (True, 20., 1),
"modelV2": (True, 20., 20),
"managerState": (True, 2., 1),
"testModel": (False, 0.),
"testLiveLocation": (False, 0.),
"testJoystick": (False, 0.),
}
service_list = {name: Service(new_port(idx), *vals) for # type: ignore
idx, (name, vals) in enumerate(services.items())}
def build_header():

2
cereal/visionipc/visionipc_client.h
View File

@ -3,7 +3,7 @@
#include <string>
#include <unistd.h>
#include "messaging.hpp"
#include "messaging.h"
#include "visionipc.h"
#include "visionbuf.h"

2
cereal/visionipc/visionipc_server.cc
View File

@ -7,7 +7,7 @@
#include <sys/socket.h>
#include <unistd.h>
#include "messaging.hpp"
#include "messaging.h"
#include "ipc.h"
#include "visionipc_server.h"

2
cereal/visionipc/visionipc_server.h
View File

@ -5,7 +5,7 @@
#include <atomic>
#include <map>
#include "messaging.hpp"
#include "messaging.h"
#include "visionipc.h"
#include "visionbuf.h"

4
common/SConscript
View File

@ -1,4 +1,4 @@
Import('envCython')
Import('envCython', 'common')
envCython.Program('clock.so', 'clock.pyx')
envCython.Program('params_pyx.so', 'params_pyx.pyx')
envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [common, 'zmq'])

3
common/params.py
View File

@ -1,5 +1,6 @@
from common.params_pyx import Params, UnknownKeyName, put_nonblocking # pylint: disable=no-name-in-module, import-error
from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking # pylint: disable=no-name-in-module, import-error
assert Params
assert ParamKeyType
assert UnknownKeyName
assert put_nonblocking

15
common/params_pxd.pxd
View File

@ -8,9 +8,20 @@ cdef extern from "selfdrive/common/util.cc":
pass
cdef extern from "selfdrive/common/params.h":
cpdef enum ParamKeyType:
PERSISTENT
CLEAR_ON_MANAGER_START
CLEAR_ON_PANDA_DISCONNECT
CLEAR_ON_IGNITION
ALL
cdef cppclass Params:
Params(bool)
Params(string)
string get(string, bool) nogil
int delete_db_value(string)
int write_db_value(string, string)
bool getBool(string)
int remove(string)
int put(string, string)
int putBool(string, bool)
bool checkKey(string)
void clearAll(ParamKeyType)

124
common/params_pyx.pyx
View File

@ -2,88 +2,19 @@
# cython: language_level = 3
from libcpp cimport bool
from libcpp.string cimport string
from common.params_pxd cimport Params as c_Params
from common.params_pxd cimport Params as c_Params, ParamKeyType as c_ParamKeyType
import os
import threading
from common.basedir import BASEDIR
cdef enum TxType:
PERSISTENT = 1
CLEAR_ON_MANAGER_START = 2
CLEAR_ON_PANDA_DISCONNECT = 3
keys = {
b"AccessToken": [TxType.CLEAR_ON_MANAGER_START],
b"ApiCache_DriveStats": [TxType.PERSISTENT],
b"ApiCache_Device": [TxType.PERSISTENT],
b"ApiCache_Owner": [TxType.PERSISTENT],
b"AthenadPid": [TxType.PERSISTENT],
b"CalibrationParams": [TxType.PERSISTENT],
b"CarBatteryCapacity": [TxType.PERSISTENT],
b"CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"CommunityFeaturesToggle": [TxType.PERSISTENT],
b"EndToEndToggle": [TxType.PERSISTENT],
b"CompletedTrainingVersion": [TxType.PERSISTENT],
b"DisablePowerDown": [TxType.PERSISTENT],
b"DisableUpdates": [TxType.PERSISTENT],
b"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
b"DongleId": [TxType.PERSISTENT],
b"GitDiff": [TxType.PERSISTENT],
b"GitBranch": [TxType.PERSISTENT],
b"GitCommit": [TxType.PERSISTENT],
b"GitRemote": [TxType.PERSISTENT],
b"GithubSshKeys": [TxType.PERSISTENT],
b"GithubUsername": [TxType.PERSISTENT],
b"HardwareSerial": [TxType.PERSISTENT],
b"HasAcceptedTerms": [TxType.PERSISTENT],
b"HasCompletedSetup": [TxType.PERSISTENT],
b"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START],
b"IMEI": [TxType.PERSISTENT],
b"IsLdwEnabled": [TxType.PERSISTENT],
b"IsMetric": [TxType.PERSISTENT],
b"IsOffroad": [TxType.CLEAR_ON_MANAGER_START],
b"IsRHD": [TxType.PERSISTENT],
b"IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START],
b"IsUpdateAvailable": [TxType.CLEAR_ON_MANAGER_START],
b"IsUploadRawEnabled": [TxType.PERSISTENT],
b"LastAthenaPingTime": [TxType.PERSISTENT],
b"LastGPSPosition": [TxType.PERSISTENT],
b"LastUpdateException": [TxType.PERSISTENT],
b"LastUpdateTime": [TxType.PERSISTENT],
b"LiveParameters": [TxType.PERSISTENT],
b"OpenpilotEnabledToggle": [TxType.PERSISTENT],
b"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"Passive": [TxType.PERSISTENT],
b"RecordFront": [TxType.PERSISTENT],
b"RecordFrontLock": [TxType.PERSISTENT], # for the internal fleet
b"ReleaseNotes": [TxType.PERSISTENT],
b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START],
b"SubscriberInfo": [TxType.PERSISTENT],
b"SshEnabled": [TxType.PERSISTENT],
b"TermsVersion": [TxType.PERSISTENT],
b"Timezone": [TxType.PERSISTENT],
b"TrainingVersion": [TxType.PERSISTENT],
b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START],
b"UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START],
b"Version": [TxType.PERSISTENT],
b"VisionRadarToggle": [TxType.PERSISTENT],
b"Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START],
b"Offroad_ConnectivityNeededPrompt": [TxType.CLEAR_ON_MANAGER_START],
b"Offroad_TemperatureTooHigh": [TxType.CLEAR_ON_MANAGER_START],
b"Offroad_PandaFirmwareMismatch": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
b"Offroad_InvalidTime": [TxType.CLEAR_ON_MANAGER_START],
b"Offroad_IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START],
b"Offroad_NeosUpdate": [TxType.CLEAR_ON_MANAGER_START],
b"Offroad_UpdateFailed": [TxType.CLEAR_ON_MANAGER_START],
b"Offroad_HardwareUnsupported": [TxType.CLEAR_ON_MANAGER_START],
b"ForcePowerDown": [TxType.CLEAR_ON_MANAGER_START],
}
cdef class ParamKeyType:
PERSISTENT = c_ParamKeyType.PERSISTENT
CLEAR_ON_MANAGER_START = c_ParamKeyType.CLEAR_ON_MANAGER_START
CLEAR_ON_PANDA_DISCONNECT = c_ParamKeyType.CLEAR_ON_PANDA_DISCONNECT
CLEAR_ON_IGNITION = c_ParamKeyType.CLEAR_ON_IGNITION
ALL = c_ParamKeyType.ALL
def ensure_bytes(v):
if isinstance(v, str):
@ -108,23 +39,21 @@ cdef class Params:
del self.p
def clear_all(self, tx_type=None):
for key in keys:
if tx_type is None or tx_type in keys[key]:
self.delete(key)
if tx_type is None:
tx_type = ParamKeyType.ALL
def manager_start(self):
self.clear_all(TxType.CLEAR_ON_MANAGER_START)
self.p.clearAll(tx_type)
def panda_disconnect(self):
self.clear_all(TxType.CLEAR_ON_PANDA_DISCONNECT)
def get(self, key, block=False, encoding=None):
def check_key(self, key):
key = ensure_bytes(key)
if key not in keys:
if not self.p.checkKey(key):
raise UnknownKeyName(key)
cdef string k = key
return key
def get(self, key, block=False, encoding=None):
cdef string k = self.check_key(key)
cdef bool b = block
cdef string val
@ -144,6 +73,10 @@ cdef class Params:
else:
return val
def get_bool(self, key):
cdef string k = self.check_key(key)
return self.p.getBool(k)
def put(self, key, dat):
"""
Warning: This function blocks until the param is written to disk!
@ -151,23 +84,24 @@ cdef class Params:
Use the put_nonblocking helper function in time sensitive code, but
in general try to avoid writing params as much as possible.
"""
key = ensure_bytes(key)
cdef string k = self.check_key(key)
dat = ensure_bytes(dat)
self.p.put(k, dat)
if key not in keys:
raise UnknownKeyName(key)
self.p.write_db_value(key, dat)
def put_bool(self, key, val):
cdef string k = self.check_key(key)
self.p.putBool(k, val)
def delete(self, key):
key = ensure_bytes(key)
self.p.delete_db_value(key)
cdef string k = self.check_key(key)
self.p.remove(k)
def put_nonblocking(key, val, d=None):
def f(key, val):
params = Params(d)
params.put(key, val)
cdef string k = ensure_bytes(key)
params.put(k, val)
t = threading.Thread(target=f, args=(key, val))
t.start()

2
common/transformations/coordinates.cc
View File

@ -6,8 +6,6 @@
#include "coordinates.hpp"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
double a = 6378137; // lgtm [cpp/short-global-name]

6
common/transformations/coordinates.hpp
View File

@ -1,5 +1,8 @@
#pragma once
#define DEG2RAD(x) ((x) * M_PI / 180.0)
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
struct ECEF {
double x, y, z;
Eigen::Vector3d to_vector(){
@ -9,6 +12,9 @@ struct ECEF {
struct NED {
double n, e, d;
Eigen::Vector3d to_vector(){
return Eigen::Vector3d(n, e, d);
}
};
struct Geodetic {

3
common/transformations/orientation.cc
View File

@ -30,7 +30,8 @@ Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
// return {euler(2), euler(1), euler(0)};
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
double theta = asin(2 * (quat.w() * quat.y() - quat.z() * quat.x()));
double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
double theta = asin(asin_arg_clipped);
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
return {gamma, theta, psi};
}

BIN
installer/updater/updater
View File

Binary file not shown.

53
installer/updater/updater.cc
View File

@ -1,37 +1,33 @@
#include <sys/stat.h>
#include <sys/statvfs.h>
#include <unistd.h>
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cassert>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/statvfs.h>
#include <string>
#include <sstream>
#include <fstream>
#include <iostream>
#include <mutex>
#include <memory>
#include <mutex>
#include <sstream>
#include <string>
#include <thread>
#include <curl/curl.h>
#include <openssl/sha.h>
#include <GLES3/gl3.h>
#include <EGL/egl.h>
#include <EGL/eglext.h>
#include <GLES3/gl3.h>
#include "nanovg.h"
#define NANOVG_GLES3_IMPLEMENTATION
#include "json11.hpp"
#include "nanovg_gl.h"
#include "nanovg_gl_utils.h"
#include "json11.hpp"
#include "common/framebuffer.h"
#include "common/touch.h"
#include "common/util.h"
#include "selfdrive/common/framebuffer.h"
#include "selfdrive/common/touch.h"
#include "selfdrive/common/util.h"
#define USER_AGENT "NEOSUpdater-0.2"
@ -250,7 +246,12 @@ struct Updater {
b_y = 720;
b_h = 220;
state = CONFIRMATION;
if (download_stage(true)) {
state = RUNNING;
update_thread_handle = std::thread(&Updater::run_stages, this);
} else {
state = CONFIRMATION;
}
}
int download_file_xferinfo(curl_off_t dltotal, curl_off_t dlno,
@ -351,11 +352,15 @@ struct Updater {
state = RUNNING;
}
std::string download(std::string url, std::string hash, std::string name) {
std::string download(std::string url, std::string hash, std::string name, bool dry_run) {
std::string out_fn = UPDATE_DIR "/" + util::base_name(url);
// start or resume downloading if hash doesn't match
std::string fn_hash = sha256_file(out_fn);
if (dry_run) {
return (hash.compare(fn_hash) != 0) ? "" : out_fn;
}
// start or resume downloading if hash doesn't match
if (hash.compare(fn_hash) != 0) {
set_progress("Downloading " + name + "...");
bool r = download_file(url, out_fn);
@ -377,14 +382,14 @@ struct Updater {
return out_fn;
}
bool download_stage() {
bool download_stage(bool dry_run = false) {
curl = curl_easy_init();
assert(curl);
// ** quick checks before download **
if (!check_space()) {
set_error("2GB of free space required to update");
if (!dry_run) set_error("2GB of free space required to update");
return false;
}
@ -432,7 +437,7 @@ struct Updater {
printf("existing recovery hash: %s\n", existing_recovery_hash.c_str());
if (existing_recovery_hash != recovery_hash) {
recovery_fn = download(recovery_url, recovery_hash, "recovery");
recovery_fn = download(recovery_url, recovery_hash, "recovery", dry_run);
if (recovery_fn.empty()) {
// error'd
return false;
@ -441,7 +446,7 @@ struct Updater {
}
// ** handle ota download **
ota_fn = download(ota_url, ota_hash, "update");
ota_fn = download(ota_url, ota_hash, "update", dry_run);
if (ota_fn.empty()) {
//error'd
return false;

3
launch_chffrplus.sh
View File

@ -165,6 +165,9 @@ function launch {
# Remove orphaned git lock if it exists on boot
[ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock
# Pull time from panda
$DIR/selfdrive/boardd/set_time.py
# Check to see if there's a valid overlay-based update available. Conditions
# are as follows:
#

2
launch_env.sh
View File

@ -11,7 +11,7 @@ if [ -z "$REQUIRED_NEOS_VERSION" ]; then
fi
if [ -z "$AGNOS_VERSION" ]; then
export AGNOS_VERSION="0.11"
export AGNOS_VERSION="0.14"
fi
if [ -z "$PASSIVE" ]; then

2
opendbc/can/parser.cc
View File

@ -272,6 +272,8 @@ void CANParser::UpdateValid(uint64_t sec) {
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
if (state.seen > 0) {
DEBUG("0x%X TIMEOUT\n", state.address);
} else {
DEBUG("0x%X MISSING\n", state.address);
}
can_valid = false;
}

17
opendbc/can/parser_pyx.pyx
View File

@ -33,14 +33,14 @@ cdef class CANParser:
bool can_valid
int can_invalid_cnt
def __init__(self, dbc_name, signals, checks=None, bus=0):
def __init__(self, dbc_name, signals, checks=None, bus=0, enforce_checks=True):
if checks is None:
checks = []
self.can_valid = True
self.dbc_name = dbc_name
self.dbc = dbc_lookup(dbc_name)
if not self.dbc:
raise RuntimeError("Can't lookup" + dbc_name)
raise RuntimeError(f"Can't find DBC: {dbc_name}")
self.vl = {}
self.ts = {}
@ -74,6 +74,14 @@ cdef class CANParser:
c = (self.msg_name_to_address[name], c[1])
checks[i] = c
if enforce_checks:
checked_addrs = {c[0] for c in checks}
signal_addrs = {s[1] for s in signals}
unchecked = signal_addrs - checked_addrs
if len(unchecked):
err_msg = ', '.join(f"{self.address_to_msg_name[addr].decode()} ({hex(addr)})" for addr in unchecked)
raise RuntimeError(f"Unchecked addrs: {err_msg}")
cdef vector[SignalParseOptions] signal_options_v
cdef SignalParseOptions spo
for sig_name, sig_address, sig_default in signals:
@ -108,7 +116,6 @@ cdef class CANParser:
self.can_invalid_cnt = 0
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
for cv in can_values:
# Cast char * directly to unicode
name = <unicode>self.address_to_msg_name[cv.address].c_str()
@ -149,8 +156,8 @@ cdef class CANDefine():
self.dbc_name = dbc_name
self.dbc = dbc_lookup(dbc_name)
if not self.dbc:
raise RuntimeError("Can't lookup" + dbc_name)
raise RuntimeError(f"Can't find DBC: '{dbc_name}'")
num_vals = self.dbc[0].num_vals
address_to_msg_name = {}

2
opendbc/honda_civic_touring_2016_can_generated.dbc
View File

@ -346,6 +346,7 @@ BO_ 862 HIGHBEAM_CONTROL: 8 ADAS
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
BO_ 884 STALK_STATUS: 8 XXX
SG_ DASHBOARD_ALERT : 39|8@0+ (1,0) [0|255] "" EON
SG_ AUTO_HEADLIGHTS : 46|1@0+ (1,0) [0|1] "" EON
SG_ HIGH_BEAM_HOLD : 47|1@0+ (1,0) [0|1] "" EON
SG_ HIGH_BEAM_FLASH : 45|1@0+ (1,0) [0|1] "" EON
@ -389,6 +390,7 @@ VAL_ 545 ECON_ON_2 0 "off" 3 "on" ;
VAL_ 662 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ;
VAL_ 662 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
VAL_ 884 DASHBOARD_ALERT 0 "none" 51 "acc_problem" 55 "cmbs_problem" 75 "key_not_detected" 79 "fasten_seatbelt" 111 "lkas_problem" 131 "brake_system_problem" 132 "brake_hold_problem" 139 "tbd" 161 "door_open"
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 927 ACC_ALERTS 29 "esp_active_acc_canceled" 10 "b_pedal_applied" 9 "speed_too_low" 8 "speed_too_high" 7 "p_brake_applied" 6 "gear_no_d" 5 "seatbelt" 4 "too_steep_downhill" 3 "too_steep_uphill" 2 "too_close" 1 "no_vehicle_ahead" ;

7
opendbc/lexus_ct200h_2018_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/lexus_is_2018_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/lexus_nx300_2018_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/lexus_nx300h_2018_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/lexus_rx_350_2016_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_avalon_2017_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_corolla_2017_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_highlander_2017_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

22
opendbc/toyota_nodsu_hybrid_pt_generated.dbc
View File

@ -1,7 +1,20 @@
CM_ "AUTOGENERATED FILE, DO NOT EDIT";
CM_ "Imported file _toyota_nodsu_bsm.dbc starts here";
CM_ "Imported file _toyota_nodsu_common.dbc starts here";
BO_ 401 STEERING_LTA: 8 XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX
SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX
SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX
SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX
SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX
SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX
SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX
SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX
BO_ 1014 BSM: 8 XXX
SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX
SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX
@ -275,6 +288,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -285,6 +299,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -381,7 +399,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

35
opendbc/toyota_nodsu_pt_generated.dbc
View File

@ -1,7 +1,20 @@
CM_ "AUTOGENERATED FILE, DO NOT EDIT";
CM_ "Imported file _toyota_nodsu_bsm.dbc starts here";
CM_ "Imported file _toyota_nodsu_common.dbc starts here";
BO_ 401 STEERING_LTA: 8 XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX
SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX
SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX
SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX
SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX
SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX
SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX
SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX
BO_ 1014 BSM: 8 XXX
SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX
SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX
@ -275,6 +288,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -285,6 +299,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -381,7 +399,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
@ -392,19 +410,6 @@ CM_ "toyota_nodsu_pt.dbc starts here";
BO_ 401 STEERING_LTA: 8 XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX
SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX
SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX
SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX
SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX
SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX
SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX
SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX
SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX
BO_ 550 BRAKE_MODULE: 8 XXX
SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX

7
opendbc/toyota_prius_2017_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_rav4_2017_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

7
opendbc/toyota_sienna_xle_2018_pt_generated.dbc
View File

@ -258,6 +258,7 @@ BO_ 1553 UI_SETTING: 8 XXX
BO_ 1556 STEERING_LEVERS: 8 XXX
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX
BO_ 1568 SEATS_DOORS: 8 XXX
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
@ -268,6 +269,10 @@ BO_ 1568 SEATS_DOORS: 8 XXX
BO_ 1570 LIGHT_STALK: 8 SCM
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX
SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX
SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX
SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX
BO_ 1161 RSA1: 8 FCM
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
@ -364,7 +369,7 @@ VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter";
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";

1
panda/.gitignore vendored
View File

@ -3,6 +3,7 @@
.*.swo
*.o
*.so
*.os
*.d
*.dump
a.out

7
panda/board/SConscript
View File

@ -136,6 +136,11 @@ panda_env = Environment(
# Common autogenerated includes
version = f'const uint8_t gitversion[] = "{get_version(BUILDER, BUILD_TYPE)}";'
gitversion = panda_env.Textfile("obj/gitversion.h", [version, ""])
Ignore('bootstub.o', gitversion)
Requires('bootstub.o', gitversion)
Ignore('main.o', gitversion)
Requires('main.o', gitversion)
certs = [get_key_header(n) for n in ["debug", "release"]]
certheader = panda_env.Textfile("obj/cert.h", certs + [""])
@ -144,13 +149,11 @@ startup = panda_env.Object(STARTUP_FILE)
# Bootstub
crypto = ["../crypto/rsa.c", "../crypto/sha.c"]
bootstub_elf = panda_env.Program(f"obj/bootstub.{PROJECT}.elf", [startup] + crypto + ["bootstub.c"])
Requires(bootstub_elf, gitversion)
bootstub_bin = panda_env.Objcopy(f"obj/bootstub.{PROJECT}.bin", bootstub_elf)
# Build main
main_elf = panda_env.Program(f"obj/{PROJECT}.elf", [startup, MAIN],
LINKFLAGS=["-Wl,--section-start,.isr_vector=0x8004000"] + flags)
Requires(main_elf, gitversion)
main_bin = panda_env.Objcopy(f"obj/{PROJECT}.bin", main_elf)
# Sign main

5
panda/board/gpio.h
View File

@ -12,6 +12,7 @@ void jump_to_bootloader(void) {
void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)0x1fff0004));
// jump to bootloader
enable_interrupts();
bootloader();
// reset on exit
@ -21,14 +22,12 @@ void jump_to_bootloader(void) {
void early(void) {
// Reset global critical depth
disable_interrupts();
global_critical_depth = 0;
// Init register and interrupt tables
init_registers();
// neccesary for DFU flashing on a non-power cycled white panda
enable_interrupts();
// after it's been in the bootloader, things are initted differently, so we reset
if ((enter_bootloader_mode != BOOT_NORMAL) &&
(enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC) &&

41
panda/board/safety/safety_hyundai.h
View File

@ -17,11 +17,9 @@ const CanMsg HYUNDAI_TX_MSGS[] = {
// {1186, 0, 8} // 4a2SCC, Bus 0
};
// TODO: missing checksum for wheel speeds message,worst failure case is
// wheel speeds stuck at 0 and we don't disengage on brake press
AddrCheckStruct hyundai_rx_checks[] = {
{.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}}},
{.msg = {{902, 0, 8, .check_checksum = false, .max_counter = 15U, .expected_timestep = 10000U}}},
{.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}}},
{.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}}},
{.msg = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}}},
};
@ -63,6 +61,8 @@ static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
uint8_t chksum;
if (addr == 608) {
chksum = GET_BYTE(to_push, 7) & 0xF;
} else if (addr == 902) {
chksum = ((GET_BYTE(to_push, 7) >> 6) << 2) | (GET_BYTE(to_push, 5) >> 6);
} else if (addr == 916) {
chksum = GET_BYTE(to_push, 6) & 0xF;
} else if (addr == 1057) {
@ -77,15 +77,36 @@ static uint8_t hyundai_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
uint8_t chksum = 0;
// same algorithm, but checksum is in a different place
for (int i = 0; i < 8; i++) {
uint8_t b = GET_BYTE(to_push, i);
if (((addr == 608) && (i == 7)) || ((addr == 916) && (i == 6)) || ((addr == 1057) && (i == 7))) {
b &= (addr == 1057) ? 0x0FU : 0xF0U; // remove checksum
if (addr == 902) {
// count the bits
for (int i = 0; i < 8; i++) {
uint8_t b = GET_BYTE(to_push, i);
for (int j = 0; j < 8; j++) {
uint8_t bit = 0;
// exclude checksum and counter
if (((i != 1) || (j < 6)) && ((i != 3) || (j < 6)) && ((i != 5) || (j < 6)) && ((i != 7) || (j < 6))) {
bit = (b >> (uint8_t)j) & 1U;
}
chksum += bit;
}
}
chksum += (b % 16U) + (b / 16U);
chksum = (chksum ^ 9U) & 15U;
} else {
// sum of nibbles
for (int i = 0; i < 8; i++) {
if ((addr == 916) && (i == 7)) {
continue; // exclude
}
uint8_t b = GET_BYTE(to_push, i);
if (((addr == 608) && (i == 7)) || ((addr == 916) && (i == 6)) || ((addr == 1057) && (i == 7))) {
b &= (addr == 1057) ? 0x0FU : 0xF0U; // remove checksum
}
chksum += (b % 16U) + (b / 16U);
}
chksum = (16U - (chksum % 16U)) % 16U;
}
return (16U - (chksum % 16U)) % 16U;
return chksum;
}
static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

21
panda/board/safety/safety_toyota.h
View File

@ -30,9 +30,9 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 845;
#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2) // avg between 2 tracks
const CanMsg TOYOTA_TX_MSGS[] = {{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, // DSU bus 0
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, // DSU bus 1
{0x2E4, 0, 5}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, // LKAS + ACC
{0x200, 0, 6}}; // interceptor
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, // DSU bus 1
{0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, // LKAS + ACC
{0x200, 0, 6}}; // interceptor
AddrCheckStruct toyota_rx_checks[] = {
{.msg = {{ 0xaa, 0, 8, .check_checksum = false, .expected_timestep = 12000U}}},
@ -180,6 +180,21 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
}
// LTA steering check
// only sent to prevent dash errors, no actuation is accepted
if (addr == 0x191) {
// check the STEER_REQUEST, STEER_REQUEST_2, and STEER_ANGLE_CMD signals
bool lta_request = (GET_BYTE(to_send, 0) & 1) != 0;
bool lta_request2 = ((GET_BYTE(to_send, 3) >> 1) & 1) != 0;
int lta_angle = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);
lta_angle = to_signed(lta_angle, 16);
// block LTA msgs with actuation requests
if (lta_request || lta_request2 || (lta_angle != 0)) {
tx = 0;
}
}
// STEER: safety check on bytes 2-3
if (addr == 0x2E4) {
int desired_torque = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);

2
panda/python/__init__.py
View File

@ -179,7 +179,7 @@ class Panda(object):
self.bootstub = device.getProductID() == 0xddee
self.legacy = (device.getbcdDevice() != 0x2300)
self._handle = device.open()
if sys.platform not in ["win32", "cygwin", "msys"]:
if sys.platform not in ["win32", "cygwin", "msys", "darwin"]:
self._handle.setAutoDetachKernelDriver(True)
if claim:
self._handle.claimInterface(0)

2
panda/python/uds.py
View File

@ -240,7 +240,7 @@ _negative_response_codes = {
0x31: 'request out of range',
0x33: 'security access denied',
0x35: 'invalid key',
0x36: 'exceed numebr of attempts',
0x36: 'exceed number of attempts',
0x37: 'required time delay not expired',
0x70: 'upload download not accepted',
0x71: 'transfer data suspended',

2
phonelibs/SConscript
View File

@ -2,3 +2,5 @@ Import('env')
env.Library('json11', ['json11/json11.cpp'])
env.Append(CPPPATH=[Dir('json11')])
env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE'])

16
phonelibs/kaitai/custom_decoder.h 100644
View File

@ -0,0 +1,16 @@
#ifndef KAITAI_CUSTOM_DECODER_H
#define KAITAI_CUSTOM_DECODER_H
#include <string>
namespace kaitai {
class custom_decoder {
public:
virtual ~custom_decoder() {};
virtual std::string decode(std::string src) = 0;
};
}
#endif

189
phonelibs/kaitai/exceptions.h 100644
View File

@ -0,0 +1,189 @@
#ifndef KAITAI_EXCEPTIONS_H
#define KAITAI_EXCEPTIONS_H
#include <kaitai/kaitaistream.h>
#include <string>
#include <stdexcept>
// We need to use "noexcept" in virtual destructor of our exceptions
// subclasses. Different compilers have different ideas on how to
// achieve that: C++98 compilers prefer `throw()`, C++11 and later
// use `noexcept`. We define KS_NOEXCEPT macro for that.
#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900)
#define KS_NOEXCEPT noexcept
#else
#define KS_NOEXCEPT throw()
#endif
namespace kaitai {
/**
* Common ancestor for all error originating from Kaitai Struct usage.
* Stores KSY source path, pointing to an element supposedly guilty of
* an error.
*/
class kstruct_error: public std::runtime_error {
public:
kstruct_error(const std::string what, const std::string src_path):
std::runtime_error(src_path + ": " + what),
m_src_path(src_path)
{
}
virtual ~kstruct_error() KS_NOEXCEPT {};
protected:
const std::string m_src_path;
};
/**
* Error that occurs when default endianness should be decided with
* a switch, but nothing matches (although using endianness expression
* implies that there should be some positive result).
*/
class undecided_endianness_error: public kstruct_error {
public:
undecided_endianness_error(const std::string src_path):
kstruct_error("unable to decide on endianness for a type", src_path)
{
}
virtual ~undecided_endianness_error() KS_NOEXCEPT {};
};
/**
* Common ancestor for all validation failures. Stores pointer to
* KaitaiStream IO object which was involved in an error.
*/
class validation_failed_error: public kstruct_error {
public:
validation_failed_error(const std::string what, kstream* io, const std::string src_path):
kstruct_error("at pos " + kstream::to_string(static_cast<int>(io->pos())) + ": validation failed: " + what, src_path),
m_io(io)
{
}
// "at pos #{io.pos}: validation failed: #{msg}"
virtual ~validation_failed_error() KS_NOEXCEPT {};
protected:
kstream* m_io;
};
/**
* Signals validation failure: we required "actual" value to be equal to
* "expected", but it turned out that it's not.
*/
template<typename T>
class validation_not_equal_error: public validation_failed_error {
public:
validation_not_equal_error<T>(const T& expected, const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not equal", io, src_path),
m_expected(expected),
m_actual(actual)
{
}
// "not equal, expected #{expected.inspect}, but got #{actual.inspect}"
virtual ~validation_not_equal_error<T>() KS_NOEXCEPT {};
protected:
const T& m_expected;
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to be greater
* than or equal to "min", but it turned out that it's not.
*/
template<typename T>
class validation_less_than_error: public validation_failed_error {
public:
validation_less_than_error<T>(const T& min, const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not in range", io, src_path),
m_min(min),
m_actual(actual)
{
}
// "not in range, min #{min.inspect}, but got #{actual.inspect}"
virtual ~validation_less_than_error<T>() KS_NOEXCEPT {};
protected:
const T& m_min;
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to be less
* than or equal to "max", but it turned out that it's not.
*/
template<typename T>
class validation_greater_than_error: public validation_failed_error {
public:
validation_greater_than_error<T>(const T& max, const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not in range", io, src_path),
m_max(max),
m_actual(actual)
{
}
// "not in range, max #{max.inspect}, but got #{actual.inspect}"
virtual ~validation_greater_than_error<T>() KS_NOEXCEPT {};
protected:
const T& m_max;
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to be from
* the list, but it turned out that it's not.
*/
template<typename T>
class validation_not_any_of_error: public validation_failed_error {
public:
validation_not_any_of_error<T>(const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not any of the list", io, src_path),
m_actual(actual)
{
}
// "not any of the list, got #{actual.inspect}"
virtual ~validation_not_any_of_error<T>() KS_NOEXCEPT {};
protected:
const T& m_actual;
};
/**
* Signals validation failure: we required "actual" value to match
* the expression, but it turned out that it doesn't.
*/
template<typename T>
class validation_expr_error: public validation_failed_error {
public:
validation_expr_error<T>(const T& actual, kstream* io, const std::string src_path):
validation_failed_error("not matching the expression", io, src_path),
m_actual(actual)
{
}
// "not matching the expression, got #{actual.inspect}"
virtual ~validation_expr_error<T>() KS_NOEXCEPT {};
protected:
const T& m_actual;
};
}
#endif

689
phonelibs/kaitai/kaitaistream.cpp 100644
View File

@ -0,0 +1,689 @@
#include <kaitai/kaitaistream.h>
#if defined(__APPLE__)
#include <machine/endian.h>
#include <libkern/OSByteOrder.h>
#define bswap_16(x) OSSwapInt16(x)
#define bswap_32(x) OSSwapInt32(x)
#define bswap_64(x) OSSwapInt64(x)
#define __BYTE_ORDER BYTE_ORDER
#define __BIG_ENDIAN BIG_ENDIAN
#define __LITTLE_ENDIAN LITTLE_ENDIAN
#elif defined(_MSC_VER) // !__APPLE__
#include <stdlib.h>
#define __LITTLE_ENDIAN 1234
#define __BIG_ENDIAN 4321
#define __BYTE_ORDER __LITTLE_ENDIAN
#define bswap_16(x) _byteswap_ushort(x)
#define bswap_32(x) _byteswap_ulong(x)
#define bswap_64(x) _byteswap_uint64(x)
#else // !__APPLE__ or !_MSC_VER
#include <endian.h>
#include <byteswap.h>
#endif
#include <iostream>
#include <vector>
#include <stdexcept>
kaitai::kstream::kstream(std::istream* io) {
m_io = io;
init();
}
kaitai::kstream::kstream(std::string& data): m_io_str(data) {
m_io = &m_io_str;
init();
}
void kaitai::kstream::init() {
exceptions_enable();
align_to_byte();
}
void kaitai::kstream::close() {
// m_io->close();
}
void kaitai::kstream::exceptions_enable() const {
m_io->exceptions(
std::istream::eofbit |
std::istream::failbit |
std::istream::badbit
);
}
// ========================================================================
// Stream positioning
// ========================================================================
bool kaitai::kstream::is_eof() const {
if (m_bits_left > 0) {
return false;
}
char t;
m_io->exceptions(
std::istream::badbit
);
m_io->get(t);
if (m_io->eof()) {
m_io->clear();
exceptions_enable();
return true;
} else {
m_io->unget();
exceptions_enable();
return false;
}
}
void kaitai::kstream::seek(uint64_t pos) {
m_io->seekg(pos);
}
uint64_t kaitai::kstream::pos() {
return m_io->tellg();
}
uint64_t kaitai::kstream::size() {
std::iostream::pos_type cur_pos = m_io->tellg();
m_io->seekg(0, std::ios::end);
std::iostream::pos_type len = m_io->tellg();
m_io->seekg(cur_pos);
return len;
}
// ========================================================================
// Integer numbers
// ========================================================================
// ------------------------------------------------------------------------
// Signed
// ------------------------------------------------------------------------
int8_t kaitai::kstream::read_s1() {
char t;
m_io->get(t);
return t;
}
// ........................................................................
// Big-endian
// ........................................................................
int16_t kaitai::kstream::read_s2be() {
int16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_16(t);
#endif
return t;
}
int32_t kaitai::kstream::read_s4be() {
int32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_32(t);
#endif
return t;
}
int64_t kaitai::kstream::read_s8be() {
int64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ........................................................................
// Little-endian
// ........................................................................
int16_t kaitai::kstream::read_s2le() {
int16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_16(t);
#endif
return t;
}
int32_t kaitai::kstream::read_s4le() {
int32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_32(t);
#endif
return t;
}
int64_t kaitai::kstream::read_s8le() {
int64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ------------------------------------------------------------------------
// Unsigned
// ------------------------------------------------------------------------
uint8_t kaitai::kstream::read_u1() {
char t;
m_io->get(t);
return t;
}
// ........................................................................
// Big-endian
// ........................................................................
uint16_t kaitai::kstream::read_u2be() {
uint16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_16(t);
#endif
return t;
}
uint32_t kaitai::kstream::read_u4be() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_32(t);
#endif
return t;
}
uint64_t kaitai::kstream::read_u8be() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ........................................................................
// Little-endian
// ........................................................................
uint16_t kaitai::kstream::read_u2le() {
uint16_t t;
m_io->read(reinterpret_cast<char *>(&t), 2);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_16(t);
#endif
return t;
}
uint32_t kaitai::kstream::read_u4le() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_32(t);
#endif
return t;
}
uint64_t kaitai::kstream::read_u8le() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_64(t);
#endif
return t;
}
// ========================================================================
// Floating point numbers
// ========================================================================
// ........................................................................
// Big-endian
// ........................................................................
float kaitai::kstream::read_f4be() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_32(t);
#endif
return reinterpret_cast<float&>(t);
}
double kaitai::kstream::read_f8be() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __LITTLE_ENDIAN
t = bswap_64(t);
#endif
return reinterpret_cast<double&>(t);
}
// ........................................................................
// Little-endian
// ........................................................................
float kaitai::kstream::read_f4le() {
uint32_t t;
m_io->read(reinterpret_cast<char *>(&t), 4);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_32(t);
#endif
return reinterpret_cast<float&>(t);
}
double kaitai::kstream::read_f8le() {
uint64_t t;
m_io->read(reinterpret_cast<char *>(&t), 8);
#if __BYTE_ORDER == __BIG_ENDIAN
t = bswap_64(t);
#endif
return reinterpret_cast<double&>(t);
}
// ========================================================================
// Unaligned bit values
// ========================================================================
void kaitai::kstream::align_to_byte() {
m_bits_left = 0;
m_bits = 0;
}
uint64_t kaitai::kstream::read_bits_int_be(int n) {
int bits_needed = n - m_bits_left;
if (bits_needed > 0) {
// 1 bit => 1 byte
// 8 bits => 1 byte
// 9 bits => 2 bytes
int bytes_needed = ((bits_needed - 1) / 8) + 1;
if (bytes_needed > 8)
throw std::runtime_error("read_bits_int: more than 8 bytes requested");
char buf[8];
m_io->read(buf, bytes_needed);
for (int i = 0; i < bytes_needed; i++) {
uint8_t b = buf[i];
m_bits <<= 8;
m_bits |= b;
m_bits_left += 8;
}
}
// raw mask with required number of 1s, starting from lowest bit
uint64_t mask = get_mask_ones(n);
// shift mask to align with highest bits available in @bits
int shift_bits = m_bits_left - n;
mask <<= shift_bits;
// derive reading result
uint64_t res = (m_bits & mask) >> shift_bits;
// clear top bits that we've just read => AND with 1s
m_bits_left -= n;
mask = get_mask_ones(m_bits_left);
m_bits &= mask;
return res;
}
// Deprecated, use read_bits_int_be() instead.
uint64_t kaitai::kstream::read_bits_int(int n) {
return read_bits_int_be(n);
}
uint64_t kaitai::kstream::read_bits_int_le(int n) {
int bits_needed = n - m_bits_left;
if (bits_needed > 0) {
// 1 bit => 1 byte
// 8 bits => 1 byte
// 9 bits => 2 bytes
int bytes_needed = ((bits_needed - 1) / 8) + 1;
if (bytes_needed > 8)
throw std::runtime_error("read_bits_int_le: more than 8 bytes requested");
char buf[8];
m_io->read(buf, bytes_needed);
for (int i = 0; i < bytes_needed; i++) {
uint8_t b = buf[i];
m_bits |= (static_cast<uint64_t>(b) << m_bits_left);
m_bits_left += 8;
}
}
// raw mask with required number of 1s, starting from lowest bit
uint64_t mask = get_mask_ones(n);
// derive reading result
uint64_t res = m_bits & mask;
// remove bottom bits that we've just read by shifting
m_bits >>= n;
m_bits_left -= n;
return res;
}
uint64_t kaitai::kstream::get_mask_ones(int n) {
if (n == 64) {
return 0xFFFFFFFFFFFFFFFF;
} else {
return ((uint64_t) 1 << n) - 1;
}
}
// ========================================================================
// Byte arrays
// ========================================================================
std::string kaitai::kstream::read_bytes(std::streamsize len) {
std::vector<char> result(len);
// NOTE: streamsize type is signed, negative values are only *supposed* to not be used.
// http://en.cppreference.com/w/cpp/io/streamsize
if (len < 0) {
throw std::runtime_error("read_bytes: requested a negative amount");
}
if (len > 0) {
m_io->read(&result[0], len);
}
return std::string(result.begin(), result.end());
}
std::string kaitai::kstream::read_bytes_full() {
std::iostream::pos_type p1 = m_io->tellg();
m_io->seekg(0, std::ios::end);
std::iostream::pos_type p2 = m_io->tellg();
size_t len = p2 - p1;
// Note: this requires a std::string to be backed with a
// contiguous buffer. Officially, it's a only requirement since
// C++11 (C++98 and C++03 didn't have this requirement), but all
// major implementations had contiguous buffers anyway.
std::string result(len, ' ');
m_io->seekg(p1);
m_io->read(&result[0], len);
return result;
}
std::string kaitai::kstream::read_bytes_term(char term, bool include, bool consume, bool eos_error) {
std::string result;
std::getline(*m_io, result, term);
if (m_io->eof()) {
// encountered EOF
if (eos_error) {
throw std::runtime_error("read_bytes_term: encountered EOF");
}
} else {
// encountered terminator
if (include)
result.push_back(term);
if (!consume)
m_io->unget();
}
return result;
}
std::string kaitai::kstream::ensure_fixed_contents(std::string expected) {
std::string actual = read_bytes(expected.length());
if (actual != expected) {
// NOTE: I think printing it outright is not best idea, it could contain non-ascii charactes like backspace and beeps and whatnot. It would be better to print hexlified version, and also to redirect it to stderr.
throw std::runtime_error("ensure_fixed_contents: actual data does not match expected data");
}
return actual;
}
std::string kaitai::kstream::bytes_strip_right(std::string src, char pad_byte) {
std::size_t new_len = src.length();
while (new_len > 0 && src[new_len - 1] == pad_byte)
new_len--;
return src.substr(0, new_len);
}
std::string kaitai::kstream::bytes_terminate(std::string src, char term, bool include) {
std::size_t new_len = 0;
std::size_t max_len = src.length();
while (new_len < max_len && src[new_len] != term)
new_len++;
if (include && new_len < max_len)
new_len++;
return src.substr(0, new_len);
}
// ========================================================================
// Byte array processing
// ========================================================================
std::string kaitai::kstream::process_xor_one(std::string data, uint8_t key) {
size_t len = data.length();
std::string result(len, ' ');
for (size_t i = 0; i < len; i++)
result[i] = data[i] ^ key;
return result;
}
std::string kaitai::kstream::process_xor_many(std::string data, std::string key) {
size_t len = data.length();
size_t kl = key.length();
std::string result(len, ' ');
size_t ki = 0;
for (size_t i = 0; i < len; i++) {
result[i] = data[i] ^ key[ki];
ki++;
if (ki >= kl)
ki = 0;
}
return result;
}
std::string kaitai::kstream::process_rotate_left(std::string data, int amount) {
size_t len = data.length();
std::string result(len, ' ');
for (size_t i = 0; i < len; i++) {
uint8_t bits = data[i];
result[i] = (bits << amount) | (bits >> (8 - amount));
}
return result;
}
#ifdef KS_ZLIB
#include <zlib.h>
std::string kaitai::kstream::process_zlib(std::string data) {
int ret;
unsigned char *src_ptr = reinterpret_cast<unsigned char*>(&data[0]);
std::stringstream dst_strm;
z_stream strm;
strm.zalloc = Z_NULL;
strm.zfree = Z_NULL;
strm.opaque = Z_NULL;
ret = inflateInit(&strm);
if (ret != Z_OK)
throw std::runtime_error("process_zlib: inflateInit error");
strm.next_in = src_ptr;
strm.avail_in = data.length();
unsigned char outbuffer[ZLIB_BUF_SIZE];
std::string outstring;
// get the decompressed bytes blockwise using repeated calls to inflate
do {
strm.next_out = reinterpret_cast<Bytef*>(outbuffer);
strm.avail_out = sizeof(outbuffer);
ret = inflate(&strm, 0);
if (outstring.size() < strm.total_out)
outstring.append(reinterpret_cast<char*>(outbuffer), strm.total_out - outstring.size());
} while (ret == Z_OK);
if (ret != Z_STREAM_END) { // an error occurred that was not EOF
std::ostringstream exc_msg;
exc_msg << "process_zlib: error #" << ret << "): " << strm.msg;
throw std::runtime_error(exc_msg.str());
}
if (inflateEnd(&strm) != Z_OK)
throw std::runtime_error("process_zlib: inflateEnd error");
return outstring;
}
#endif
// ========================================================================
// Misc utility methods
// ========================================================================
int kaitai::kstream::mod(int a, int b) {
if (b <= 0)
throw std::invalid_argument("mod: divisor b <= 0");
int r = a % b;
if (r < 0)
r += b;
return r;
}
#include <stdio.h>
std::string kaitai::kstream::to_string(int val) {
// if int is 32 bits, "-2147483648" is the longest string representation
// => 11 chars + zero => 12 chars
// if int is 64 bits, "-9223372036854775808" is the longest
// => 20 chars + zero => 21 chars
char buf[25];
int got_len = snprintf(buf, sizeof(buf), "%d", val);
// should never happen, but check nonetheless
if (got_len > sizeof(buf))
throw std::invalid_argument("to_string: integer is longer than string buffer");
return std::string(buf);
}
#include <algorithm>
std::string kaitai::kstream::reverse(std::string val) {
std::reverse(val.begin(), val.end());
return val;
}
uint8_t kaitai::kstream::byte_array_min(const std::string val) {
uint8_t min = 0xff; // UINT8_MAX
std::string::const_iterator end = val.end();
for (std::string::const_iterator it = val.begin(); it != end; ++it) {
uint8_t cur = static_cast<uint8_t>(*it);
if (cur < min) {
min = cur;
}
}
return min;
}
uint8_t kaitai::kstream::byte_array_max(const std::string val) {
uint8_t max = 0; // UINT8_MIN
std::string::const_iterator end = val.end();
for (std::string::const_iterator it = val.begin(); it != end; ++it) {
uint8_t cur = static_cast<uint8_t>(*it);
if (cur > max) {
max = cur;
}
}
return max;
}
// ========================================================================
// Other internal methods
// ========================================================================
#ifndef KS_STR_DEFAULT_ENCODING
#define KS_STR_DEFAULT_ENCODING "UTF-8"
#endif
#ifdef KS_STR_ENCODING_ICONV
#include <iconv.h>
#include <cerrno>
#include <stdexcept>
std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
iconv_t cd = iconv_open(KS_STR_DEFAULT_ENCODING, src_enc.c_str());
if (cd == (iconv_t) -1) {
if (errno == EINVAL) {
throw std::runtime_error("bytes_to_str: invalid encoding pair conversion requested");
} else {
throw std::runtime_error("bytes_to_str: error opening iconv");
}
}
size_t src_len = src.length();
size_t src_left = src_len;
// Start with a buffer length of double the source length.
size_t dst_len = src_len * 2;
std::string dst(dst_len, ' ');
size_t dst_left = dst_len;
char *src_ptr = &src[0];
char *dst_ptr = &dst[0];
while (true) {
size_t res = iconv(cd, &src_ptr, &src_left, &dst_ptr, &dst_left);
if (res == (size_t) -1) {
if (errno == E2BIG) {
// dst buffer is not enough to accomodate whole string
// enlarge the buffer and try again
size_t dst_used = dst_len - dst_left;
dst_left += dst_len;
dst_len += dst_len;
dst.resize(dst_len);
// dst.resize might have allocated destination buffer in another area
// of memory, thus our previous pointer "dst" will be invalid; re-point
// it using "dst_used".
dst_ptr = &dst[dst_used];
} else {
throw std::runtime_error("bytes_to_str: iconv error");
}
} else {
// conversion successful
dst.resize(dst_len - dst_left);
break;
}
}
if (iconv_close(cd) != 0) {
throw std::runtime_error("bytes_to_str: iconv close error");
}
return dst;
}
#elif defined(KS_STR_ENCODING_NONE)
std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
return src;
}
#else
#error Need to decide how to handle strings: please define one of: KS_STR_ENCODING_ICONV, KS_STR_ENCODING_NONE
#endif

268
phonelibs/kaitai/kaitaistream.h 100644
View File

@ -0,0 +1,268 @@
#ifndef KAITAI_STREAM_H
#define KAITAI_STREAM_H
// Kaitai Struct runtime API version: x.y.z = 'xxxyyyzzz' decimal
#define KAITAI_STRUCT_VERSION 9000L
#include <istream>
#include <sstream>
#include <stdint.h>
#include <sys/types.h>
namespace kaitai {
/**
* Kaitai Stream class (kaitai::kstream) is an implementation of
* <a href="https://doc.kaitai.io/stream_api.html">Kaitai Struct stream API</a>
* for C++/STL. It's implemented as a wrapper over generic STL std::istream.
*
* It provides a wide variety of simple methods to read (parse) binary
* representations of primitive types, such as integer and floating
* point numbers, byte arrays and strings, and also provides stream
* positioning / navigation methods with unified cross-language and
* cross-toolkit semantics.
*
* Typically, end users won't access Kaitai Stream class manually, but would
* describe a binary structure format using .ksy language and then would use
* Kaitai Struct compiler to generate source code in desired target language.
* That code, in turn, would use this class and API to do the actual parsing
* job.
*/
class kstream {
public:
/**
* Constructs new Kaitai Stream object, wrapping a given std::istream.
* \param io istream object to use for this Kaitai Stream
*/
kstream(std::istream* io);
/**
* Constructs new Kaitai Stream object, wrapping a given in-memory data
* buffer.
* \param data data buffer to use for this Kaitai Stream
*/
kstream(std::string& data);
void close();
/** @name Stream positioning */
//@{
/**
* Check if stream pointer is at the end of stream. Note that the semantics
* are different from traditional STL semantics: one does *not* need to do a
* read (which will fail) after the actual end of the stream to trigger EOF
* flag, which can be accessed after that read. It is sufficient to just be
* at the end of the stream for this method to return true.
* \return "true" if we are located at the end of the stream.
*/
bool is_eof() const;
/**
* Set stream pointer to designated position.
* \param pos new position (offset in bytes from the beginning of the stream)
*/
void seek(uint64_t pos);
/**
* Get current position of a stream pointer.
* \return pointer position, number of bytes from the beginning of the stream
*/
uint64_t pos();
/**
* Get total size of the stream in bytes.
* \return size of the stream in bytes
*/
uint64_t size();
//@}
/** @name Integer numbers */
//@{
// ------------------------------------------------------------------------
// Signed
// ------------------------------------------------------------------------
int8_t read_s1();
// ........................................................................
// Big-endian
// ........................................................................
int16_t read_s2be();
int32_t read_s4be();
int64_t read_s8be();
// ........................................................................
// Little-endian
// ........................................................................
int16_t read_s2le();
int32_t read_s4le();
int64_t read_s8le();
// ------------------------------------------------------------------------
// Unsigned
// ------------------------------------------------------------------------
uint8_t read_u1();
// ........................................................................
// Big-endian
// ........................................................................
uint16_t read_u2be();
uint32_t read_u4be();
uint64_t read_u8be();
// ........................................................................
// Little-endian
// ........................................................................
uint16_t read_u2le();
uint32_t read_u4le();
uint64_t read_u8le();
//@}
/** @name Floating point numbers */
//@{
// ........................................................................
// Big-endian
// ........................................................................
float read_f4be();
double read_f8be();
// ........................................................................
// Little-endian
// ........................................................................
float read_f4le();
double read_f8le();
//@}
/** @name Unaligned bit values */
//@{
void align_to_byte();
uint64_t read_bits_int_be(int n);
uint64_t read_bits_int(int n);
uint64_t read_bits_int_le(int n);
//@}
/** @name Byte arrays */
//@{
std::string read_bytes(std::streamsize len);
std::string read_bytes_full();
std::string read_bytes_term(char term, bool include, bool consume, bool eos_error);
std::string ensure_fixed_contents(std::string expected);
static std::string bytes_strip_right(std::string src, char pad_byte);
static std::string bytes_terminate(std::string src, char term, bool include);
static std::string bytes_to_str(std::string src, std::string src_enc);
//@}
/** @name Byte array processing */
//@{
/**
* Performs a XOR processing with given data, XORing every byte of input with a single
* given value.
* @param data data to process
* @param key value to XOR with
* @return processed data
*/
static std::string process_xor_one(std::string data, uint8_t key);
/**
* Performs a XOR processing with given data, XORing every byte of input with a key
* array, repeating key array many times, if necessary (i.e. if data array is longer
* than key array).
* @param data data to process
* @param key array of bytes to XOR with
* @return processed data
*/
static std::string process_xor_many(std::string data, std::string key);
/**
* Performs a circular left rotation shift for a given buffer by a given amount of bits,
* using groups of 1 bytes each time. Right circular rotation should be performed
* using this procedure with corrected amount.
* @param data source data to process
* @param amount number of bits to shift by
* @return copy of source array with requested shift applied
*/
static std::string process_rotate_left(std::string data, int amount);
#ifdef KS_ZLIB
/**
* Performs an unpacking ("inflation") of zlib-compressed data with usual zlib headers.
* @param data data to unpack
* @return unpacked data
* @throws IOException
*/
static std::string process_zlib(std::string data);
#endif
//@}
/**
* Performs modulo operation between two integers: dividend `a`
* and divisor `b`. Divisor `b` is expected to be positive. The
* result is always 0 <= x <= b - 1.
*/
static int mod(int a, int b);
/**
* Converts given integer `val` to a decimal string representation.
* Should be used in place of std::to_string() (which is available only
* since C++11) in older C++ implementations.
*/
static std::string to_string(int val);
/**
* Reverses given string `val`, so that the first character becomes the
* last and the last one becomes the first. This should be used to avoid
* the need of local variables at the caller.
*/
static std::string reverse(std::string val);
/**
* Finds the minimal byte in a byte array, treating bytes as
* unsigned values.
* @param val byte array to scan
* @return minimal byte in byte array as integer
*/
static uint8_t byte_array_min(const std::string val);
/**
* Finds the maximal byte in a byte array, treating bytes as
* unsigned values.
* @param val byte array to scan
* @return maximal byte in byte array as integer
*/
static uint8_t byte_array_max(const std::string val);
private:
std::istream* m_io;
std::istringstream m_io_str;
int m_bits_left;
uint64_t m_bits;
void init();
void exceptions_enable() const;
static uint64_t get_mask_ones(int n);
static const int ZLIB_BUF_SIZE = 128 * 1024;
};
}
#endif

20
phonelibs/kaitai/kaitaistruct.h 100644
View File

@ -0,0 +1,20 @@
#ifndef KAITAI_STRUCT_H
#define KAITAI_STRUCT_H
#include <kaitai/kaitaistream.h>
namespace kaitai {
class kstruct {
public:
kstruct(kstream *_io) { m__io = _io; }
virtual ~kstruct() {}
protected:
kstream *m__io;
public:
kstream *_io() { return m__io; }
};
}
#endif

40
rednose/SConscript 100644
View File

@ -0,0 +1,40 @@
Import('env', 'envCython', 'arch', 'rednose_config')
generated_folder = rednose_config['generated_folder']
templates = Glob('#rednose/templates/*')
sympy_helpers = "#rednose/helpers/sympy_helpers.py"
ekf_sym = "#rednose/helpers/ekf_sym.py"
ekf_sym_pyx = "#rednose/helpers/ekf_sym_pyx.pyx"
ekf_sym_cc = env.Object("#rednose/helpers/ekf_sym.cc")
common_ekf = "#rednose/helpers/common_ekf.cc"
found = {}
for target, (command, combined_lib, extra_generated) in rednose_config['to_build'].items():
if File(command).exists():
found[target] = (command, combined_lib, extra_generated)
lib_target = [common_ekf]
for target, (command, combined_lib, extra_generated) in found.items():
target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h'])
extra_generated = [File(f'{generated_folder}/{x}') for x in extra_generated]
command_file = File(command)
env.Command(target_files + extra_generated,
[templates, command_file, sympy_helpers, ekf_sym],
command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath())
if combined_lib:
lib_target.append(target_files[0])
else:
env.SharedLibrary(f'{generated_folder}/' + target, [target_files[0], common_ekf])
libkf = env.SharedLibrary(f'{generated_folder}/libkf', lib_target)
lenv = envCython.Clone()
lenv["LINKFLAGS"] += [libkf[0].get_labspath()]
ekf_sym_so = lenv.Program('#rednose/helpers/ekf_sym_pyx.so', [ekf_sym_pyx, ekf_sym_cc, common_ekf])
lenv.Depends(ekf_sym_so, libkf)
Export('libkf')

9
rednose/helpers/__init__.py
View File

@ -13,14 +13,19 @@ def write_code(folder, name, code, header):
open(os.path.join(folder, f"{name}.h"), 'w').write(header)
def load_code(folder, name):
def load_code(folder, name, lib_name=None):
if lib_name is None:
lib_name = name
shared_ext = "dylib" if platform.system() == "Darwin" else "so"
shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}")
shared_fn = os.path.join(folder, f"lib{lib_name}.{shared_ext}")
header_fn = os.path.join(folder, f"{name}.h")
with open(header_fn) as f:
header = f.read()
# is the only thing that can be parsed by cffi
header = "\n".join([line for line in header.split("\n") if line.startswith("void ")])
ffi = FFI()
ffi.cdef(header)
return (ffi, ffi.dlopen(shared_fn))

19
rednose/helpers/common_ekf.cc 100644
View File

@ -0,0 +1,19 @@
#include "common_ekf.h"
std::vector<const EKF*>& get_ekfs() {
static std::vector<const EKF*> vec;
return vec;
}
void ekf_register(const EKF* ekf) {
get_ekfs().push_back(ekf);
}
const EKF* ekf_lookup(const std::string& ekf_name) {
for (const auto& ekfi : get_ekfs()) {
if (ekf_name == ekfi->name) {
return ekfi;
}
}
return NULL;
}

43
rednose/helpers/common_ekf.h 100644
View File

@ -0,0 +1,43 @@
#pragma once
#include <iostream>
#include <cassert>
#include <string>
#include <vector>
#include <deque>
#include <unordered_map>
#include <map>
#include <cmath>
#include <eigen3/Eigen/Dense>
typedef void (*extra_routine_t)(double *, double *);
struct EKF {
std::string name;
std::vector<int> kinds;
std::vector<int> feature_kinds;
void (*f_fun)(double *, double, double *);
void (*F_fun)(double *, double, double *);
void (*err_fun)(double *, double *, double *);
void (*inv_err_fun)(double *, double *, double *);
void (*H_mod_fun)(double *, double *);
void (*predict)(double *, double *, double *, double);
std::unordered_map<int, void (*)(double *, double *, double *)> hs = {};
std::unordered_map<int, void (*)(double *, double *, double *)> Hs = {};
std::unordered_map<int, void (*)(double *, double *, double *, double *, double *)> updates = {};
std::unordered_map<int, void (*)(double *, double *, double *)> Hes = {};
std::unordered_map<std::string, void (*)(double)> sets = {};
std::unordered_map<std::string, extra_routine_t> extra_routines = {};
};
std::vector<const EKF*>& get_ekfs();
const EKF* ekf_lookup(const std::string& ekf_name);
void ekf_register(const EKF* ekf);
#define ekf_init(ekf) \
static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \
ekf_register(&ekf); \
}

223
rednose/helpers/ekf_sym.cc 100644
View File

@ -0,0 +1,223 @@
#include "ekf_sym.h"
using namespace EKFS;
using namespace Eigen;
EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<MatrixXdr> P_initial, int dim_main,
int dim_main_err, int N, int dim_augment, int dim_augment_err, std::vector<int> maha_test_kinds,
std::vector<int> quaternion_idxs, std::vector<std::string> global_vars, double max_rewind_age)
{
// TODO: add logger
this->ekf = ekf_lookup(name);
assert(this->ekf);
this->msckf = N > 0;
this->N = N;
this->dim_augment = dim_augment;
this->dim_augment_err = dim_augment_err;
this->dim_main = dim_main;
this->dim_main_err = dim_main_err;
this->dim_x = x_initial.rows();
this->dim_err = P_initial.rows();
assert(dim_main + dim_augment * N == dim_x);
assert(dim_main_err + dim_augment_err * N == this->dim_err);
assert(Q.rows() == P_initial.rows() && Q.cols() == P_initial.cols());
// kinds that should get mahalanobis distance
// tested for outlier rejection
this->maha_test_kinds = maha_test_kinds;
// quaternions need normalization
this->quaternion_idxs = quaternion_idxs;
this->global_vars = global_vars;
// Process noise
this->Q = Q;
this->max_rewind_age = max_rewind_age;
this->init_state(x_initial, P_initial, NAN);
}
void EKFSym::init_state(Map<VectorXd> state, Map<MatrixXdr> covs, double filter_time) {
this->x = state;
this->P = covs;
this->filter_time = filter_time;
this->augment_times = VectorXd::Zero(this->N);
this->reset_rewind();
}
VectorXd EKFSym::state() {
return this->x;
}
MatrixXdr EKFSym::covs() {
return this->P;
}
void EKFSym::set_filter_time(double t) {
this->filter_time = t;
}
double EKFSym::get_filter_time() {
return this->filter_time;
}
void EKFSym::normalize_quaternions() {
for(std::size_t i = 0; i < this->quaternion_idxs.size(); ++i) {
this->normalize_slice(this->quaternion_idxs[i], this->quaternion_idxs[i] + 4);
}
}
void EKFSym::normalize_slice(int slice_start, int slice_end_ex) {
this->x.block(slice_start, 0, slice_end_ex - slice_start, this->x.cols()).normalize();
}
void EKFSym::set_global(std::string global_var, double val) {
this->ekf->sets.at(global_var)(val);
}
std::optional<Estimate> EKFSym::predict_and_update_batch(double t, int kind, std::vector<Map<VectorXd>> z_map,
std::vector<Map<MatrixXdr>> R_map, std::vector<std::vector<double>> extra_args, bool augment)
{
// TODO handle rewinding at this level
std::deque<Observation> rewound;
if (!std::isnan(this->filter_time) && t < this->filter_time) {
if (this->rewind_t.empty() || t < this->rewind_t.front() || t < this->rewind_t.back() - this->max_rewind_age) {
std::cout << "observation too old at " << t << " with filter at " << this->filter_time << ", ignoring" << std::endl;
return std::nullopt;
}
rewound = this->rewind(t);
}
Observation obs;
obs.t = t;
obs.kind = kind;
obs.extra_args = extra_args;
for (Map<VectorXd> zi : z_map) {
obs.z.push_back(zi);
}
for (Map<MatrixXdr> Ri : R_map) {
obs.R.push_back(Ri);
}
std::optional<Estimate> res = std::make_optional(this->predict_and_update_batch(obs, augment));
// optional fast forward
while (!rewound.empty()) {
this->predict_and_update_batch(rewound.front(), false);
rewound.pop_front();
}
return res;
}
void EKFSym::reset_rewind() {
this->rewind_obscache.clear();
this->rewind_t.clear();
this->rewind_states.clear();
}
std::deque<Observation> EKFSym::rewind(double t) {
std::deque<Observation> rewound;
// rewind observations until t is after previous observation
while (this->rewind_t.back() > t) {
rewound.push_front(this->rewind_obscache.back());
this->rewind_t.pop_back();
this->rewind_states.pop_back();
this->rewind_obscache.pop_back();
}
// set the state to the time right before that
this->filter_time = this->rewind_t.back();
this->x = this->rewind_states.back().first;
this->P = this->rewind_states.back().second;
return rewound;
}
void EKFSym::checkpoint(Observation& obs) {
// push to rewinder
this->rewind_t.push_back(this->filter_time);
this->rewind_states.push_back(std::make_pair(this->x, this->P));
this->rewind_obscache.push_back(obs);
// only keep a certain number around
if (this->rewind_t.size() > REWIND_TO_KEEP) {
this->rewind_t.pop_front();
this->rewind_states.pop_front();
this->rewind_obscache.pop_front();
}
}
Estimate EKFSym::predict_and_update_batch(Observation& obs, bool augment) {
assert(obs.z.size() == obs.R.size());
assert(obs.z.size() == obs.extra_args.size());
this->predict(obs.t);
Estimate res;
res.t = obs.t;
res.kind = obs.kind;
res.z = obs.z;
res.extra_args = obs.extra_args;
res.xk1 = this->x;
res.Pk1 = this->P;
// update batch
std::vector<VectorXd> y;
for (int i = 0; i < obs.z.size(); i++) {
assert(obs.z[i].rows() == obs.R[i].rows());
assert(obs.z[i].rows() == obs.R[i].cols());
// update state
y.push_back(this->update(obs.kind, obs.z[i], obs.R[i], obs.extra_args[i]));
}
res.xk = this->x;
res.Pk = this->P;
res.y = y;
assert(!augment); // TODO
// if (augment) {
// this->augment();
// }
this->checkpoint(obs);
return res;
}
void EKFSym::predict(double t) {
// initialize time
if (std::isnan(this->filter_time)) {
this->filter_time = t;
}
// predict
double dt = t - this->filter_time;
assert(dt >= 0.0);
this->ekf->predict(this->x.data(), this->P.data(), this->Q.data(), dt);
this->normalize_quaternions();
this->filter_time = t;
}
VectorXd EKFSym::update(int kind, VectorXd z, MatrixXdr R, std::vector<double> extra_args) {
this->ekf->updates.at(kind)(this->x.data(), this->P.data(), z.data(), R.data(), extra_args.data());
this->normalize_quaternions();
if (this->msckf && std::find(this->feature_track_kinds.begin(), this->feature_track_kinds.end(), kind) != this->feature_track_kinds.end()) {
return z.head(z.rows() - extra_args.size());
}
return z;
}
extra_routine_t EKFSym::get_extra_routine(const std::string& routine) {
return this->ekf->extra_routines.at(routine);
}

112
rednose/helpers/ekf_sym.h 100644
View File

@ -0,0 +1,112 @@
#pragma once
#include <iostream>
#include <cassert>
#include <string>
#include <vector>
#include <deque>
#include <unordered_map>
#include <map>
#include <cmath>
#include <optional>
#include <eigen3/Eigen/Dense>
#include "common_ekf.h"
#define REWIND_TO_KEEP 512
namespace EKFS {
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXdr;
typedef struct Observation {
double t;
int kind;
std::vector<Eigen::VectorXd> z;
std::vector<MatrixXdr> R;
std::vector<std::vector<double>> extra_args;
} Observation;
typedef struct Estimate {
Eigen::VectorXd xk1;
Eigen::VectorXd xk;
MatrixXdr Pk1;
MatrixXdr Pk;
double t;
int kind;
std::vector<Eigen::VectorXd> y;
std::vector<Eigen::VectorXd> z;
std::vector<std::vector<double>> extra_args;
} Estimate;
class EKFSym {
public:
EKFSym(std::string name, Eigen::Map<MatrixXdr> Q, Eigen::Map<Eigen::VectorXd> x_initial,
Eigen::Map<MatrixXdr> P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0,
int dim_augment_err = 0, std::vector<int> maha_test_kinds = std::vector<int>(),
std::vector<int> quaternion_idxs = std::vector<int>(),
std::vector<std::string> global_vars = std::vector<std::string>(), double max_rewind_age = 1.0);
void init_state(Eigen::Map<Eigen::VectorXd> state, Eigen::Map<MatrixXdr> covs, double filter_time);
Eigen::VectorXd state();
MatrixXdr covs();
void set_filter_time(double t);
double get_filter_time();
void normalize_quaternions();
void normalize_slice(int slice_start, int slice_end_ex);
void set_global(std::string global_var, double val);
void reset_rewind();
void predict(double t);
std::optional<Estimate> predict_and_update_batch(double t, int kind, std::vector<Eigen::Map<Eigen::VectorXd>> z,
std::vector<Eigen::Map<MatrixXdr>> R, std::vector<std::vector<double>> extra_args = {{}}, bool augment = false);
extra_routine_t get_extra_routine(const std::string& routine);
private:
std::deque<Observation> rewind(double t);
void checkpoint(Observation& obs);
Estimate predict_and_update_batch(Observation& obs, bool augment);
Eigen::VectorXd update(int kind, Eigen::VectorXd z, MatrixXdr R, std::vector<double> extra_args);
// stuct with linked sympy generated functions
const EKF *ekf = NULL;
Eigen::VectorXd x; // state
MatrixXdr P; // covs
bool msckf;
int N;
int dim_augment;
int dim_augment_err;
int dim_main;
int dim_main_err;
// state
int dim_x;
int dim_err;
double filter_time;
std::vector<int> maha_test_kinds;
std::vector<int> quaternion_idxs;
std::vector<std::string> global_vars;
// process noise
MatrixXdr Q;
// rewind stuff
double max_rewind_age;
std::deque<double> rewind_t;
std::deque<std::pair<Eigen::VectorXd, MatrixXdr>> rewind_states;
std::deque<Observation> rewind_obscache;
Eigen::VectorXd augment_times;
std::vector<int> feature_track_kinds;
};
}

188
rednose/helpers/ekf_sym.py
View File

@ -7,7 +7,7 @@ import sympy as sp
from numpy import dot
from rednose.helpers.sympy_helpers import sympy_into_c
from rednose.helpers import (TEMPLATE_DIR, load_code, write_code)
from rednose.helpers import TEMPLATE_DIR, load_code
from rednose.helpers.chi2_lookup import chi2_ppf
@ -27,7 +27,7 @@ def null(H, eps=1e-12):
def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, # pylint: disable=dangerous-default-value
maha_test_kinds=[], global_vars=None):
maha_test_kinds=[], quaternion_idxs=[], global_vars=None, extra_routines=[]):
# optional state transition matrix, H modifier
# and err_function if an error-state kalman filter (ESKF)
# is desired. Best described in "Quaternion kinematics
@ -91,6 +91,9 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
# collect sympy functions
sympy_functions = []
# extra routines
sympy_functions += extra_routines
# error functions
sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]]))
sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]]))
@ -110,15 +113,26 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
# Generate and wrap all th c code
header, code = sympy_into_c(sympy_functions, global_vars)
extra_header = "#define DIM %d\n" % dim_x
extra_header += "#define EDIM %d\n" % dim_err
extra_header += "#define MEDIM %d\n" % dim_main_err
extra_header += "typedef void (*Hfun)(double *, double *, double *);\n"
sympy_header, code = sympy_into_c(sympy_functions, global_vars)
extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);"
header = "#pragma once\n"
header += "#include \"rednose/helpers/common_ekf.h\"\n"
header += "extern \"C\" {\n"
extra_post = ""
pre_code = f"#include \"{name}.h\"\n"
pre_code += "\nnamespace {\n"
pre_code += "#define DIM %d\n" % dim_x
pre_code += "#define EDIM %d\n" % dim_err
pre_code += "#define MEDIM %d\n" % dim_main_err
pre_code += "typedef void (*Hfun)(double *, double *, double *);\n"
if global_vars is not None:
for var in global_vars:
pre_code += f"\ndouble {var.name};\n"
pre_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n"
post_code = "\n}\n" # namespace
post_code += "extern \"C\" {\n\n"
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
if msckf and kind in feature_track_kinds:
@ -129,36 +143,80 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
# ea_dim = 1 # not really dim of ea but makes c function work
maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_test = kind in maha_test_kinds
extra_post += """
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d);
}
""" % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind)
extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh)
extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind
code += '\nextern "C"{\n' + extra_header + "\n}\n"
code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read()
code += '\nextern "C"{\n' + extra_post + "\n}\n"
pre_code += f"const static double MAHA_THRESH_{kind} = {maha_thresh};\n"
header += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);\n"
post_code += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {{\n"
post_code += f" update<{h_sym.shape[0]}, 3, {int(maha_test)}>(in_x, in_P, h_{kind}, H_{kind}, {He_str}, in_z, in_R, in_ea, MAHA_THRESH_{kind});\n"
post_code += "}\n"
# For ffi loading of specific functions
for line in sympy_header.split("\n"):
if line.startswith("void "): # sympy functions
func_call = line[5: line.index(')') + 1]
header += f"void {name}_{func_call};\n"
post_code += f"void {name}_{func_call} {{\n"
post_code += f" {func_call.replace('double *', '').replace('double', '')};\n"
post_code += "}\n"
header += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt);\n"
post_code += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt) {{\n"
post_code += " predict(in_x, in_P, in_Q, dt);\n"
post_code += "}\n"
if global_vars is not None:
global_code = '\nextern "C"{\n'
for var in global_vars:
global_code += f"\ndouble {var.name};\n"
global_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n"
extra_header += f"\nvoid set_{var.name}(double x);\n"
header += f"void {name}_set_{var.name}(double x);\n"
post_code += f"void {name}_set_{var.name}(double x) {{\n"
post_code += f" set_{var.name}(x);\n"
post_code += "}\n"
global_code += '\n}\n'
code = global_code + code
post_code += "}\n\n" # extern c
header += "\n" + extra_header
funcs = ['f_fun', 'F_fun', 'err_fun', 'inv_err_fun', 'H_mod_fun', 'predict']
func_lists = {
'h': [kind for _, kind, _, _, _ in obs_eqs],
'H': [kind for _, kind, _, _, _ in obs_eqs],
'update': [kind for _, kind, _, _, _ in obs_eqs],
'He': [kind for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds],
'set': [var.name for var in global_vars] if global_vars is not None else [],
}
func_extra = [x[0] for x in extra_routines]
write_code(folder, name, code, header)
# For dynamic loading of specific functions
post_code += f"const EKF {name} = {{\n"
post_code += f" .name = \"{name}\",\n"
post_code += f" .kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs])} }},\n"
post_code += f" .feature_kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds])} }},\n"
for func in funcs:
post_code += f" .{func} = {name}_{func},\n"
for group, kinds in func_lists.items():
post_code += f" .{group}s = {{\n"
for kind in kinds:
str_kind = f"\"{kind}\"" if type(kind) == str else kind
post_code += f" {{ {str_kind}, {name}_{group}_{kind} }},\n"
post_code += " },\n"
post_code += " .extra_routines = {\n"
for f in func_extra:
post_code += f" {{ \"{f}\", {name}_{f} }},\n"
post_code += " },\n"
post_code += "};\n\n"
post_code += f"ekf_init({name});\n"
# merge code blocks
header += "}"
code = "\n".join([pre_code, code, open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read(), post_code])
# write to file
if not os.path.exists(folder):
os.mkdir(folder)
open(os.path.join(folder, f"{name}.h"), 'w').write(header) # header is used for ffi import
open(os.path.join(folder, f"{name}.cpp"), 'w').write(code)
class EKF_sym():
def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err, # pylint: disable=dangerous-default-value
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None, max_rewind_age=1.0, logger=logging):
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], quaternion_idxs=[], global_vars=None, max_rewind_age=1.0, logger=logging):
"""Generates process function and all observation functions for the kalman filter."""
self.msckf = N > 0
self.N = N
@ -181,7 +239,8 @@ class EKF_sym():
# tested for outlier rejection
self.maha_test_kinds = maha_test_kinds
self.global_vars = global_vars
# quaternions need normalization
self.quaternion_idxs = quaternion_idxs
# process noise
self.Q = Q
@ -193,25 +252,25 @@ class EKF_sym():
self.rewind_obscache = []
self.init_state(x_initial, P_initial, None)
ffi, lib = load_code(folder, name)
ffi, lib = load_code(folder, name, "kf")
kinds, self.feature_track_kinds = [], []
for func in dir(lib):
if func[:2] == 'h_':
kinds.append(int(func[2:]))
if func[:3] == 'He_':
self.feature_track_kinds.append(int(func[3:]))
if func[:len(name) + 3] == f'{name}_h_':
kinds.append(int(func[len(name) + 3:]))
if func[:len(name) + 4] == f'{name}_He_':
self.feature_track_kinds.append(int(func[len(name) + 4:]))
# wrap all the sympy functions
def wrap_1lists(name):
func = eval("lib.%s" % name, {"lib": lib}) # pylint: disable=eval-used
def wrap_1lists(func_name):
func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used
def ret(lst1, out):
func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double *", out.ctypes.data))
return ret
def wrap_2lists(name):
func = eval("lib.%s" % name, {"lib": lib}) # pylint: disable=eval-used
def wrap_2lists(func_name):
func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used
def ret(lst1, lst2, out):
func(ffi.cast("double *", lst1.ctypes.data),
@ -219,8 +278,8 @@ class EKF_sym():
ffi.cast("double *", out.ctypes.data))
return ret
def wrap_1list_1float(name):
func = eval("lib.%s" % name, {"lib": lib}) # pylint: disable=eval-used
def wrap_1list_1float(func_name):
func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used
def ret(lst1, fl, out):
func(ffi.cast("double *", lst1.ctypes.data),
@ -237,27 +296,28 @@ class EKF_sym():
self.hs, self.Hs, self.Hes = {}, {}, {}
for kind in kinds:
self.hs[kind] = wrap_2lists("h_%d" % kind)
self.Hs[kind] = wrap_2lists("H_%d" % kind)
self.hs[kind] = wrap_2lists(f"h_{kind}")
self.Hs[kind] = wrap_2lists(f"H_{kind}")
if self.msckf and kind in self.feature_track_kinds:
self.Hes[kind] = wrap_2lists("He_%d" % kind)
self.Hes[kind] = wrap_2lists(f"He_{kind}")
if self.global_vars is not None:
for var in self.global_vars:
fun_name = f"set_{var.name}"
setattr(self, fun_name, getattr(lib, fun_name))
self.set_globals = {}
if global_vars is not None:
for global_var in global_vars:
self.set_globals[global_var] = getattr(lib, f"{name}_set_{global_var}")
# wrap the C++ predict function
def _predict_blas(x, P, dt):
lib.predict(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", P.ctypes.data),
ffi.cast("double *", self.Q.ctypes.data),
ffi.cast("double", dt))
func = eval(f"lib.{name}_predict", {"lib": lib}) # pylint: disable=eval-used
func(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", P.ctypes.data),
ffi.cast("double *", self.Q.ctypes.data),
ffi.cast("double", dt))
return x, P
# wrap the C++ update function
def fun_wrapper(f, kind):
f = eval("lib.%s" % f, {"lib": lib}) # pylint: disable=eval-used
f = eval(f"lib.{name}_{f}", {"lib": lib}) # pylint: disable=eval-used
def _update_inner_blas(x, P, z, R, extra_args):
f(ffi.cast("double *", x.ctypes.data),
@ -333,6 +393,25 @@ class EKF_sym():
def covs(self):
return self.P
def set_filter_time(self, t):
self.filter_time = t
def get_filter_time(self):
return self.filter_time
def normalize_quaternions(self):
for idx in self.quaternion_idxs:
self.normalize_slice(idx, idx+4)
def normalize_slice(self, slice_start, slice_end_ex):
self.x[slice_start:slice_end_ex] /= np.linalg.norm(self.x[slice_start:slice_end_ex])
def get_augment_times(self):
return self.augment_times
def set_global(self, global_var, val):
self.set_globals[global_var](val)
def rewind(self, t):
# find where we are rewinding to
idx = bisect_right(self.rewind_t, t)
@ -376,6 +455,7 @@ class EKF_sym():
dt = t - self.filter_time
assert dt >= 0
self.x, self.P = self._predict(self.x, self.P, dt)
self.normalize_quaternions()
self.filter_time = t
def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # pylint: disable=dangerous-default-value
@ -401,11 +481,9 @@ class EKF_sym():
def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
"""The main kalman filter function
Predicts the state and then updates a batch of observations
dim_x: dimensionality of the state space
dim_z: dimensionality of the observation and depends on kind
n: number of observations
Args:
t (float): Time of observation
kind (int): Type of observation
@ -437,6 +515,7 @@ class EKF_sym():
extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
# update
self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
self.normalize_quaternions()
y.append(y_i)
xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
@ -570,7 +649,6 @@ class EKF_sym():
'''
Returns rts smoothed results of
kalman filter estimates
If the kalman state is augmented with
old states only the main state is smoothed
'''

190
rednose/helpers/ekf_sym_pyx.pyx 100644
View File

@ -0,0 +1,190 @@
# cython: language_level=3
# cython: profile=True
# distutils: language = c++
cimport cython
from libcpp.string cimport string
from libcpp.vector cimport vector
from libcpp cimport bool
cimport numpy as np
import numpy as np
cdef extern from "<optional>" namespace "std" nogil:
cdef cppclass optional[T]:
ctypedef T value_type
bool has_value()
T& value()
cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS":
cdef cppclass MapVectorXd "Eigen::Map<Eigen::VectorXd>":
MapVectorXd(double*, int)
cdef cppclass MapMatrixXdr "Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >":
MapMatrixXdr(double*, int, int)
cdef cppclass VectorXd "Eigen::VectorXd":
VectorXd()
double* data()
int rows()
cdef cppclass MatrixXdr "Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>":
MatrixXdr()
double* data()
int rows()
int cols()
ctypedef struct Estimate:
VectorXd xk1
VectorXd xk
MatrixXdr Pk1
MatrixXdr Pk
double t
int kind
vector[VectorXd] y
vector[VectorXd] z
vector[vector[double]] extra_args
cdef cppclass EKFSym:
EKFSym(string name, MapMatrixXdr Q, MapVectorXd x_initial, MapMatrixXdr P_initial, int dim_main,
int dim_main_err, int N, int dim_augment, int dim_augment_err, vector[int] maha_test_kinds,
vector[int] quaternion_idxs, vector[string] global_vars, double max_rewind_age)
void init_state(MapVectorXd state, MapMatrixXdr covs, double filter_time)
VectorXd state()
MatrixXdr covs()
void set_filter_time(double t)
double get_filter_time()
void set_global(string name, double val)
void reset_rewind()
void predict(double t)
optional[Estimate] predict_and_update_batch(double t, int kind, vector[MapVectorXd] z, vector[MapMatrixXdr] z,
vector[vector[double]] extra_args, bool augment)
# Functions like `numpy_to_matrix` are not possible, cython requires default
# constructor for return variable types which aren't available with Eigen::Map
@cython.wraparound(False)
@cython.boundscheck(False)
cdef np.ndarray[np.float64_t, ndim=2, mode="c"] matrix_to_numpy(MatrixXdr arr):
cdef double[:,:] mem_view = <double[:arr.rows(),:arr.cols()]>arr.data()
return np.copy(np.asarray(mem_view, dtype=np.double, order="C"))
@cython.wraparound(False)
@cython.boundscheck(False)
cdef np.ndarray[np.float64_t, ndim=1, mode="c"] vector_to_numpy(VectorXd arr):
cdef double[:] mem_view = <double[:arr.rows()]>arr.data()
return np.copy(np.asarray(mem_view, dtype=np.double, order="C"))
cdef class EKF_sym:
cdef EKFSym* ekf
def __cinit__(self, str gen_dir, str name, np.ndarray[np.float64_t, ndim=2] Q,
np.ndarray[np.float64_t, ndim=1] x_initial, np.ndarray[np.float64_t, ndim=2] P_initial, int dim_main,
int dim_main_err, int N=0, int dim_augment=0, int dim_augment_err=0, list maha_test_kinds=[],
list quaternion_idxs=[], list global_vars=[], double max_rewind_age=1.0, logger=None):
# TODO logger
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Q_b = np.ascontiguousarray(Q, dtype=np.double)
cdef np.ndarray[np.float64_t, ndim=1, mode='c'] x_initial_b = np.ascontiguousarray(x_initial, dtype=np.double)
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] P_initial_b = np.ascontiguousarray(P_initial, dtype=np.double)
self.ekf = new EKFSym(
name.encode('utf8'),
MapMatrixXdr(<double*> Q_b.data, Q.shape[0], Q.shape[1]),
MapVectorXd(<double*> x_initial_b.data, x_initial.shape[0]),
MapMatrixXdr(<double*> P_initial_b.data, P_initial.shape[0], P_initial.shape[1]),
dim_main,
dim_main_err,
N,
dim_augment,
dim_augment_err,
maha_test_kinds,
quaternion_idxs,
[x.encode('utf8') for x in global_vars],
max_rewind_age
)
def init_state(self, np.ndarray[np.float64_t, ndim=1] state, np.ndarray[np.float64_t, ndim=2] covs, filter_time):
cdef np.ndarray[np.float64_t, ndim=1, mode='c'] state_b = np.ascontiguousarray(state, dtype=np.double)
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] covs_b = np.ascontiguousarray(covs, dtype=np.double)
self.ekf.init_state(
MapVectorXd(<double*> state_b.data, state.shape[0]),
MapMatrixXdr(<double*> covs_b.data, covs.shape[0], covs.shape[1]),
np.nan if filter_time is None else filter_time
)
def state(self):
cdef np.ndarray res = vector_to_numpy(self.ekf.state())
return res
def covs(self):
return matrix_to_numpy(self.ekf.covs())
def set_filter_time(self, double t):
self.ekf.set_filter_time(t)
def get_filter_time(self):
return self.ekf.get_filter_time()
def set_global(self, str global_var, double val):
self.ekf.set_global(global_var.encode('utf8'), val)
def reset_rewind(self):
self.ekf.reset_rewind()
def predict(self, double t):
self.ekf.predict(t)
def predict_and_update_batch(self, double t, int kind, z, R, extra_args=[[]], bool augment=False):
cdef vector[MapVectorXd] z_map
cdef np.ndarray[np.float64_t, ndim=1, mode='c'] zi_b
for zi in z:
zi_b = np.ascontiguousarray(zi, dtype=np.double)
z_map.push_back(MapVectorXd(<double*> zi_b.data, zi.shape[0]))
cdef vector[MapMatrixXdr] R_map
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Ri_b
for Ri in R:
Ri_b = np.ascontiguousarray(Ri, dtype=np.double)
R_map.push_back(MapMatrixXdr(<double*> Ri_b.data, Ri.shape[0], Ri.shape[1]))
cdef vector[vector[double]] extra_args_map
cdef vector[double] args_map
for args in extra_args:
args_map.clear()
for a in args:
args_map.push_back(a)
extra_args_map.push_back(args_map)
cdef optional[Estimate] res = self.ekf.predict_and_update_batch(t, kind, z_map, R_map, extra_args_map, augment)
if not res.has_value():
return None
cdef VectorXd tmpvec
return (
vector_to_numpy(res.value().xk1),
vector_to_numpy(res.value().xk),
matrix_to_numpy(res.value().Pk1),
matrix_to_numpy(res.value().Pk),
res.value().t,
res.value().kind,
[vector_to_numpy(tmpvec) for tmpvec in res.value().y],
z, # TODO: take return values?
extra_args,
)
def augment(self):
raise NotImplementedError() # TODO
def get_augment_times(self):
raise NotImplementedError() # TODO
def rts_smooth(self, estimates, norm_quats=False):
raise NotImplementedError() # TODO
def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
raise NotImplementedError() # TODO
def __dealloc__(self):
del self.ekf

2
rednose/helpers/feature_handler.py
View File

@ -35,7 +35,9 @@ class FeatureHandler():
c_code = "#include <math.h>\n"
c_code += "#include <string.h>\n"
c_code += "#define K %d\n" % K
c_code += "extern \"C\" {\n"
c_code += "\n" + open(os.path.join(TEMPLATE_DIR, "feature_handler.c")).read()
c_code += "\n}\n"
filename = f"{FeatureHandler.name}_{K}"
write_code(generated_dir, filename, c_code, c_header)

11
rednose/__init__.py → rednose/helpers/kalmanfilter.py
View File

@ -2,8 +2,6 @@ from typing import Any, Dict
import numpy as np
from rednose.helpers.ekf_sym import EKF_sym
class KalmanFilter:
name = "<name>"
@ -12,12 +10,7 @@ class KalmanFilter:
Q = np.zeros((0, 0))
obs_noise: Dict[int, Any] = {}
def __init__(self, generated_dir):
dim_state = self.initial_x.shape[0]
dim_state_err = self.initial_P_diag.shape[0]
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
filter = None # Should be initialized when initializating a KalmanFilter implementation
@property
def x(self):
@ -25,7 +18,7 @@ class KalmanFilter:
@property
def t(self):
return self.filter.filter_time
return self.filter.get_filter_time()
@property
def P(self):

12
rednose/helpers/lst_sq_computer.py
View File

@ -48,14 +48,16 @@ class LstSqComputer():
@staticmethod
def generate_code(generated_dir, K=4):
sympy_functions = generate_residual(K)
header, code = sympy_into_c(sympy_functions)
header, sympy_code = sympy_into_c(sympy_functions)
code = "\n#include \"rednose/helpers/common_ekf.h\"\n"
code += "\n#define KDIM %d\n" % K
code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read()
code += "extern \"C\" {\n"
code += sympy_code
code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read() + "\n"
code += "}\n"
header += """
void compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);
"""
header += "\nvoid compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);\n"
filename = f"{LstSqComputer.name}_{K}"
write_code(generated_dir, filename, code, header)

1
rednose/helpers/sympy_helpers.py
View File

@ -151,6 +151,5 @@ def sympy_into_c(sympy_functions, global_vars=None):
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n"
return c_header, c_code

2
rednose/templates/compute_pos.c
View File

@ -7,7 +7,6 @@ typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
typedef Eigen::Matrix<double, 3, 1> O1M;
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> M3D;
extern "C" {
void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
double res[KDIM*2] = {0};
@ -51,4 +50,3 @@ void compute_pos(double *to_c, double *poses, double *img_positions, double *par
ecef_output = rot*ecef_output + ecef_offset;
memcpy(pos, ecef_output.data(), 3 * sizeof(double));
}
}

2
rednose/templates/feature_handler.c
View File

@ -1,4 +1,3 @@
extern "C"{
bool sane(double track [K + 1][5]) {
double diffs_x [K-1];
double diffs_y [K-1];
@ -55,4 +54,3 @@ void merge_features(double *tracks, double *features, long long *empty_idxs) {
}
memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double));
}
}

16
selfdrive/athena/athenad.py
View File

@ -17,7 +17,7 @@ from typing import Any
import requests
from jsonrpc import JSONRPCResponseManager, dispatcher
from websocket import ABNF, WebSocketTimeoutException, create_connection
from websocket import ABNF, WebSocketTimeoutException, WebSocketException, create_connection
import cereal.messaging as messaging
from cereal.services import service_list
@ -29,6 +29,8 @@ from selfdrive.hardware import HARDWARE, PC
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.swaglog import cloudlog, SWAGLOG_DIR
import selfdrive.crash as crash
from selfdrive.version import dirty, origin, branch, commit
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
@ -409,7 +411,12 @@ def backoff(retries):
def main():
params = Params()
dongle_id = params.get("DongleId").decode('utf-8')
dongle_id = params.get("DongleId", encoding='utf-8')
crash.init()
crash.bind_user(id=dongle_id)
crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit,
device=HARDWARE.get_device_type())
ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
api = Api(dongle_id)
@ -426,8 +433,13 @@ def main():
handle_long_poll(ws)
except (KeyboardInterrupt, SystemExit):
break
except (ConnectionError, TimeoutError, WebSocketException):
conn_retries += 1
params.delete("LastAthenaPingTime")
except Exception:
crash.capture_exception()
cloudlog.exception("athenad.main.exception")
conn_retries += 1
params.delete("LastAthenaPingTime")

4
selfdrive/athena/manage_athenad.py
View File

@ -3,7 +3,6 @@
import time
from multiprocessing import Process
import selfdrive.crash as crash
from common.params import Params
from selfdrive.manager.process import launcher
from selfdrive.swaglog import cloudlog
@ -16,9 +15,6 @@ def main():
params = Params()
dongle_id = params.get("DongleId").decode('utf-8')
cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty)
crash.bind_user(id=dongle_id)
crash.bind_extra(version=version, dirty=dirty)
crash.install()
try:
while 1:

50
selfdrive/registration.py → selfdrive/athena/registration.py
View File

@ -10,27 +10,22 @@ from common.params import Params
from common.spinner import Spinner
from common.file_helpers import mkdirs_exists_ok
from common.basedir import PERSIST
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.hardware import HARDWARE
from selfdrive.swaglog import cloudlog
from selfdrive.version import version, terms_version, training_version, get_git_commit, \
get_git_branch, get_git_remote
def register(show_spinner=False):
UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
def register(show_spinner=False) -> str:
params = Params()
params.put("Version", version)
params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
params.put("GitCommit", get_git_commit(default=""))
params.put("GitBranch", get_git_branch(default=""))
params.put("GitRemote", get_git_remote(default=""))
params.put("SubscriberInfo", HARDWARE.get_subscriber_info())
IMEI = params.get("IMEI", encoding='utf8')
HardwareSerial = params.get("HardwareSerial", encoding='utf8')
needs_registration = (None in [IMEI, HardwareSerial])
dongle_id = params.get("DongleId", encoding='utf8')
needs_registration = None in (IMEI, HardwareSerial, dongle_id)
# create a key for auth
# your private key is kept on your device persist partition and never sent to our servers
@ -44,22 +39,15 @@ def register(show_spinner=False):
os.rename(PERSIST+"/comma/id_rsa.tmp", PERSIST+"/comma/id_rsa")
os.rename(PERSIST+"/comma/id_rsa.tmp.pub", PERSIST+"/comma/id_rsa.pub")
# make key readable by app users (ai.comma.plus.offroad)
os.chmod(PERSIST+'/comma/', 0o755)
os.chmod(PERSIST+'/comma/id_rsa', 0o744)
dongle_id = params.get("DongleId", encoding='utf8')
needs_registration = needs_registration or dongle_id is None
if needs_registration:
if show_spinner:
spinner = Spinner()
spinner.update("registering device")
# Create registration token, in the future, this key will make JWTs directly
private_key = open(PERSIST+"/comma/id_rsa").read()
public_key = open(PERSIST+"/comma/id_rsa.pub").read()
register_token = jwt.encode({'register': True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
with open(PERSIST+"/comma/id_rsa.pub") as f1, open(PERSIST+"/comma/id_rsa") as f2:
public_key = f1.read()
private_key = f2.read()
# Block until we get the imei
imei1, imei2 = None, None
@ -74,22 +62,32 @@ def register(show_spinner=False):
params.put("IMEI", imei1)
params.put("HardwareSerial", serial)
backoff = 0
while True:
try:
register_token = jwt.encode({'register': True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/", method='POST', timeout=15,
imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token)
dongleauth = json.loads(resp.text)
dongle_id = dongleauth["dongle_id"]
params.put("DongleId", dongle_id)
if resp.status_code in (402, 403):
cloudlog.info(f"Unable to register device, got {resp.status_code}")
dongle_id = UNREGISTERED_DONGLE_ID
else:
dongleauth = json.loads(resp.text)
dongle_id = dongleauth["dongle_id"]
break
except Exception:
cloudlog.exception("failed to authenticate")
time.sleep(1)
backoff = min(backoff + 1, 15)
time.sleep(backoff)
if show_spinner:
spinner.close()
if dongle_id:
params.put("DongleId", dongle_id)
set_offroad_alert("Offroad_UnofficialHardware", dongle_id == UNREGISTERED_DONGLE_ID)
return dongle_id

132
selfdrive/boardd/boardd.cc
View File

@ -1,36 +1,35 @@
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <sched.h>
#include <errno.h>
#include <sched.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/resource.h>
#include <sys/types.h>
#include <unistd.h>
#include <ctime>
#include <cassert>
#include <iostream>
#include <algorithm>
#include <bitset>
#include <thread>
#include <atomic>
#include <bitset>
#include <cassert>
#include <ctime>
#include <iostream>
#include <thread>
#include <unordered_map>
#include <libusb-1.0/libusb.h>
#include "cereal/gen/cpp/car.capnp.h"
#include "cereal/messaging/messaging.h"
#include "selfdrive/common/params.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
#include "selfdrive/locationd/ublox_msg.h"
#include "common/util.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "messaging.hpp"
#include "locationd/ublox_msg.h"
#include "panda.h"
#include "pigeon.h"
#include "selfdrive/boardd/panda.h"
#include "selfdrive/boardd/pigeon.h"
#define MAX_IR_POWER 0.5f
#define MIN_IR_POWER 0.0f
@ -64,6 +63,8 @@ void safety_setter_thread() {
// diagnostic only is the default, needed for VIN query
panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
Params p = Params();
// switch to SILENT when CarVin param is read
while (true) {
if (do_exit || !panda->connected){
@ -71,12 +72,11 @@ void safety_setter_thread() {
return;
};
std::vector<char> value_vin = Params().read_db_bytes("CarVin");
std::string value_vin = p.get("CarVin");
if (value_vin.size() > 0) {
// sanity check VIN format
assert(value_vin.size() == 17);
std::string str_vin(value_vin.begin(), value_vin.end());
LOGW("got CarVin %s", str_vin.c_str());
LOGW("got CarVin %s", value_vin.c_str());
break;
}
util::sleep_for(100);
@ -85,7 +85,7 @@ void safety_setter_thread() {
// VIN query done, stop listening to OBDII
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
std::vector<char> params;
std::string params;
LOGW("waiting for params to set safety model");
while (true) {
if (do_exit || !panda->connected){
@ -93,8 +93,10 @@ void safety_setter_thread() {
return;
};
params = Params().read_db_bytes("CarParams");
if (params.size() > 0) break;
if (p.getBool("ControlsReady")) {
params = p.get("CarParams");
if (params.size() > 0) break;
}
util::sleep_for(100);
}
LOGW("got %d bytes CarParams", params.size());
@ -133,7 +135,7 @@ bool usb_connect() {
}
if (auto fw_sig = tmp_panda->get_firmware_version(); fw_sig) {
params.write_db_value("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size());
params.put("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size());
// Convert to hex for offroad
char fw_sig_hex_buf[16] = {0};
@ -143,13 +145,13 @@ bool usb_connect() {
fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF);
}
params.write_db_value("PandaFirmwareHex", fw_sig_hex_buf, 16);
params.put("PandaFirmwareHex", fw_sig_hex_buf, 16);
LOGW("fw signature: %.*s", 16, fw_sig_hex_buf);
} else { return false; }
// get panda serial
if (auto serial = tmp_panda->get_serial(); serial) {
params.write_db_value("PandaDongleId", serial->c_str(), serial->length());
params.put("PandaDongleId", serial->c_str(), serial->length());
LOGW("panda serial: %s", serial->c_str());
} else { return false; }
@ -161,13 +163,18 @@ bool usb_connect() {
#endif
if (tmp_panda->has_rtc){
setenv("TZ","UTC",1);
struct tm sys_time = get_time();
struct tm rtc_time = tmp_panda->get_rtc();
if (!time_valid(sys_time) && time_valid(rtc_time)) {
LOGE("System time wrong, setting from RTC");
LOGE("System time wrong, setting from RTC. "
"System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d",
sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday,
sys_time.tm_hour, sys_time.tm_min, sys_time.tm_sec,
rtc_time.tm_year + 1900, rtc_time.tm_mon + 1, rtc_time.tm_mday,
rtc_time.tm_hour, rtc_time.tm_min, rtc_time.tm_sec);
setenv("TZ","UTC",1);
const struct timeval tv = {mktime(&rtc_time), 0};
settimeofday(&tv, 0);
}
@ -313,10 +320,7 @@ void panda_state_thread(bool spoofing_started) {
// clear VIN, CarParams, and set new safety on car start
if (ignition && !ignition_last) {
int result = params.delete_db_value("CarVin");
assert((result == 0) || (result == ERR_NO_VALUE));
result = params.delete_db_value("CarParams");
assert((result == 0) || (result == ERR_NO_VALUE));
params.clearAll(CLEAR_ON_IGNITION);
if (!safety_setter_thread_running) {
safety_setter_thread_running = true;
@ -329,9 +333,23 @@ void panda_state_thread(bool spoofing_started) {
// Write to rtc once per minute when no ignition present
if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)){
// Write time to RTC if it looks reasonable
setenv("TZ","UTC",1);
struct tm sys_time = get_time();
if (time_valid(sys_time)){
panda->set_rtc(sys_time);
struct tm rtc_time = panda->get_rtc();
double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
if (std::abs(seconds) > 1.1) {
panda->set_rtc(sys_time);
LOGW("Updating panda RTC. dt = %.2f "
"System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d",
seconds,
sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday,
sys_time.tm_hour, sys_time.tm_min, sys_time.tm_sec,
rtc_time.tm_year + 1900, rtc_time.tm_mon + 1, rtc_time.tm_mday,
rtc_time.tm_hour, rtc_time.tm_min, rtc_time.tm_sec);
}
}
}
@ -343,13 +361,18 @@ void panda_state_thread(bool spoofing_started) {
auto ps = msg.initEvent().initPandaState();
ps.setUptime(pandaState.uptime);
#ifdef QCOM2
ps.setVoltage(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input")));
ps.setCurrent(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input")));
#else
ps.setVoltage(pandaState.voltage);
ps.setCurrent(pandaState.current);
#endif
if (Hardware::TICI()) {
double read_time = millis_since_boot();
ps.setVoltage(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input")));
ps.setCurrent(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input")));
read_time = millis_since_boot() - read_time;
if (read_time > 50) {
LOGW("reading hwmon took %lfms", read_time);
}
} else {
ps.setVoltage(pandaState.voltage);
ps.setCurrent(pandaState.current);
}
ps.setIgnitionLine(pandaState.ignition_line);
ps.setIgnitionCan(pandaState.ignition_can);
@ -394,17 +417,14 @@ void hardware_control_thread() {
uint16_t prev_fan_speed = 999;
uint16_t ir_pwr = 0;
uint16_t prev_ir_pwr = 999;
#if defined(QCOM) || defined(QCOM2)
bool prev_charging_disabled = false;
#endif
unsigned int cnt = 0;
while (!do_exit && panda->connected) {
cnt++;
sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update?
#if defined(QCOM) || defined(QCOM2)
if (sm.updated("deviceState")){
if (!Hardware::PC() && sm.updated("deviceState")){
// Charging mode
bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled();
if (charging_disabled != prev_charging_disabled){
@ -418,7 +438,6 @@ void hardware_control_thread() {
prev_charging_disabled = charging_disabled;
}
}
#endif
// Other pandas don't have fan/IR to control
if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue;
@ -468,16 +487,12 @@ void pigeon_thread() {
PubMaster pm({"ubloxRaw"});
bool ignition_last = false;
#ifdef QCOM2
Pigeon *pigeon = Pigeon::connect("/dev/ttyHS0");
#else
Pigeon *pigeon = Pigeon::connect(panda);
#endif
Pigeon *pigeon = Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda);
std::unordered_map<char, uint64_t> last_recv_time;
std::unordered_map<char, int64_t> cls_max_dt = {
{(char)ublox::CLASS_NAV, int64_t(250000000ULL)}, // 0.25s
{(char)ublox::CLASS_RXM, int64_t(250000000ULL)}, // 0.25s
{(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s
{(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s
};
while (!do_exit && panda->connected) {
@ -548,10 +563,9 @@ int main() {
// set process priority and affinity
err = set_realtime_priority(54);
LOG("set priority returns %d", err);
err = set_core_affinity(3);
LOG("set affinity returns %d", err);
panda_set_power(true);
err = set_core_affinity(Hardware::TICI() ? 4 : 3);
LOG("set affinity returns %d", err);
while (!do_exit){
std::vector<std::thread> threads;

2
selfdrive/boardd/boardd_api_impl.pyx
View File

@ -14,6 +14,8 @@ cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, st
def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
cdef vector[can_frame] can_list
can_list.reserve(len(can_msgs))
cdef can_frame f
for can_msg in can_msgs:
f.address = can_msg[0]

2
selfdrive/boardd/can_list_to_can_capnp.cc
View File

@ -1,4 +1,4 @@
#include "messaging.hpp"
#include "cereal/messaging/messaging.h"
typedef struct {
long address;

35
selfdrive/boardd/panda.cc
View File

@ -1,31 +1,16 @@
#include <stdexcept>
#include <cassert>
#include <iostream>
#include <vector>
#include "selfdrive/boardd/panda.h"
#include <unistd.h>
#include "common/swaglog.h"
#include "common/gpio.h"
#include "common/util.h"
#include "messaging.hpp"
#include "panda.h"
#include <cassert>
#include <iostream>
#include <stdexcept>
#include <vector>
void panda_set_power(bool power){
#ifdef QCOM2
int err = 0;
err += gpio_init(GPIO_STM_RST_N, true);
err += gpio_init(GPIO_STM_BOOT0, true);
err += gpio_set(GPIO_STM_RST_N, true);
err += gpio_set(GPIO_STM_BOOT0, false);
util::sleep_for(100); // 100 ms
err += gpio_set(GPIO_STM_RST_N, !power);
assert(err == 0);
#endif
}
#include "cereal/messaging/messaging.h"
#include "selfdrive/common/gpio.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
Panda::Panda(){
// init libusb

10
selfdrive/boardd/panda.h
View File

@ -1,11 +1,13 @@
#pragma once
#include <ctime>
#include <cstdint>
#include <pthread.h>
#include <atomic>
#include <cstdint>
#include <ctime>
#include <mutex>
#include <vector>
#include <optional>
#include <vector>
#include <libusb-1.0/libusb.h>
@ -39,8 +41,6 @@ struct __attribute__((packed)) health_t {
};
void panda_set_power(bool power);
class Panda {
private:
libusb_context *ctx = NULL;

128
selfdrive/boardd/pigeon.cc
View File

@ -1,14 +1,15 @@
#include <cassert>
#include <unistd.h>
#include <fcntl.h>
#include "selfdrive/boardd/pigeon.h"
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include "common/swaglog.h"
#include "common/gpio.h"
#include "common/util.h"
#include <cassert>
#include "pigeon.h"
#include "selfdrive/common/gpio.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
// Termios on macos doesn't define all baud rate constants
#ifndef B460800
@ -17,6 +18,11 @@
using namespace std::string_literals;
extern ExitHandler do_exit;
const std::string ack = "\xb5\x62\x05\x01\x02\x00";
const std::string nack = "\xb5\x62\x05\x00\x02\x00";
Pigeon * Pigeon::connect(Panda * p){
PandaPigeon * pigeon = new PandaPigeon();
@ -32,53 +38,83 @@ Pigeon * Pigeon::connect(const char * tty){
return pigeon;
}
bool Pigeon::wait_for_ack(){
std::string s;
while (!do_exit){
s += receive();
if (s.find(ack) != std::string::npos){
LOGD("Received ACK from ublox");
return true;
} else if (s.find(nack) != std::string::npos) {
LOGE("Received NACK from ublox");
return false;
} else if (s.size() > 0x1000) {
LOGE("No response from ublox");
return false;
}
util::sleep_for(1); // Allow other threads to be scheduled
}
return false;
}
bool Pigeon::send_with_ack(std::string cmd){
send(cmd);
return wait_for_ack();
}
void Pigeon::init() {
util::sleep_for(1000);
LOGW("panda GPS start");
for (int i = 0; i < 10; i++){
if (do_exit) return;
LOGW("panda GPS start");
// power off pigeon
set_power(false);
util::sleep_for(100);
// power off pigeon
set_power(false);
util::sleep_for(100);
// 9600 baud at init
set_baud(9600);
// 9600 baud at init
set_baud(9600);
// power on pigeon
set_power(true);
util::sleep_for(500);
// power on pigeon
set_power(true);
util::sleep_for(500);
// baud rate upping
send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s);
util::sleep_for(100);
// baud rate upping
send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s);
util::sleep_for(100);
// set baud rate to 460800
set_baud(460800);
util::sleep_for(100);
// set baud rate to 460800
set_baud(460800);
// init from ubloxd
// To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py
send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s);
send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"s);
send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s);
send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s);
send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s);
send("\xB5\x62\x06\x00\x00\x00\x06\x18"s);
send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s);
send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23"s);
send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s);
send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s);
send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s);
send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s);
send("\xB5\x62\x06\x24\x00\x00\x2A\x84"s);
send("\xB5\x62\x06\x23\x00\x00\x29\x81"s);
send("\xB5\x62\x06\x1E\x00\x00\x24\x72"s);
send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s);
send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s);
send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s);
send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s);
send("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s);
// init from ubloxd
// To generate this data, run selfdrive/locationd/test/ubloxd.py
if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x00\x00\x06\x18"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x24\x00\x00\x2A\x84"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x23\x00\x00\x29\x81"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x1E\x00\x00\x24\x72"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x39\x00\x00\x3F\xC3"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue;
LOGW("panda GPS on");
LOGW("panda GPS on");
return;
}
LOGE("failed to initialize panda GPS");
}
void PandaPigeon::connect(Panda * p) {

8
selfdrive/boardd/pigeon.h
View File

@ -1,9 +1,11 @@
#pragma once
#include <string>
#include <termios.h>
#include <atomic>
#include <string>
#include "panda.h"
#include "selfdrive/boardd/panda.h"
class Pigeon {
public:
@ -12,6 +14,8 @@ class Pigeon {
virtual ~Pigeon(){};
void init();
bool wait_for_ack();
bool send_with_ack(std::string cmd);
virtual void set_baud(int baud) = 0;
virtual void send(const std::string &s) = 0;
virtual std::string receive() = 0;

37
selfdrive/boardd/set_time.py 100755
View File

@ -0,0 +1,37 @@
#!/usr/bin/env python3
import datetime
import os
import struct
import usb1
REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE
MIN_DATE = datetime.datetime(year=2021, month=4, day=1)
def set_time(logger):
sys_time = datetime.datetime.today()
if sys_time > MIN_DATE:
logger.info("System time valid")
return
try:
ctx = usb1.USBContext()
dev = ctx.openByVendorIDAndProductID(0xbbaa, 0xddcc)
if dev is None:
logger.info("No panda found")
return
# Set system time from panda RTC time
dat = dev.controlRead(REQUEST_IN, 0xa0, 0, 0, 8)
a = struct.unpack("HBBBBBB", dat)
panda_time = datetime.datetime(a[0], a[1], a[2], a[4], a[5], a[6])
if panda_time > MIN_DATE:
logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'")
os.system(f"TZ=UTC date -s '{panda_time}'")
except Exception:
logger.warn("Failed to fetch time from panda")
if __name__ == "__main__":
import logging
logging.basicConfig(level=logging.INFO)
set_time(logging)

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

@ -1,29 +1,31 @@
#include <thread>
#include <chrono>
#include <stdio.h>
#include "selfdrive/camerad/cameras/camera_common.h"
#include <assert.h>
#include <stdio.h>
#include <unistd.h>
#include <chrono>
#include <thread>
#if defined(QCOM) && !defined(QCOM_REPLAY)
#include "cameras/camera_qcom.h"
#elif QCOM2
#include "cameras/camera_qcom2.h"
#elif WEBCAM
#include "cameras/camera_webcam.h"
#else
#include "cameras/camera_frame_stream.h"
#endif
#include "camera_common.h"
#include <libyuv.h>
#include "libyuv.h"
#include <jpeglib.h>
#include "clutil.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "modeldata.h"
#include "imgproc/utils.h"
#include "selfdrive/camerad/imgproc/utils.h"
#include "selfdrive/common/clutil.h"
#include "selfdrive/common/modeldata.h"
#include "selfdrive/common/params.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
#if defined(QCOM) && !defined(QCOM_REPLAY)
#include "selfdrive/camerad/cameras/camera_qcom.h"
#elif QCOM2
#include "selfdrive/camerad/cameras/camera_qcom2.h"
#elif WEBCAM
#include "selfdrive/camerad/cameras/camera_webcam.h"
#else
#include "selfdrive/camerad/cameras/camera_frame_stream.h"
#endif
static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b, const CameraState *s) {
char args[4096];
@ -35,11 +37,8 @@ static cl_program build_debayer_program(cl_device_id device_id, cl_context conte
ci->frame_width, ci->frame_height, ci->frame_stride,
b->rgb_width, b->rgb_height, b->rgb_stride,
ci->bayer_flip, ci->hdr, s->camera_num);
#ifdef QCOM2
return cl_program_from_file(context, device_id, "cameras/real_debayer.cl", args);
#else
return cl_program_from_file(context, device_id, "cameras/debayer.cl", args);
#endif
const char *cl_file = Hardware::TICI() ? "cameras/real_debayer.cl" : "cameras/debayer.cl";
return cl_program_from_file(context, device_id, cl_file, args);
}
void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType rgb_type, VisionStreamType yuv_type, release_cb release_callback) {
@ -64,13 +63,13 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
rgb_width = ci->frame_width;
rgb_height = ci->frame_height;
#ifndef QCOM2
// debayering does a 2x downscale
if (ci->bayer) {
if (!Hardware::TICI() && ci->bayer) {
// debayering does a 2x downscale
rgb_width = ci->frame_width / 2;
rgb_height = ci->frame_height / 2;
}
#endif
yuv_transform = get_model_yuv_transform(ci->bayer);
vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height);
@ -84,7 +83,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
CL_CHECK(clReleaseProgram(prg_debayer));
}
rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, rgb_width, rgb_height, rgb_stride);
rgb2yuv = std::make_unique<Rgb2Yuv>(context, device_id, rgb_width, rgb_height, rgb_stride);
#ifdef __APPLE__
q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
@ -99,7 +98,6 @@ CameraBuf::~CameraBuf() {
camera_bufs[i].free();
}
if (rgb_to_yuv_state.rgb_to_yuv_krnl) rgb_to_yuv_destroy(&rgb_to_yuv_state);
if (krnl_debayer) CL_CHECK(clReleaseKernel(krnl_debayer));
if (q) CL_CHECK(clReleaseCommandQueue(q));
}
@ -114,7 +112,6 @@ bool CameraBuf::acquire() {
}
cur_frame_data = camera_bufs_metadata[cur_buf_idx];
cur_rgb_buf = vipc_server->get_buffer(rgb_type);
cl_event debayer_event;
@ -151,7 +148,7 @@ bool CameraBuf::acquire() {
CL_CHECK(clReleaseEvent(debayer_event));
cur_yuv_buf = vipc_server->get_buffer(yuv_type);
rgb_to_yuv_queue(&rgb_to_yuv_state, q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
rgb2yuv->queue(q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
VisionIpcBufExtra extra = {
cur_frame_data.frame_id,
@ -348,73 +345,60 @@ std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, pro
return std::thread(processing_thread, cameras, cs, callback);
}
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) {
static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
static const bool is_rhd = Params().getBool("IsRHD");
struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;};
const CameraBuf *b = &c->buf;
static int x_min = 0, x_max = 0, y_min = 0, y_max = 0;
static const bool is_rhd = Params().read_db_bool("IsRHD");
// auto exposure
if (cnt % 3 == 0) {
if (sm->update(0) > 0 && sm->updated("driverState")) {
auto state = (*sm)["driverState"].getDriverState();
// set driver camera metering target
if (state.getFaceProb() > 0.4) {
auto face_position = state.getFacePosition();
bool hist_ceil = false, hl_weighted = false;
int x_offset = 0, y_offset = 0;
int frame_width = b->rgb_width, frame_height = b->rgb_height;
#ifndef QCOM2
int frame_width = b->rgb_width;
int frame_height = b->rgb_height;
int analog_gain = -1;
#else
int frame_width = 668;
int frame_height = frame_width / 1.33;
int analog_gain = c->analog_gain;
#endif
int x_offset = is_rhd ? 0 : frame_width - (0.5 * frame_height);
x_offset += (face_position[0] * (is_rhd ? -1.0 : 1.0) + 0.5) * (0.5 * frame_height);
int y_offset = (face_position[1] + 0.5) * frame_height;
#ifdef QCOM2
x_offset += 630;
y_offset += 156;
#endif
x_min = std::max(0, x_offset - 72);
x_max = std::min(b->rgb_width - 1, x_offset + 72);
y_min = std::max(0, y_offset - 72);
y_max = std::min(b->rgb_height - 1, y_offset + 72);
} else { // use default setting if no face
x_min = x_max = y_min = y_max = 0;
}
}
int skip = 1;
// use driver face crop for AE
if (x_max == 0) {
// default setting
#ifndef QCOM2
x_min = is_rhd ? 0 : b->rgb_width * 3 / 5;
x_max = is_rhd ? b->rgb_width * 2 / 5 : b->rgb_width;
y_min = b->rgb_height / 3;
y_max = b->rgb_height;
#else
x_min = 96;
x_max = 1832;
y_min = 242;
y_max = 1148;
skip = 4;
#endif
}
#ifdef QCOM2
camera_autoexposure(c, set_exposure_target(b, x_min, x_max, 2, y_min, y_max, skip, (int)c->analog_gain, true, true));
#else
camera_autoexposure(c, set_exposure_target(b, x_min, x_max, 2, y_min, y_max, skip, -1, false, false));
#endif
ExpRect def_rect;
if (Hardware::TICI()) {
hist_ceil = hl_weighted = true;
x_offset = 630, y_offset = 156;
frame_width = 668, frame_height = frame_width / 1.33;
def_rect = {96, 1832, 2, 242, 1148, 4};
} else {
def_rect = {is_rhd ? 0 : b->rgb_width * 3 / 5, is_rhd ? b->rgb_width * 2 / 5 : b->rgb_width, 2,
b->rgb_height / 3, b->rgb_height, 1};
}
static ExpRect rect = def_rect;
// use driver face crop for AE
if (sm.updated("driverState")) {
if (auto state = sm["driverState"].getDriverState(); state.getFaceProb() > 0.4) {
auto face_position = state.getFacePosition();
int x = is_rhd ? 0 : frame_width - (0.5 * frame_height);
x += (face_position[0] * (is_rhd ? -1.0 : 1.0) + 0.5) * (0.5 * frame_height) + x_offset;
int y = (face_position[1] + 0.5) * frame_height + y_offset;
rect = {std::max(0, x - 72), std::min(b->rgb_width - 1, x + 72), 2,
std::max(0, y - 72), std::min(b->rgb_height - 1, y + 72), 1};
} else {
rect = def_rect;
}
}
camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip, analog_gain, hist_ceil, hl_weighted));
}
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) {
if (cnt % 3 == 0) {
sm->update(0);
driver_cam_auto_exposure(c, *sm);
}
MessageBuilder msg;
auto framed = msg.initEvent().initDriverCameraState();
framed.setFrameType(cereal::FrameData::FrameType::FRONT);
fill_frame_data(framed, b->cur_frame_data);
fill_frame_data(framed, c->buf.cur_frame_data);
if (env_send_driver) {
framed.setImage(get_frame_image(b));
framed.setImage(get_frame_image(&c->buf));
}
pm->send("driverCameraState", msg);
}

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

@ -1,20 +1,21 @@
#pragma once
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <memory>
#include <thread>
#include "common/mat.h"
#include "common/swaglog.h"
#include "common/queue.h"
#include "visionbuf.h"
#include "common/visionimg.h"
#include "messaging.hpp"
#include "transforms/rgb_to_yuv.h"
#include "visionipc.h"
#include "visionipc_server.h"
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionbuf.h"
#include "cereal/visionipc/visionipc.h"
#include "cereal/visionipc/visionipc_server.h"
#include "selfdrive/camerad/transforms/rgb_to_yuv.h"
#include "selfdrive/common/mat.h"
#include "selfdrive/common/queue.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/visionimg.h"
#define CAMERA_ID_IMX298 0
#define CAMERA_ID_IMX179 1
@ -94,7 +95,7 @@ private:
CameraState *camera_state;
cl_kernel krnl_debayer;
RGBToYUVState rgb_to_yuv_state;
std::unique_ptr<Rgb2Yuv> rgb2yuv;
VisionStreamType rgb_type, yuv_type;

43
selfdrive/camerad/cameras/camera_frame_stream.cc
View File

@ -1,12 +1,12 @@
#include "camera_frame_stream.h"
#include "selfdrive/camerad/cameras/camera_frame_stream.h"
#include <unistd.h>
#include <cassert>
#include <capnp/dynamic.h>
#include "messaging.hpp"
#include "common/util.h"
#include "cereal/messaging/messaging.h"
#include "selfdrive/common/util.h"
#define FRAME_WIDTH 1164
#define FRAME_HEIGHT 874
@ -35,7 +35,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
};
void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
assert(camera_id < ARRAYSIZE(cameras_supported));
assert(camera_id < std::size(cameras_supported));
s->ci = cameras_supported[camera_id];
assert(s->ci.frame_width != 0);
@ -49,25 +49,26 @@ void run_frame_stream(CameraState &camera, const char* frame_pkt) {
size_t buf_idx = 0;
while (!do_exit) {
if (sm.update(1000) == 0) continue;
sm.update(1000);
if(sm.updated(frame_pkt)){
auto msg = static_cast<capnp::DynamicStruct::Reader>(sm[frame_pkt]);
auto frame = msg.get(frame_pkt).as<capnp::DynamicStruct>();
camera.buf.camera_bufs_metadata[buf_idx] = {
.frame_id = frame.get("frameId").as<uint32_t>(),
.timestamp_eof = frame.get("timestampEof").as<uint64_t>(),
.frame_length = frame.get("frameLength").as<unsigned>(),
.integ_lines = frame.get("integLines").as<unsigned>(),
.global_gain = frame.get("globalGain").as<unsigned>(),
};
auto msg = static_cast<capnp::DynamicStruct::Reader>(sm[frame_pkt]);
auto frame = msg.get(frame_pkt).as<capnp::DynamicStruct>();
camera.buf.camera_bufs_metadata[buf_idx] = {
.frame_id = frame.get("frameId").as<uint32_t>(),
.timestamp_eof = frame.get("timestampEof").as<uint64_t>(),
.frame_length = frame.get("frameLength").as<unsigned>(),
.integ_lines = frame.get("integLines").as<unsigned>(),
.global_gain = frame.get("globalGain").as<unsigned>(),
};
cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q;
cl_mem yuv_cl = camera.buf.camera_bufs[buf_idx].buf_cl;
cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q;
cl_mem yuv_cl = camera.buf.camera_bufs[buf_idx].buf_cl;
auto image = frame.get("image").as<capnp::Data>();
clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, image.size(), image.begin(), 0, NULL, NULL);
camera.buf.queue(buf_idx);
buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
auto image = frame.get("image").as<capnp::Data>();
clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, image.size(), image.begin(), 0, NULL, NULL);
camera.buf.queue(buf_idx);
buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
}
}
}

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

@ -1,32 +1,31 @@
#include <stdio.h>
#include <stdbool.h>
#include "selfdrive/camerad/cameras/camera_qcom.h"
#include <assert.h>
#include <unistd.h>
#include <fcntl.h>
#include <math.h>
#include <poll.h>
#include <pthread.h>
#include <stdbool.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <atomic>
#include <algorithm>
#include <unistd.h>
#include <linux/media.h>
#include <algorithm>
#include <atomic>
#include <cutils/properties.h>
#include <linux/media.h>
#include <pthread.h>
#include "msmb_isp.h"
#include "msmb_ispif.h"
#include "msmb_camera.h"
#include "msm_cam_sensor.h"
#include "common/util.h"
#include "common/timing.h"
#include "common/swaglog.h"
#include "common/params.h"
#include "clutil.h"
#include "sensor_i2c.h"
#include "camera_qcom.h"
#include "selfdrive/camerad/cameras/sensor_i2c.h"
#include "selfdrive/camerad/include/msm_cam_sensor.h"
#include "selfdrive/camerad/include/msmb_camera.h"
#include "selfdrive/camerad/include/msmb_isp.h"
#include "selfdrive/camerad/include/msmb_ispif.h"
#include "selfdrive/common/clutil.h"
#include "selfdrive/common/params.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
// leeco actuator (DW9800W H-Bridge Driver IC)
// from sniff
@ -34,6 +33,13 @@ const uint16_t INFINITY_DAC = 364;
extern ExitHandler do_exit;
static int cam_ioctl(int fd, unsigned long int request, void *arg, const char *log_msg = nullptr) {
int err = ioctl(fd, request, arg);
if (err != 0 && log_msg) {
LOG(util::string_format("%s: %d", log_msg, err).c_str());
}
return err;
}
// global var for AE/AF ops
std::atomic<CameraExpInfo> road_cam_exp{{0}};
std::atomic<CameraExpInfo> driver_cam_exp{{0}};
@ -93,10 +99,10 @@ int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size
return ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &cfg_data);
}
static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) {
static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, uint32_t frame_length) {
int analog_gain = std::min(gain, 448);
s->digital_gain = gain > 448 ? (512.0/(512-(gain))) / 8.0 : 1.0;
//printf("%5d/%5d %5d %f\n", s->cur_integ_lines, s->cur_frame_length, analog_gain, s->digital_gain);
//printf("%5d/%5d %5d %f\n", s->cur_integ_lines, s->frame_length, analog_gain, s->digital_gain);
struct msm_camera_i2c_reg_array reg_array[] = {
// REG_HOLD
@ -120,17 +126,17 @@ static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int
// REG_HOLD
{0x104,0x0,0},
};
return sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
return sensor_write_regs(s, reg_array, std::size(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
}
static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) {
static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, uint32_t frame_length) {
//printf("driver camera: %d %d %d\n", gain, integ_lines, frame_length);
int coarse_gain_bitmap, fine_gain_bitmap;
// get bitmaps from iso
static const int gains[] = {0, 100, 200, 400, 800};
int i;
for (i = 1; i < ARRAYSIZE(gains); i++) {
for (i = 1; i < std::size(gains); i++) {
if (gain >= gains[i - 1] && gain < gains[i])
break;
}
@ -155,7 +161,7 @@ static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int
//{0x104,0x0,0},
};
return sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
return sensor_write_regs(s, reg_array, std::size(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
}
static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int camera_num,
@ -165,14 +171,14 @@ static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int c
s->camera_num = camera_num;
s->camera_id = camera_id;
assert(camera_id < ARRAYSIZE(cameras_supported));
assert(camera_id < std::size(cameras_supported));
s->ci = cameras_supported[camera_id];
assert(s->ci.frame_width != 0);
s->pixel_clock = pixel_clock;
s->line_length_pclk = line_length_pclk;
s->max_gain = max_gain;
s->fps = fps;
s->frame_length = s->pixel_clock / line_length_pclk / s->fps;
s->self_recover = 0;
s->apply_exposure = (camera_id == CAMERA_ID_IMX298) ? imx298_apply_exposure : ov8865_apply_exposure;
@ -228,18 +234,15 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
int err = 0;
uint32_t frame_length = s->pixel_clock / s->line_length_pclk / s->fps;
uint32_t gain = s->cur_gain;
uint32_t integ_lines = s->cur_integ_lines;
if (exposure_frac >= 0) {
exposure_frac = std::clamp(exposure_frac, 2.0f / frame_length, 1.0f);
integ_lines = frame_length * exposure_frac;
exposure_frac = std::clamp(exposure_frac, 2.0f / s->frame_length, 1.0f);
integ_lines = s->frame_length * exposure_frac;
// See page 79 of the datasheet, this is the max allowed (-1 for phase adjust)
integ_lines = std::min(integ_lines, frame_length-11);
integ_lines = std::min(integ_lines, s->frame_length - 11);
}
if (gain_frac >= 0) {
@ -256,19 +259,15 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
gain = (s->max_gain/510) * (512 - 512/(256*gain_frac));
}
if (gain != s->cur_gain
|| integ_lines != s->cur_integ_lines
|| frame_length != s->cur_frame_length) {
if (gain != s->cur_gain || integ_lines != s->cur_integ_lines) {
if (s->apply_exposure == ov8865_apply_exposure) {
gain = 800 * gain_frac; // ISO
}
err = s->apply_exposure(s, gain, integ_lines, frame_length);
err = s->apply_exposure(s, gain, integ_lines, s->frame_length);
if (err == 0) {
std::lock_guard lk(s->frame_info_lock);
s->cur_gain = gain;
s->cur_integ_lines = integ_lines;
s->cur_frame_length = frame_length;
} else {
LOGE("camera %d apply_exposure err: %d", s->camera_num, err);
}
@ -290,9 +289,8 @@ static void do_autoexposure(CameraState *s, float grey_frac) {
const float gain_frac_min = 0.015625;
const float gain_frac_max = 1.0;
// exposure time limits
uint32_t frame_length = s->pixel_clock / s->line_length_pclk / s->fps;
const uint32_t exposure_time_min = 16;
const uint32_t exposure_time_max = frame_length - 11; // copied from set_exposure()
const uint32_t exposure_time_max = s->frame_length - 11; // copied from set_exposure()
float cur_gain_frac = s->cur_gain_frac;
float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.05);
@ -325,14 +323,8 @@ static void do_autoexposure(CameraState *s, float grey_frac) {
}
static void sensors_init(MultiCameraState *s) {
unique_fd sensorinit_fd;
sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK);
assert(sensorinit_fd >= 0);
// init road camera sensor
struct msm_camera_sensor_slave_info slave_info = {0};
slave_info = (struct msm_camera_sensor_slave_info){
msm_camera_sensor_slave_info slave_infos[2] = {
(msm_camera_sensor_slave_info){ // road camera
.sensor_name = "imx298",
.eeprom_name = "sony_imx298",
.actuator_name = "dw9800w",
@ -367,16 +359,8 @@ static void sensors_init(MultiCameraState *s) {
.is_init_params_valid = 0,
.sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 90},
.output_format = MSM_SENSOR_BAYER,
};
slave_info.power_setting_array.power_setting = &slave_info.power_setting_array.power_setting_a[0];
slave_info.power_setting_array.power_down_setting = &slave_info.power_setting_array.power_down_setting_a[0];
sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &slave_info};
int err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg);
LOG("sensor init cfg (road camera): %d", err);
assert(err >= 0);
struct msm_camera_sensor_slave_info slave_info2 = {0};
slave_info2 = (struct msm_camera_sensor_slave_info){
},
(msm_camera_sensor_slave_info){ // driver camera
.sensor_name = "ov8865_sunny",
.eeprom_name = "ov8865_plus",
.actuator_name = "",
@ -409,14 +393,17 @@ static void sensors_init(MultiCameraState *s) {
.is_init_params_valid = 0,
.sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270},
.output_format = MSM_SENSOR_BAYER,
};
slave_info2.power_setting_array.power_setting = &slave_info2.power_setting_array.power_setting_a[0];
slave_info2.power_setting_array.power_down_setting = &slave_info2.power_setting_array.power_down_setting_a[0];
sensor_init_cfg.cfgtype = CFG_SINIT_PROBE;
sensor_init_cfg.cfg.setting = &slave_info2;
err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg);
LOG("sensor init cfg (driver camera): %d", err);
assert(err >= 0);
}};
unique_fd sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK);
assert(sensorinit_fd >= 0);
for (auto &info : slave_infos) {
info.power_setting_array.power_setting = &info.power_setting_array.power_setting_a[0];
info.power_setting_array.power_down_setting = &info.power_setting_array.power_down_setting_a[0];
sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &info};
int err = cam_ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg, "sensor init cfg");
assert(err >= 0);
}
}
static void camera_open(CameraState *s, bool is_road_cam) {
@ -463,23 +450,19 @@ static void camera_open(CameraState *s, bool is_road_cam) {
struct msm_camera_csi_lane_params csi_lane_params = {0};
csi_lane_params.csi_lane_mask = 0x1f;
csiphy_cfg_data csiphy_cfg_data = { .cfg.csi_lane_params = &csi_lane_params, .cfgtype = CSIPHY_RELEASE};
int err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data);
LOG("release csiphy: %d", err);
int err = cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "release csiphy");
// CSID: release csid
csid_cfg_data.cfgtype = CSID_RELEASE;
err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data);
LOG("release csid: %d", err);
cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "release csid");
// SENSOR: send power down
struct sensorb_cfg_data sensorb_cfg_data = {.cfgtype = CFG_POWER_DOWN};
err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data);
LOG("sensor power down: %d", err);
cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power down");
// actuator powerdown
actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERDOWN;
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
LOG("actuator powerdown: %d", err);
cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerdown");
// reset isp
// struct msm_vfe_axi_halt_cmd halt_cmd = {
@ -508,39 +491,35 @@ static void camera_open(CameraState *s, bool is_road_cam) {
// CSID: init csid
csid_cfg_data.cfgtype = CSID_INIT;
err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data);
LOG("init csid: %d", err);
cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "init csid");
// CSIPHY: init csiphy
csiphy_cfg_data = {.cfgtype = CSIPHY_INIT};
err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data);
LOG("init csiphy: %d", err);
cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "init csiphy");
// SENSOR: stop stream
struct msm_camera_i2c_reg_setting stop_settings = {
.reg_setting = stop_reg_array,
.size = ARRAYSIZE(stop_reg_array),
.size = std::size(stop_reg_array),
.addr_type = MSM_CAMERA_I2C_WORD_ADDR,
.data_type = MSM_CAMERA_I2C_BYTE_DATA,
.delay = 0
};
sensorb_cfg_data.cfgtype = CFG_SET_STOP_STREAM_SETTING;
sensorb_cfg_data.cfg.setting = &stop_settings;
err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data);
LOG("stop stream: %d", err);
cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "stop stream");
// SENSOR: send power up
sensorb_cfg_data = {.cfgtype = CFG_POWER_UP};
err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data);
LOG("sensor power up: %d", err);
cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power up");
// **** configure the sensor ****
// SENSOR: send i2c configuration
if (s->camera_id == CAMERA_ID_IMX298) {
err = sensor_write_regs(s, init_array_imx298, ARRAYSIZE(init_array_imx298), MSM_CAMERA_I2C_BYTE_DATA);
err = sensor_write_regs(s, init_array_imx298, std::size(init_array_imx298), MSM_CAMERA_I2C_BYTE_DATA);
} else if (s->camera_id == CAMERA_ID_OV8865) {
err = sensor_write_regs(s, init_array_ov8865, ARRAYSIZE(init_array_ov8865), MSM_CAMERA_I2C_BYTE_DATA);
err = sensor_write_regs(s, init_array_ov8865, std::size(init_array_ov8865), MSM_CAMERA_I2C_BYTE_DATA);
} else {
assert(false);
}
@ -549,12 +528,10 @@ static void camera_open(CameraState *s, bool is_road_cam) {
if (is_road_cam) {
// init the actuator
actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERUP;
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
LOG("actuator powerup: %d", err);
cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerup");
actuator_cfg_data.cfgtype = CFG_ACTUATOR_INIT;
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
LOG("actuator init: %d", err);
cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator init");
struct msm_actuator_reg_params_t actuator_reg_params[] = {
{
@ -605,12 +582,11 @@ static void camera_open(CameraState *s, bool is_road_cam) {
},
};
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
LOG("actuator set info: %d", err);
cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set info");
}
if (s->camera_id == CAMERA_ID_IMX298) {
err = sensor_write_regs(s, mode_setting_array_imx298, ARRAYSIZE(mode_setting_array_imx298), MSM_CAMERA_I2C_BYTE_DATA);
err = sensor_write_regs(s, mode_setting_array_imx298, std::size(mode_setting_array_imx298), MSM_CAMERA_I2C_BYTE_DATA);
LOG("sensor setup: %d", err);
}
@ -624,8 +600,7 @@ static void camera_open(CameraState *s, bool is_road_cam) {
}
csiphy_cfg_data.cfgtype = CSIPHY_CFG;
csiphy_cfg_data.cfg.csiphy_params = &csiphy_params;
err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data);
LOG("csiphy configure: %d", err);
cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "csiphy configure");
// CSID: configure csid
#define CSI_STATS 0x35
@ -648,13 +623,11 @@ static void camera_open(CameraState *s, bool is_road_cam) {
csid_cfg_data.cfgtype = CSID_CFG;
csid_cfg_data.cfg.csid_params = &csid_params;
err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data);
LOG("csid configure: %d", err);
cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "csid configure");
// ISP: SMMU_ATTACH
msm_vfe_smmu_attach_cmd smmu_attach_cmd = {.security_mode = 0, .iommu_attach_mode = IOMMU_ATTACH};
err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd);
LOG("isp smmu attach: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd, "isp smmu attach");
// ******************* STREAM RAW *****************************
@ -714,8 +687,7 @@ static void camera_open(CameraState *s, bool is_road_cam) {
ss->buf_request.num_buf = FRAME_BUF_COUNT;
ss->buf_request.buf_type = ISP_PRIVATE_BUF;
ss->buf_request.handle = 0;
err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request);
LOG("isp request buf: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request, "isp request buf");
LOG("got buf handle: 0x%x", ss->buf_request.handle);
// ENQUEUE all buffers
@ -734,16 +706,14 @@ static void camera_open(CameraState *s, bool is_road_cam) {
update_cmd.update_info[0].user_stream_id = ss->stream_req.stream_id;
update_cmd.update_info[0].stream_handle = ss->stream_req.axi_stream_handle;
update_cmd.update_type = UPDATE_STREAM_ADD_BUFQ;
err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd);
LOG("isp update stream: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd, "isp update stream");
}
LOG("******** START STREAMS ********");
sub.id = 0;
sub.type = 0x1ff;
err = ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
LOG("isp subscribe: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub, "isp subscribe");
// ISP: START_STREAM
s->stream_cfg.cmd = START_STREAM;
@ -751,14 +721,13 @@ static void camera_open(CameraState *s, bool is_road_cam) {
for (int i = 0; i < s->stream_cfg.num_streams; i++) {
s->stream_cfg.stream_handle[i] = s->ss[i].stream_req.axi_stream_handle;
}
err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg);
LOG("isp start stream: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp start stream");
}
static void road_camera_start(CameraState *s) {
set_exposure(s, 1.0, 1.0);
int err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA);
int err = sensor_write_regs(s, start_reg_array, std::size(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA);
LOG("sensor start regs: %d", err);
int inf_step = 512 - INFINITY_DAC;
@ -775,8 +744,7 @@ static void road_camera_start(CameraState *s) {
.pos = {INFINITY_DAC, 0},
.delay = {0,}
};
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
LOG("actuator set pos: %d", err);
cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set pos");
// TODO: confirm this isn't needed
/*memset(&actuator_cfg_data, 0, sizeof(actuator_cfg_data));
@ -881,7 +849,8 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
}
static std::optional<float> get_accel_z(SubMaster *sm) {
if (sm->update(0) > 0) {
sm->update(0);
if(sm->updated("sensorEvents")){
for (auto event : (*sm)["sensorEvents"].getSensorEvents()) {
if (event.which() == cereal::SensorEventData::ACCELERATION) {
if (auto v = event.getAcceleration().getV(); v.size() >= 3)
@ -936,7 +905,7 @@ void camera_autoexposure(CameraState *s, float grey_frac) {
static void driver_camera_start(CameraState *s) {
set_exposure(s, 1.0, 1.0);
int err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA);
int err = sensor_write_regs(s, start_reg_array, std::size(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA);
LOG("sensor start regs: %d", err);
}
@ -994,16 +963,13 @@ void cameras_open(MultiCameraState *s) {
// ISPIF: setup
ispif_cfg_data = {.cfg_type = ISPIF_INIT, .csid_version = 0x30050000 /* CSID_VERSION_V35*/};
err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data);
LOG("ispif setup: %d", err);
cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif setup");
ispif_cfg_data = {.cfg_type = ISPIF_CFG, .params = ispif_params};
err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data);
LOG("ispif cfg: %d", err);
cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif cfg");
ispif_cfg_data.cfg_type = ISPIF_START_FRAME_BOUNDARY;
err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data);
LOG("ispif start_frame_boundary: %d", err);
cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif start_frame_boundary");
driver_camera_start(&s->driver_cam);
road_camera_start(&s->road_cam);
@ -1013,20 +979,17 @@ void cameras_open(MultiCameraState *s) {
static void camera_close(CameraState *s) {
// ISP: STOP_STREAM
s->stream_cfg.cmd = STOP_STREAM;
int err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg);
LOG("isp stop stream: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp stop stream");
for (int i = 0; i < 3; i++) {
StreamState *ss = &s->ss[i];
if (ss->stream_req.axi_stream_handle != 0) {
err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request);
LOG("isp release buf: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request, "isp release buf");
struct msm_vfe_axi_stream_release_cmd stream_release = {
.stream_handle = ss->stream_req.axi_stream_handle,
};
err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release);
LOG("isp release stream: %d", err);
cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release, "isp release stream");
}
}
}
@ -1161,7 +1124,7 @@ void cameras_run(MultiCameraState *s) {
while (!do_exit) {
struct pollfd fds[2] = {{.fd = cameras[0]->isp_fd, .events = POLLPRI},
{.fd = cameras[1]->isp_fd, .events = POLLPRI}};
int ret = poll(fds, ARRAYSIZE(fds), 1000);
int ret = poll(fds, std::size(fds), 1000);
if (ret < 0) {
if (errno == EINTR || errno == EAGAIN) continue;
LOGE("poll failed (%d - %d)", ret, errno);
@ -1199,7 +1162,7 @@ void cameras_run(MultiCameraState *s) {
c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){
.frame_id = isp_event_data->frame_id,
.timestamp_eof = timestamp,
.frame_length = (uint32_t)c->cur_frame_length,
.frame_length = (uint32_t)c->frame_length,
.integ_lines = (uint32_t)c->cur_integ_lines,
.global_gain = (uint32_t)c->cur_gain,
.lens_pos = c->cur_lens_pos,

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

@ -1,24 +1,22 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include <pthread.h>
#include <memory>
#include <stdbool.h>
#include <stdint.h>
#include <atomic>
#include "messaging.hpp"
#include <memory>
#include "msmb_isp.h"
#include "msmb_ispif.h"
#include "msmb_camera.h"
#include "msm_cam_sensor.h"
#include "visionbuf.h"
#include "common/mat.h"
#include "common/util.h"
#include "imgproc/utils.h"
#include "camera_common.h"
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionbuf.h"
#include "selfdrive/camerad/cameras/camera_common.h"
#include "selfdrive/camerad/imgproc/utils.h"
#include "selfdrive/camerad/include/msm_cam_sensor.h"
#include "selfdrive/camerad/include/msmb_camera.h"
#include "selfdrive/camerad/include/msmb_isp.h"
#include "selfdrive/camerad/include/msmb_ispif.h"
#include "selfdrive/common/mat.h"
#include "selfdrive/common/util.h"
#define FRAME_BUF_COUNT 4
#define METADATA_BUF_COUNT 4
@ -35,7 +33,7 @@
typedef struct CameraState CameraState;
typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, int frame_length);
typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, uint32_t frame_length);
typedef struct StreamState {
struct msm_isp_buf_request buf_request;
@ -67,10 +65,10 @@ typedef struct CameraState {
// exposure
uint32_t pixel_clock, line_length_pclk;
uint32_t max_gain;
uint32_t frame_length;
unsigned int max_gain;
float cur_exposure_frac, cur_gain_frac;
int cur_gain, cur_integ_lines;
int cur_frame_length;
std::atomic<float> digital_gain;
camera_apply_exposure_func apply_exposure;

172
selfdrive/camerad/cameras/camera_qcom2.cc
View File

@ -1,4 +1,3 @@
#include <stdio.h>
#include <string.h>
#include <errno.h>
@ -11,19 +10,17 @@
#include <math.h>
#include <atomic>
#include "common/util.h"
#include "common/swaglog.h"
#include "camera_qcom2.h"
#include "media/cam_defs.h"
#include "media/cam_isp.h"
#include "media/cam_isp_ife.h"
#include "media/cam_sensor_cmn_header.h"
#include "media/cam_sensor.h"
#include "media/cam_sync.h"
#include "sensor2_i2c.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/camerad/cameras/camera_qcom2.h"
#define FRAME_WIDTH 1928
#define FRAME_HEIGHT 1208
//#define FRAME_STRIDE 1936 // for 8 bit output
@ -140,7 +137,7 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
void sensors_poke(struct CameraState *s, int request_id) {
uint32_t cam_packet_handle = 0;
int size = sizeof(struct cam_packet);
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
pkt->num_cmd_buf = 0;
pkt->kmd_cmd_buf_index = -1;
pkt->header.size = size;
@ -157,14 +154,14 @@ void sensors_poke(struct CameraState *s, int request_id) {
assert(ret == 0);
munmap(pkt, size);
release_fd(s->video0_fd, cam_packet_handle);
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
}
void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) {
// LOGD("sensors_i2c: %d", len);
uint32_t cam_packet_handle = 0;
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
pkt->num_cmd_buf = 1;
pkt->kmd_cmd_buf_index = -1;
pkt->header.size = size;
@ -174,7 +171,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload);
buf_desc[0].type = CAM_CMD_BUF_I2C;
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
i2c_random_wr->header.count = len;
i2c_random_wr->header.op_code = 1;
i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR;
@ -192,10 +189,17 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
assert(ret == 0);
munmap(i2c_random_wr, buf_desc[0].size);
release_fd(s->video0_fd, buf_desc[0].mem_handle);
release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
munmap(pkt, size);
release_fd(s->video0_fd, cam_packet_handle);
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
}
static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings)));
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
unconditional_wait->delay = delay_ms;
unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
return (struct cam_cmd_power *)(unconditional_wait + 1);
};
void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
uint32_t cam_packet_handle = 0;
@ -246,11 +250,8 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
//buf_desc[1].size = buf_desc[1].length = 148;
buf_desc[1].size = buf_desc[1].length = 196;
buf_desc[1].type = CAM_CMD_BUF_I2C;
struct cam_cmd_power *power = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
memset(power, 0, buf_desc[1].size);
struct cam_cmd_unconditional_wait *unconditional_wait;
//void *ptr = power;
struct cam_cmd_power *power_settings = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
memset(power_settings, 0, buf_desc[1].size);
// 7750
/*power->count = 2;
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
@ -259,45 +260,28 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/
// 885a
struct cam_cmd_power *power = power_settings;
power->count = 4;
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
power->power_settings[0].power_seq_type = 3; // clock??
power->power_settings[1].power_seq_type = 1; // analog
power->power_settings[2].power_seq_type = 2; // digital
power->power_settings[3].power_seq_type = 8; // reset low
power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)));
unconditional_wait = (struct cam_cmd_unconditional_wait *)power;
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
unconditional_wait->delay = 5;
unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait));
power = power_set_wait(power, 5);
// set clock
power->count = 1;
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
power->power_settings[0].power_seq_type = 0;
power->power_settings[0].config_val_low = 24000000; //Hz
power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)));
unconditional_wait = (struct cam_cmd_unconditional_wait *)power;
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
unconditional_wait->delay = 10; // ms
unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait));
power = power_set_wait(power, 10);
// 8,1 is this reset?
power->count = 1;
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
power->power_settings[0].power_seq_type = 8;
power->power_settings[0].config_val_low = 1;
power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)));
unconditional_wait = (struct cam_cmd_unconditional_wait *)power;
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
unconditional_wait->delay = 100; // ms
unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait));
power = power_set_wait(power, 100);
// probe happens here
@ -306,39 +290,21 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
power->power_settings[0].power_seq_type = 0;
power->power_settings[0].config_val_low = 0;
power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)));
unconditional_wait = (struct cam_cmd_unconditional_wait *)power;
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
unconditional_wait->delay = 1;
unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait));
power = power_set_wait(power, 1);
// reset high
power->count = 1;
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
power->power_settings[0].power_seq_type = 8;
power->power_settings[0].config_val_low = 1;
power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)));
unconditional_wait = (struct cam_cmd_unconditional_wait *)power;
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
unconditional_wait->delay = 1;
unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait));
power = power_set_wait(power, 1);
// reset low
power->count = 1;
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
power->power_settings[0].power_seq_type = 8;
power->power_settings[0].config_val_low = 0;
power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)));
unconditional_wait = (struct cam_cmd_unconditional_wait *)power;
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
unconditional_wait->delay = 1;
unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait));
power = power_set_wait(power, 1);
// 7750
/*power->count = 1;
@ -352,7 +318,6 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
power->power_settings[0].power_seq_type = 2;
power->power_settings[1].power_seq_type = 1;
power->power_settings[2].power_seq_type = 3;
power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings)));
LOGD("probing the sensor");
int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
@ -360,7 +325,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
munmap(i2c_info, buf_desc[0].size);
release_fd(video0_fd, buf_desc[0].mem_handle);
munmap(power, buf_desc[1].size);
munmap(power_settings, buf_desc[1].size);
release_fd(video0_fd, buf_desc[1].mem_handle);
munmap(pkt, size);
release_fd(video0_fd, cam_packet_handle);
@ -372,7 +337,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
if (io_mem_handle != 0) {
size += sizeof(struct cam_buf_io_cfg);
}
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
pkt->num_cmd_buf = 2;
pkt->kmd_cmd_buf_index = 0;
@ -408,7 +373,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
}
buf_desc[1].type = CAM_CMD_BUF_GENERIC;
buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
// cam_isp_packet_generic_blob_handler
uint32_t tmp[] = {
@ -466,16 +431,16 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
config_dev_cmd.offset = 0;
config_dev_cmd.packet_handle = cam_packet_handle;
int ret = cam_control(s->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
int ret = cam_control(s->multi_cam_state->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
if (ret != 0) {
printf("ISP CONFIG FAILED\n");
}
munmap(buf2, buf_desc[1].size);
release_fd(s->video0_fd, buf_desc[1].mem_handle);
// release_fd(s->video0_fd, buf_desc[0].mem_handle);
release_fd(s->multi_cam_state->video0_fd, buf_desc[1].mem_handle);
// release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
munmap(pkt, size);
release_fd(s->video0_fd, cam_packet_handle);
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
}
void enqueue_buffer(struct CameraState *s, int i, bool dp) {
@ -483,12 +448,12 @@ void enqueue_buffer(struct CameraState *s, int i, bool dp) {
int request_id = s->request_ids[i];
if (s->buf_handle[i]) {
release(s->video0_fd, s->buf_handle[i]);
release(s->multi_cam_state->video0_fd, s->buf_handle[i]);
// wait
struct cam_sync_wait sync_wait = {0};
sync_wait.sync_obj = s->sync_objs[i];
sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
ret = cam_control(s->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
// LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
@ -498,7 +463,7 @@ void enqueue_buffer(struct CameraState *s, int i, bool dp) {
struct cam_sync_info sync_destroy = {0};
strcpy(sync_destroy.name, "NodeOutputPortFence");
sync_destroy.sync_obj = s->sync_objs[i];
ret = cam_control(s->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
// LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
}
@ -507,23 +472,23 @@ void enqueue_buffer(struct CameraState *s, int i, bool dp) {
req_mgr_sched_request.session_hdl = s->session_handle;
req_mgr_sched_request.link_hdl = s->link_handle;
req_mgr_sched_request.req_id = request_id;
ret = cam_control(s->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
// LOGD("sched req: %d %d", ret, request_id);
// create output fence
struct cam_sync_info sync_create = {0};
strcpy(sync_create.name, "NodeOutputPortFence");
ret = cam_control(s->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
// LOGD("fence req: %d %d", ret, sync_create.sync_obj);
s->sync_objs[i] = sync_create.sync_obj;
// configure ISP to put the image in place
struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
mem_mgr_map_cmd.mmu_hdls[0] = s->device_iommu;
mem_mgr_map_cmd.mmu_hdls[0] = s->multi_cam_state->device_iommu;
mem_mgr_map_cmd.num_hdl = 1;
mem_mgr_map_cmd.flags = 1;
mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd;
ret = cam_control(s->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
// LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
@ -544,10 +509,10 @@ void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) {
// ******************* camera *******************
static void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
LOGD("camera init %d", camera_num);
assert(camera_id < ARRAYSIZE(cameras_supported));
s->multi_cam_state = multi_cam_state;
assert(camera_id < std::size(cameras_supported));
s->ci = cameras_supported[camera_id];
assert(s->ci.frame_width != 0);
@ -606,10 +571,10 @@ static void camera_open(CameraState *s) {
// probe the sensor
LOGD("-- Probing sensor %d", s->camera_num);
sensors_init(s->video0_fd, s->sensor_fd, s->camera_num);
sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
memset(&s->req_mgr_session_info, 0, sizeof(s->req_mgr_session_info));
ret = cam_control(s->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
LOGD("get session: %d 0x%X", ret, s->req_mgr_session_info.session_hdl);
s->session_handle = s->req_mgr_session_info.session_hdl;
// access the sensor
@ -689,7 +654,7 @@ static void camera_open(CameraState *s) {
.comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
};
ret = cam_control(s->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
ret = cam_control(s->multi_cam_state->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
LOGD("acquire isp dev: %d", ret);
free(in_port_info);
s->isp_dev_handle = acquire_dev_cmd.dev_handle;
@ -710,7 +675,7 @@ static void camera_open(CameraState *s) {
// acquires done
// config ISP
alloc_w_mmu_hdl(s->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->device_iommu, s->cdm_iommu);
alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu);
config_isp(s, 0, 0, 1, s->buf0_handle, 0);
LOG("-- Configuring sensor");
@ -726,7 +691,7 @@ static void camera_open(CameraState *s) {
{
uint32_t cam_packet_handle = 0;
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
pkt->num_cmd_buf = 1;
pkt->kmd_cmd_buf_index = -1;
pkt->header.size = size;
@ -735,7 +700,7 @@ static void camera_open(CameraState *s) {
buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info);
buf_desc[0].type = CAM_CMD_BUF_GENERIC;
struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
csiphy_info->lane_mask = 0x1f;
csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels??
csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane
@ -754,8 +719,8 @@ static void camera_open(CameraState *s) {
int ret = cam_control(s->csiphy_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
assert(ret == 0);
release(s->video0_fd, buf_desc[0].mem_handle);
release(s->video0_fd, cam_packet_handle);
release(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
release(s->multi_cam_state->video0_fd, cam_packet_handle);
}
// link devices
@ -765,7 +730,7 @@ static void camera_open(CameraState *s) {
req_mgr_link_info.num_devices = 2;
req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
LOGD("link: %d", ret);
s->link_handle = req_mgr_link_info.link_hdl;
@ -774,13 +739,13 @@ static void camera_open(CameraState *s) {
req_mgr_link_control.session_hdl = s->session_handle;
req_mgr_link_control.num_links = 1;
req_mgr_link_control.link_hdls[0] = s->link_handle;
ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
LOGD("link control: %d", ret);
LOGD("start csiphy: %d", ret);
ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
LOGD("start isp: %d", ret);
ret = device_control(s->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
ret = device_control(s->multi_cam_state->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
LOGD("start sensor: %d", ret);
ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
@ -788,13 +753,13 @@ static void camera_open(CameraState *s) {
}
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); // swap left/right
printf("road camera initted \n");
camera_init(v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
VISION_STREAM_RGB_WIDE, VISION_STREAM_YUV_WIDE);
printf("wide road camera initted \n");
camera_init(v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
printf("driver camera initted \n");
@ -810,19 +775,16 @@ void cameras_open(MultiCameraState *s) {
s->video0_fd = open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK);
assert(s->video0_fd >= 0);
LOGD("opened video0");
s->road_cam.video0_fd = s->driver_cam.video0_fd = s->wide_road_cam.video0_fd = s->video0_fd;
// video1 is cam_sync, the target of some ioctls
s->video1_fd = open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK);
assert(s->video1_fd >= 0);
LOGD("opened video1");
s->road_cam.video1_fd = s->driver_cam.video1_fd = s->wide_road_cam.video1_fd = s->video1_fd;
// looks like there's only one of these
s->isp_fd = open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK);
assert(s->isp_fd >= 0);
LOGD("opened isp");
s->road_cam.isp_fd = s->driver_cam.isp_fd = s->wide_road_cam.isp_fd = s->isp_fd;
// query icp for MMU handles
LOG("-- Query ICP for MMU handles");
@ -835,10 +797,8 @@ void cameras_open(MultiCameraState *s) {
assert(ret == 0);
LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure);
LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure);
int device_iommu = isp_query_cap_cmd.device_iommu.non_secure;
int cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure;
s->road_cam.device_iommu = s->driver_cam.device_iommu = s->wide_road_cam.device_iommu = device_iommu;
s->road_cam.cdm_iommu = s->driver_cam.cdm_iommu = s->wide_road_cam.cdm_iommu = cdm_iommu;
s->device_iommu = isp_query_cap_cmd.device_iommu.non_secure;
s->cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure;
// subscribe
LOG("-- Subscribing");
@ -863,7 +823,7 @@ static void camera_close(CameraState *s) {
LOG("-- Stop devices");
// ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
// LOGD("stop sensor: %d", ret);
ret = device_control(s->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
ret = device_control(s->multi_cam_state->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
LOGD("stop isp: %d", ret);
ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle);
LOGD("stop csiphy: %d", ret);
@ -874,7 +834,7 @@ static void camera_close(CameraState *s) {
req_mgr_link_control.session_hdl = s->session_handle;
req_mgr_link_control.num_links = 1;
req_mgr_link_control.link_hdls[0] = s->link_handle;
ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
LOGD("link control stop: %d", ret);
// unlink
@ -882,19 +842,19 @@ static void camera_close(CameraState *s) {
static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0};
req_mgr_unlink_info.session_hdl = s->session_handle;
req_mgr_unlink_info.link_hdl = s->link_handle;
ret = cam_control(s->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
LOGD("unlink: %d", ret);
// release devices
LOGD("-- Release devices");
ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle);
LOGD("release sensor: %d", ret);
ret = device_control(s->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle);
ret = device_control(s->multi_cam_state->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle);
LOGD("release isp: %d", ret);
ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle);
LOGD("release csiphy: %d", ret);
ret = cam_control(s->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
LOGD("destroyed session: %d", ret);
}
@ -923,7 +883,7 @@ void handle_camera_event(CameraState *s, void *evdat) {
// check for skipped frames
if (main_id > s->frame_id_last + 1 && !s->skipped) {
// realign
clear_req_queue(s->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0);
s->skipped = true;
} else if (main_id == s->frame_id_last + 1) {
@ -953,7 +913,7 @@ void handle_camera_event(CameraState *s, void *evdat) {
} else { // not ready
// reset after half second of no response
if (main_id > s->frame_id_last + 10) {
clear_req_queue(s->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0);
s->frame_id_last = main_id;
s->skipped = true;
@ -1141,7 +1101,7 @@ void cameras_run(MultiCameraState *s) {
fds[0].fd = s->video0_fd;
fds[0].events = POLLPRI;
int ret = poll(fds, ARRAYSIZE(fds), 1000);
int ret = poll(fds, std::size(fds), 1000);
if (ret < 0) {
if (errno == EINTR || errno == EAGAIN) continue;
LOGE("poll failed (%d - %d)", ret, errno);

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