openpilot v0.8.3 release
|
@ -48,6 +48,7 @@ selfdrive/loggerd/bootlog
|
|||
selfdrive/sensord/_gpsd
|
||||
selfdrive/sensord/_sensord
|
||||
selfdrive/camerad/camerad
|
||||
selfdrive/camerad/test/ae_gray_test
|
||||
selfdrive/modeld/_modeld
|
||||
selfdrive/modeld/_dmonitoringmodeld
|
||||
/src/
|
||||
|
@ -58,8 +59,6 @@ notebooks
|
|||
xx
|
||||
hyperthneed
|
||||
panda_jungle
|
||||
apks
|
||||
openpilot-apks
|
||||
|
||||
.coverage*
|
||||
coverage.xml
|
||||
|
@ -70,7 +69,7 @@ pandaextra
|
|||
flycheck_*
|
||||
|
||||
cppcheck_report.txt
|
||||
comma.sh
|
||||
comma*.sh
|
||||
|
||||
selfdrive/modeld/thneed/compile
|
||||
models/*.thneed
|
||||
|
|
|
@ -1,5 +1,5 @@
|
|||
def phone(String ip, String step_label, String cmd) {
|
||||
withCredentials([file(credentialsId: 'id_rsa_public', variable: 'key_file')]) {
|
||||
withCredentials([file(credentialsId: 'id_rsa', variable: 'key_file')]) {
|
||||
def ssh_cmd = """
|
||||
ssh -tt -o StrictHostKeyChecking=no -i ${key_file} -p 8022 'comma@${ip}' /usr/bin/bash <<'EOF'
|
||||
|
||||
|
@ -11,6 +11,9 @@ export GIT_BRANCH=${env.GIT_BRANCH}
|
|||
export GIT_COMMIT=${env.GIT_COMMIT}
|
||||
|
||||
source ~/.bash_profile
|
||||
if [ -f /TICI ]; then
|
||||
source /etc/profile
|
||||
fi
|
||||
|
||||
ln -snf ${env.TEST_DIR} /data/pythonpath
|
||||
|
||||
|
@ -126,12 +129,10 @@ pipeline {
|
|||
phone_steps("eon-build", [
|
||||
["build", "SCONS_CACHE=1 scons -j4"],
|
||||
["test athena", "nosetests -s selfdrive/athena/tests/test_athenad_old.py"],
|
||||
["test manager", "python selfdrive/test/test_manager.py"],
|
||||
["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"],
|
||||
["test spinner build", "cd selfdrive/ui/spinner && make clean && make"],
|
||||
["test text window build", "cd selfdrive/ui/text && make clean && make"],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
@ -153,7 +154,6 @@ pipeline {
|
|||
["test boardd loopback", "nosetests -s selfdrive/boardd/tests/test_boardd_loopback.py"],
|
||||
["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
|
||||
["test encoder", "python selfdrive/loggerd/tests/test_encoder.py"],
|
||||
["test camerad", "python selfdrive/camerad/test/test_camerad.py"],
|
||||
["test logcatd", "python selfdrive/logcatd/tests/test_logcatd_android.py"],
|
||||
//["test updater", "python installer/updater/test_updater.py"],
|
||||
])
|
||||
|
@ -169,12 +169,31 @@ 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"],
|
||||
["test camerad", "python selfdrive/camerad/test/test_camerad.py"],
|
||||
//["build release3-staging", "cd release && PUSH=${env.R3_PUSH} ./build_release3.sh"],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
stage('camerad') {
|
||||
steps {
|
||||
phone_steps("eon-party", [
|
||||
["build", "SCONS_CACHE=1 scons -j16"],
|
||||
["test camerad", "python selfdrive/camerad/test/test_camerad.py"],
|
||||
["test exposure", "python selfdrive/camerad/test/test_exposure.py"],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
stage('Tici camerad') {
|
||||
steps {
|
||||
phone_steps("tici-party", [
|
||||
["build", "SCONS_CACHE=1 scons -j16"],
|
||||
["test camerad", "python selfdrive/camerad/test/test_camerad.py"],
|
||||
["test exposure", "python selfdrive/camerad/test/test_exposure.py"],
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
22
README.md
|
@ -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 | All | Stock | 0mph | 3mph |
|
||||
| Acura | RDX 2020-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 |
|
||||
|
@ -82,7 +82,7 @@ Supported Cars
|
|||
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 0mph |
|
||||
| Honda | Passport 2019 | All | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | Pilot 2016-19 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | Ridgeline 2017-20 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| Honda | Ridgeline 2017-21 | Honda Sensing | openpilot | 25mph<sup>1</sup> | 12mph |
|
||||
| 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 |
|
||||
|
@ -97,7 +97,7 @@ Supported Cars
|
|||
| 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, 2021 | TSS-P | Stock<sup>3</sup>| 20mph<sup>1</sup> | 0mph |
|
||||
| Toyota | Avalon 2016-18, 2020-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 |
|
||||
|
@ -161,19 +161,34 @@ Community Maintained Cars and Features
|
|||
| 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 | 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 | 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 |
|
||||
| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Forester 2019-20 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph |
|
||||
| Volkswagen| e-Golf 2014, 2020 | Driver Assistance | Stock | 0mph | 0mph |
|
||||
| Volkswagen| Golf 2015-19 | 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| 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/)
|
||||
|
||||
|
@ -310,7 +325,6 @@ And [follow us on Twitter](https://twitter.com/comma_ai).
|
|||
Directory Structure
|
||||
------
|
||||
.
|
||||
├── apk # The apk files used for the UI
|
||||
├── 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
|
||||
|
|
14
RELEASES.md
|
@ -1,3 +1,17 @@
|
|||
Version 0.8.3 (2021-04-01)
|
||||
========================
|
||||
* New model
|
||||
* Trained on new diverse dataset from 2000+ users from 30+ countries
|
||||
* Trained with improved segnet from the comma-pencil community project
|
||||
* 🥬 Dramatically improved end-to-end lateral performance 🥬
|
||||
* Toggle added to disable the use of lanelines
|
||||
* NEOS update: update packages and support for new UI
|
||||
* New offroad UI based on Qt
|
||||
* Default SSH key only used for setup
|
||||
* Kia Ceed 2019 support thanks to ZanZaD13!
|
||||
* Kia Seltos 2021 support thanks to speedking456!
|
||||
* Added support for many Volkswagen and Škoda models thanks to jyoung8607!
|
||||
|
||||
Version 0.8.2 (2021-02-26)
|
||||
========================
|
||||
* Use model points directly in MPC (no more polyfits), making lateral planning more accurate
|
||||
|
|
113
SConstruct
|
@ -263,60 +263,69 @@ else:
|
|||
Export('envCython')
|
||||
|
||||
# Qt build environment
|
||||
qt_env = None
|
||||
if arch in ["x86_64", "Darwin", "larch64"]:
|
||||
qt_env = env.Clone()
|
||||
qt_env = env.Clone()
|
||||
qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets"]
|
||||
if arch != "aarch64":
|
||||
qt_modules += ["DBus"]
|
||||
|
||||
qt_modules = ["Widgets", "Gui", "Core", "DBus", "Multimedia", "Network", "Concurrent", "WebEngine", "WebEngineWidgets"]
|
||||
|
||||
qt_libs = []
|
||||
if arch == "Darwin":
|
||||
qt_env['QTDIR'] = "/usr/local/opt/qt"
|
||||
QT_BASE = "/usr/local/opt/qt/"
|
||||
qt_dirs = [
|
||||
QT_BASE + "include/",
|
||||
]
|
||||
qt_dirs += [f"{QT_BASE}include/Qt{m}" for m in qt_modules]
|
||||
qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"]
|
||||
qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"]
|
||||
else:
|
||||
qt_env['QTDIR'] = "/usr"
|
||||
qt_dirs = [
|
||||
f"/usr/include/{real_arch}-linux-gnu/qt5",
|
||||
f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.5.1/QtGui",
|
||||
f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.12.8/QtGui",
|
||||
]
|
||||
qt_dirs += [f"/usr/include/{real_arch}-linux-gnu/qt5/Qt{m}" for m in qt_modules]
|
||||
|
||||
qt_libs = [f"Qt5{m}" for m in qt_modules]
|
||||
if arch == "larch64":
|
||||
qt_libs += ["GLESv2", "wayland-client"]
|
||||
elif arch != "Darwin":
|
||||
qt_libs += ["GL"]
|
||||
|
||||
qt_env.Tool('qt')
|
||||
qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"]
|
||||
qt_flags = [
|
||||
"-D_REENTRANT",
|
||||
"-DQT_NO_DEBUG",
|
||||
"-DQT_WIDGETS_LIB",
|
||||
"-DQT_GUI_LIB",
|
||||
"-DQT_CORE_LIB"
|
||||
qt_libs = []
|
||||
if arch == "Darwin":
|
||||
qt_env['QTDIR'] = "/usr/local/opt/qt@5"
|
||||
qt_dirs = [
|
||||
os.path.join(qt_env['QTDIR'], "include"),
|
||||
]
|
||||
qt_env['CXXFLAGS'] += qt_flags
|
||||
qt_env['LIBPATH'] += ['#selfdrive/ui']
|
||||
qt_env['LIBS'] = qt_libs
|
||||
qt_dirs += [f"{qt_env['QTDIR']}/include/Qt{m}" for m in qt_modules]
|
||||
qt_env["LINKFLAGS"] += ["-F" + os.path.join(qt_env['QTDIR'], "lib")]
|
||||
qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"]
|
||||
elif arch == "aarch64":
|
||||
qt_env['QTDIR'] = "/system/comma/usr"
|
||||
qt_dirs = [
|
||||
f"/system/comma/usr/include/qt",
|
||||
]
|
||||
qt_dirs += [f"/system/comma/usr/include/qt/Qt{m}" for m in qt_modules]
|
||||
|
||||
if GetOption("clazy"):
|
||||
checks = [
|
||||
"level0",
|
||||
"level1",
|
||||
"no-range-loop",
|
||||
"no-non-pod-global-static",
|
||||
]
|
||||
qt_env['CXX'] = 'clazy'
|
||||
qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
|
||||
qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
|
||||
qt_libs = [f"Qt5{m}" for m in qt_modules]
|
||||
qt_libs += ['EGL', 'GLESv3', 'c++_shared']
|
||||
else:
|
||||
qt_env['QTDIR'] = "/usr"
|
||||
qt_dirs = [
|
||||
f"/usr/include/{real_arch}-linux-gnu/qt5",
|
||||
f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.12.8/QtGui",
|
||||
]
|
||||
qt_dirs += [f"/usr/include/{real_arch}-linux-gnu/qt5/Qt{m}" for m in qt_modules]
|
||||
|
||||
qt_libs = [f"Qt5{m}" for m in qt_modules]
|
||||
if arch == "larch64":
|
||||
qt_libs += ["GLESv2", "wayland-client"]
|
||||
elif arch != "Darwin":
|
||||
qt_libs += ["GL"]
|
||||
|
||||
qt_env.Tool('qt')
|
||||
qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"]
|
||||
qt_flags = [
|
||||
"-D_REENTRANT",
|
||||
"-DQT_NO_DEBUG",
|
||||
"-DQT_WIDGETS_LIB",
|
||||
"-DQT_GUI_LIB",
|
||||
"-DQT_QUICK_LIB",
|
||||
"-DQT_QUICKWIDGETS_LIB",
|
||||
"-DQT_QML_LIB",
|
||||
"-DQT_CORE_LIB"
|
||||
]
|
||||
qt_env['CXXFLAGS'] += qt_flags
|
||||
qt_env['LIBPATH'] += ['#selfdrive/ui']
|
||||
qt_env['LIBS'] = qt_libs
|
||||
|
||||
if GetOption("clazy"):
|
||||
checks = [
|
||||
"level0",
|
||||
"level1",
|
||||
"no-range-loop",
|
||||
"no-non-pod-global-static",
|
||||
]
|
||||
qt_env['CXX'] = 'clazy'
|
||||
qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
|
||||
qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
|
||||
Export('qt_env')
|
||||
|
||||
|
||||
|
@ -351,6 +360,7 @@ Export('common', 'gpucommon', 'visionipc')
|
|||
# Build openpilot
|
||||
|
||||
SConscript(['cereal/SConscript'])
|
||||
SConscript(['panda/board/SConscript'])
|
||||
SConscript(['opendbc/can/SConscript'])
|
||||
|
||||
SConscript(['phonelibs/SConscript'])
|
||||
|
@ -383,7 +393,6 @@ if arch != "Darwin":
|
|||
|
||||
if real_arch == "x86_64":
|
||||
SConscript(['tools/nui/SConscript'])
|
||||
SConscript(['tools/lib/index_log/SConscript'])
|
||||
|
||||
external_sconscript = GetOption('external_sconscript')
|
||||
if external_sconscript:
|
||||
|
|
|
@ -99,8 +99,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
|||
fanMalfunction @91;
|
||||
cameraMalfunction @92;
|
||||
gpsMalfunction @94;
|
||||
startupOneplus @82;
|
||||
processNotRunning @95;
|
||||
dashcamMode @96;
|
||||
|
||||
radarCanErrorDEPRECATED @15;
|
||||
radarCommIssueDEPRECATED @67;
|
||||
|
@ -120,6 +120,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
|||
focusRecoverActiveDEPRECATED @86;
|
||||
neosUpdateRequiredDEPRECATED @88;
|
||||
modelLagWarningDEPRECATED @93;
|
||||
startupOneplusDEPRECATED @82;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -372,11 +373,11 @@ struct CarParams {
|
|||
brakeMaxV @16 :List(Float32);
|
||||
|
||||
# things about the car in the manual
|
||||
mass @17 :Float32; # [kg] running weight
|
||||
wheelbase @18 :Float32; # [m] distance from rear to front axle
|
||||
centerToFront @19 :Float32; # [m] GC distance to front axle
|
||||
steerRatio @20 :Float32; # [] ratio between front wheels and steering wheel angles
|
||||
steerRatioRear @21 :Float32; # [] rear steering ratio wrt front steering (usually 0)
|
||||
mass @17 :Float32; # [kg] curb weight: all fluids no cargo
|
||||
wheelbase @18 :Float32; # [m] distance from rear axle to front axle
|
||||
centerToFront @19 :Float32; # [m] distance from center of mass to front axle
|
||||
steerRatio @20 :Float32; # [] ratio of steering wheel angle to front wheel angle
|
||||
steerRatioRear @21 :Float32; # [] ratio of steering wheel angle to rear wheel angle (usually 0)
|
||||
|
||||
# things we can derive
|
||||
rotationalInertia @22 :Float32; # [kg*m2] body rotational inertia
|
||||
|
|
|
@ -21,6 +21,21 @@ struct LiveUI @0xc08240f996aefced {
|
|||
awarenessStatus @3 :Float32;
|
||||
}
|
||||
|
||||
struct UiLayoutState @0x88dcce08ad29dda0 {
|
||||
activeApp @0 :App;
|
||||
sidebarCollapsed @1 :Bool;
|
||||
mapEnabled @2 :Bool;
|
||||
mockEngaged @3 :Bool;
|
||||
|
||||
enum App @0x9917470acf94d285 {
|
||||
home @0;
|
||||
music @1;
|
||||
nav @2;
|
||||
settings @3;
|
||||
none @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct OrbslamCorrection @0x8afd33dc9b35e1aa {
|
||||
correctionMonoTime @0 :UInt64;
|
||||
prePositionECEF @1 :List(Float64);
|
||||
|
|
|
@ -39,7 +39,7 @@ struct InitData {
|
|||
|
||||
dirty @9 :Bool;
|
||||
passive @12 :Bool;
|
||||
params @17 :Map(Text, Text);
|
||||
params @17 :Map(Text, Data);
|
||||
|
||||
enum DeviceType {
|
||||
unknown @0;
|
||||
|
@ -356,6 +356,7 @@ struct PandaState @0xa7649e2575e4591e {
|
|||
usbPowerMode @12 :UsbPowerMode;
|
||||
ignitionCan @13 :Bool;
|
||||
safetyModel @14 :Car.CarParams.SafetyModel;
|
||||
safetyParam @20 :Int16;
|
||||
faultStatus @15 :FaultStatus;
|
||||
powerSaveEnabled @16 :Bool;
|
||||
uptime @17 :UInt32;
|
||||
|
@ -521,6 +522,7 @@ struct ControlsState @0x97ff69c53601abf1 {
|
|||
indiState @52 :LateralINDIState;
|
||||
pidState @53 :LateralPIDState;
|
||||
lqrState @55 :LateralLQRState;
|
||||
angleState @58 :LateralAngleState;
|
||||
}
|
||||
|
||||
enum OpenpilotState @0xdbe58b96d2d1ac61 {
|
||||
|
@ -585,6 +587,13 @@ struct ControlsState @0x97ff69c53601abf1 {
|
|||
saturated @5 :Bool;
|
||||
}
|
||||
|
||||
struct LateralAngleState {
|
||||
active @0 :Bool;
|
||||
steeringAngleDeg @1 :Float32;
|
||||
output @2 :Float32;
|
||||
saturated @3 :Bool;
|
||||
}
|
||||
|
||||
# deprecated
|
||||
vEgoDEPRECATED @0 :Float32;
|
||||
vEgoRawDEPRECATED @32 :Float32;
|
||||
|
@ -770,14 +779,17 @@ struct LateralPlan @0xe1e9318e2ae8b51e {
|
|||
dPathPoints @20 :List(Float32);
|
||||
dProb @21 :Float32;
|
||||
|
||||
steeringAngleDeg @8 :Float32; # deg
|
||||
steeringRateDeg @13 :Float32; # deg/s
|
||||
mpcSolutionValid @9 :Bool;
|
||||
angleOffsetDeg @11 :Float32;
|
||||
desire @17 :Desire;
|
||||
laneChangeState @18 :LaneChangeState;
|
||||
laneChangeDirection @19 :LaneChangeDirection;
|
||||
|
||||
# curvature is in rad/m
|
||||
curvature @22 :Float32;
|
||||
curvatureRate @23 :Float32;
|
||||
rawCurvature @24 :Float32;
|
||||
rawCurvatureRate @25 :Float32;
|
||||
|
||||
enum Desire {
|
||||
none @0;
|
||||
turnLeft @1;
|
||||
|
@ -812,6 +824,9 @@ struct LateralPlan @0xe1e9318e2ae8b51e {
|
|||
posenetValidDEPRECATED @16 :Bool;
|
||||
sensorValidDEPRECATED @14 :Bool;
|
||||
paramsValidDEPRECATED @10 :Bool;
|
||||
steeringAngleDegDEPRECATED @8 :Float32; # deg
|
||||
steeringRateDegDEPRECATED @13 :Float32; # deg/s
|
||||
angleOffsetDegDEPRECATED @11 :Float32;
|
||||
}
|
||||
|
||||
struct LiveLocationKalman {
|
||||
|
@ -1102,7 +1117,7 @@ struct LiveMpcData {
|
|||
x @0 :List(Float32);
|
||||
y @1 :List(Float32);
|
||||
psi @2 :List(Float32);
|
||||
delta @3 :List(Float32);
|
||||
curvature @3 :List(Float32);
|
||||
qpIterations @4 :UInt32;
|
||||
calculationTime @5 :UInt64;
|
||||
cost @6 :Float64;
|
||||
|
@ -1122,21 +1137,6 @@ struct LiveLongitudinalMpcData {
|
|||
cost @10 :Float64;
|
||||
}
|
||||
|
||||
struct UiLayoutState {
|
||||
activeApp @0 :App;
|
||||
sidebarCollapsed @1 :Bool;
|
||||
mapEnabled @2 :Bool;
|
||||
mockEngaged @3 :Bool;
|
||||
|
||||
enum App {
|
||||
home @0;
|
||||
music @1;
|
||||
nav @2;
|
||||
settings @3;
|
||||
none @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct Joystick {
|
||||
# convenient for debug and live tuning
|
||||
axes @0: List(Float32);
|
||||
|
@ -1282,7 +1282,6 @@ struct Event {
|
|||
liveTracks @16 :List(LiveTracks);
|
||||
sendcan @17 :List(CanData);
|
||||
liveCalibration @19 :LiveCalibrationData;
|
||||
gpsLocation @21 :GpsLocationData;
|
||||
carState @22 :Car.CarState;
|
||||
carControl @23 :Car.CarControl;
|
||||
longitudinalPlan @24 :LongitudinalPlan;
|
||||
|
@ -1292,7 +1291,6 @@ struct Event {
|
|||
liveLongitudinalMpc @37 :LiveLongitudinalMpcData;
|
||||
ubloxRaw @39 :Data;
|
||||
gpsLocationExternal @48 :GpsLocationData;
|
||||
uiLayoutState @57 :UiLayoutState;
|
||||
driverState @59 :DriverState;
|
||||
liveParameters @61 :LiveParametersData;
|
||||
cameraOdometry @63 :CameraOdometry;
|
||||
|
@ -1356,5 +1354,7 @@ struct Event {
|
|||
orbFeaturesSummaryDEPRECATED @58 :Legacy.OrbFeaturesSummary;
|
||||
featuresDEPRECATED @10 :Legacy.CalibrationFeatures;
|
||||
kalmanOdometryDEPRECATED @65 :Legacy.KalmanOdometry;
|
||||
gpsLocationDEPRECATED @21 :GpsLocationData;
|
||||
uiLayoutStateDEPRECATED @57 :Legacy.UiLayoutState;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -120,3 +120,21 @@ public:
|
|||
private:
|
||||
std::map<std::string, PubSocket *> sockets_;
|
||||
};
|
||||
|
||||
class AlignedBuffer {
|
||||
public:
|
||||
kj::ArrayPtr<const capnp::word> align(const char *data, const size_t size) {
|
||||
words_size = size / sizeof(capnp::word) + 1;
|
||||
if (aligned_buf.size() < words_size) {
|
||||
aligned_buf = kj::heapArray<capnp::word>(words_size < 512 ? 512 : words_size);
|
||||
}
|
||||
memcpy(aligned_buf.begin(), data, size);
|
||||
return aligned_buf.slice(0, words_size);
|
||||
}
|
||||
inline kj::ArrayPtr<const capnp::word> align(Message *m) {
|
||||
return align(m->getData(), m->getSize());
|
||||
}
|
||||
private:
|
||||
kj::Array<capnp::word> aligned_buf;
|
||||
size_t words_size;
|
||||
};
|
||||
|
|
|
@ -39,7 +39,7 @@ struct SubMaster::SubMessage {
|
|||
uint64_t rcv_time = 0, rcv_frame = 0;
|
||||
void *allocated_msg_reader = nullptr;
|
||||
capnp::FlatArrayMessageReader *msg_reader = nullptr;
|
||||
kj::Array<capnp::word> buf;
|
||||
AlignedBuffer aligned_buf;
|
||||
cereal::Event::Reader event;
|
||||
};
|
||||
|
||||
|
@ -56,8 +56,7 @@ SubMaster::SubMaster(const std::initializer_list<const char *> &service_list, co
|
|||
.socket = socket,
|
||||
.freq = serv->frequency,
|
||||
.ignore_alive = inList(ignore_alive, name),
|
||||
.allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)),
|
||||
.buf = kj::heapArray<capnp::word>(1024)};
|
||||
.allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader))};
|
||||
messages_[socket] = m;
|
||||
services_[name] = m;
|
||||
}
|
||||
|
@ -75,17 +74,11 @@ int SubMaster::update(int timeout) {
|
|||
if (msg == nullptr) continue;
|
||||
|
||||
SubMessage *m = messages_.at(s);
|
||||
const size_t size = (msg->getSize() / sizeof(capnp::word)) + 1;
|
||||
if (m->buf.size() < size) {
|
||||
m->buf = kj::heapArray<capnp::word>(size);
|
||||
}
|
||||
memcpy(m->buf.begin(), msg->getData(), msg->getSize());
|
||||
delete msg;
|
||||
|
||||
if (m->msg_reader) {
|
||||
m->msg_reader->~FlatArrayMessageReader();
|
||||
}
|
||||
m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(kj::ArrayPtr<capnp::word>(m->buf.begin(), size));
|
||||
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>();
|
||||
m->updated = true;
|
||||
m->rcv_time = current_time;
|
||||
|
|
|
@ -31,7 +31,6 @@ service_list = {
|
|||
"carControl": Service(8023, True, 100., 10),
|
||||
"longitudinalPlan": Service(8024, True, 20., 2),
|
||||
"liveLocation": Service(8025, True, 0., 1),
|
||||
"gpsLocation": Service(8026, True, 1., 1),
|
||||
"procLog": Service(8031, True, 0.5),
|
||||
"gpsLocationExternal": Service(8032, True, 10., 1),
|
||||
"ubloxGnss": Service(8033, True, 10.),
|
||||
|
|
|
@ -0,0 +1,9 @@
|
|||
# remove all keys that end in DEPRECATED
|
||||
def strip_deprecated_keys(d):
|
||||
for k in list(d.keys()):
|
||||
if isinstance(k, str):
|
||||
if k.endswith('DEPRECATED'):
|
||||
d.pop(k)
|
||||
elif isinstance(d[k], dict):
|
||||
strip_deprecated_keys(d[k])
|
||||
return d
|
|
@ -3,6 +3,7 @@ import os
|
|||
import sys
|
||||
import copy
|
||||
import json
|
||||
import uuid
|
||||
import socket
|
||||
import logging
|
||||
import traceback
|
||||
|
@ -62,8 +63,48 @@ class SwagFormatter(logging.Formatter):
|
|||
return record_dict
|
||||
|
||||
def format(self, record):
|
||||
if self.swaglogger is None:
|
||||
raise Exception("must set swaglogger before calling format()")
|
||||
return json_robust_dumps(self.format_dict(record))
|
||||
|
||||
class SwagLogFileFormatter(SwagFormatter):
|
||||
def fix_kv(self, k, v):
|
||||
# append type to names to preserve legacy naming in logs
|
||||
# avoids overlapping key namespaces with different types
|
||||
# e.g. log.info() creates 'msg' -> 'msg$s'
|
||||
# log.event() creates 'msg.health.logMonoTime' -> 'msg.health.logMonoTime$i'
|
||||
# because overlapping namespace 'msg' caused problems
|
||||
if isinstance(v, (str, bytes)):
|
||||
k += "$s"
|
||||
elif isinstance(v, float):
|
||||
k += "$f"
|
||||
elif isinstance(v, bool):
|
||||
k += "$b"
|
||||
elif isinstance(v, int):
|
||||
k += "$i"
|
||||
elif isinstance(v, dict):
|
||||
nv = {}
|
||||
for ik, iv in v.items():
|
||||
ik, iv = self.fix_kv(ik, iv)
|
||||
nv[ik] = iv
|
||||
v = nv
|
||||
elif isinstance(v, list):
|
||||
k += "$a"
|
||||
return k, v
|
||||
|
||||
def format(self, record):
|
||||
if isinstance(record, str):
|
||||
v = json.loads(record)
|
||||
else:
|
||||
v = self.format_dict(record)
|
||||
|
||||
mk, mv = self.fix_kv('msg', v['msg'])
|
||||
del v['msg']
|
||||
v[mk] = mv
|
||||
v['id'] = uuid.uuid4().hex
|
||||
|
||||
return json_robust_dumps(v)
|
||||
|
||||
class SwagErrorFilter(logging.Filter):
|
||||
def filter(self, record):
|
||||
return record.levelno < logging.ERROR
|
||||
|
@ -117,6 +158,8 @@ class SwagLogger(logging.Logger):
|
|||
evt.update(kwargs)
|
||||
if 'error' in kwargs:
|
||||
self.error(evt)
|
||||
if 'debug' in kwargs:
|
||||
self.debug(evt)
|
||||
else:
|
||||
self.info(evt)
|
||||
|
||||
|
|
|
@ -15,6 +15,9 @@ cdef enum TxType:
|
|||
|
||||
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],
|
||||
|
@ -22,15 +25,18 @@ keys = {
|
|||
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],
|
||||
|
@ -49,12 +55,12 @@ keys = {
|
|||
b"LastUpdateTime": [TxType.PERSISTENT],
|
||||
b"LiveParameters": [TxType.PERSISTENT],
|
||||
b"OpenpilotEnabledToggle": [TxType.PERSISTENT],
|
||||
b"LaneChangeEnabled": [TxType.PERSISTENT],
|
||||
b"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
b"PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
b"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
b"Passive": [TxType.PERSISTENT],
|
||||
b"RecordFront": [TxType.PERSISTENT],
|
||||
b"RecordFrontLock": [TxType.PERSISTENT], # for the internal fleet
|
||||
b"ReleaseNotes": [TxType.PERSISTENT],
|
||||
b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START],
|
||||
b"SubscriberInfo": [TxType.PERSISTENT],
|
||||
|
|
|
@ -25,7 +25,7 @@ class Spinner():
|
|||
pass
|
||||
|
||||
def update_progress(self, cur: int, total: int):
|
||||
self.update(str(int(100 * cur / total)))
|
||||
self.update(str(round(100 * cur / total)))
|
||||
|
||||
def close(self):
|
||||
if self.spinner_proc is not None:
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
{
|
||||
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-e85f507777cb6b22f88ba1c8be6bbaa2630c484b971344b645fca2d1c461cd47.zip",
|
||||
"ota_hash": "e85f507777cb6b22f88ba1c8be6bbaa2630c484b971344b645fca2d1c461cd47",
|
||||
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-db31ffe79dfd60be966fba6d1525a5081a920062b883644dc8f5734bcc6806bb.img",
|
||||
"recovery_len": 15926572,
|
||||
"recovery_hash": "db31ffe79dfd60be966fba6d1525a5081a920062b883644dc8f5734bcc6806bb"
|
||||
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-969e22c42e5c6314e54bc3ccaa5c6a684f3130a53a7a70e0cea9f1453ceb0b06.zip",
|
||||
"ota_hash": "969e22c42e5c6314e54bc3ccaa5c6a684f3130a53a7a70e0cea9f1453ceb0b06",
|
||||
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-9c784a24826c25df315d0ace864224478e9c0e86b904f5d1f8e18ea1037e842b.img",
|
||||
"recovery_len": 15209772,
|
||||
"recovery_hash": "9c784a24826c25df315d0ace864224478e9c0e86b904f5d1f8e18ea1037e842b"
|
||||
}
|
||||
|
|
|
@ -9,6 +9,7 @@ source "$BASEDIR/launch_env.sh"
|
|||
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||
|
||||
function two_init {
|
||||
|
||||
# Wifi scan
|
||||
wpa_cli IFNAME=wlan0 SCAN
|
||||
|
||||
|
@ -54,8 +55,7 @@ function two_init {
|
|||
echo 1 > /proc/irq/6/smp_affinity_list # MDSS
|
||||
|
||||
# USB traffic needs realtime handling on cpu 3
|
||||
[ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco
|
||||
[ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T
|
||||
[ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list
|
||||
|
||||
# GPU and camera get cpu 2
|
||||
CAM_IRQS="177 178 179 180 181 182 183 184 185 186 192"
|
||||
|
@ -91,18 +91,6 @@ function two_init {
|
|||
|
||||
"$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json"
|
||||
fi
|
||||
|
||||
# One-time fix for a subset of OP3T with gyro orientation offsets.
|
||||
# Remove and regenerate qcom sensor registry. Only done on OP3T mainboards.
|
||||
# Performed exactly once. The old registry is preserved just-in-case, and
|
||||
# doubles as a flag denoting we've already done the reset.
|
||||
if ! $(grep -q "letv" /proc/cmdline) && [ ! -f "/persist/comma/op3t-sns-reg-backup" ]; then
|
||||
echo "Performing OP3T sensor registry reset"
|
||||
mv /persist/sensors/sns.reg /persist/comma/op3t-sns-reg-backup &&
|
||||
rm -f /persist/sensors/sensors_settings /persist/sensors/error_log /persist/sensors/gyro_sensitity_cal &&
|
||||
echo "restart" > /sys/kernel/debug/msm_subsys/slpi &&
|
||||
sleep 5 # Give Android sensor subsystem a moment to recover
|
||||
fi
|
||||
}
|
||||
|
||||
function tici_init {
|
||||
|
@ -232,8 +220,8 @@ function launch {
|
|||
tmux capture-pane -pq -S-1000 > /tmp/launch_log
|
||||
|
||||
# start manager
|
||||
cd selfdrive
|
||||
./manager.py
|
||||
cd selfdrive/manager
|
||||
./build.py && ./manager.py
|
||||
|
||||
# if broken, keep on screen error
|
||||
while true; do sleep 1; done
|
||||
|
|
|
@ -7,11 +7,11 @@ export OPENBLAS_NUM_THREADS=1
|
|||
export VECLIB_MAXIMUM_THREADS=1
|
||||
|
||||
if [ -z "$REQUIRED_NEOS_VERSION" ]; then
|
||||
export REQUIRED_NEOS_VERSION="15-1"
|
||||
export REQUIRED_NEOS_VERSION="16.2"
|
||||
fi
|
||||
|
||||
if [ -z "$AGNOS_VERSION" ]; then
|
||||
export AGNOS_VERSION="0.6"
|
||||
export AGNOS_VERSION="0.11"
|
||||
fi
|
||||
|
||||
if [ -z "$PASSIVE" ]; then
|
||||
|
|
|
@ -112,7 +112,7 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
|
|||
case 0x86: // LWI_01 Steering Angle
|
||||
crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter];
|
||||
break;
|
||||
case 0x9F: // EPS_01 Electric Power Steering
|
||||
case 0x9F: // LH_EPS_03 Electric Power Steering
|
||||
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
|
||||
break;
|
||||
case 0xAD: // Getriebe_11 Automatic Gearbox
|
||||
|
@ -125,7 +125,7 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
|
|||
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
|
||||
break;
|
||||
case 0x117: // ACC_10 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter];
|
||||
crc ^= (uint8_t[]){0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16}[counter];
|
||||
break;
|
||||
case 0x120: // TSK_06 Drivetrain Coordinator
|
||||
crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter];
|
||||
|
@ -151,6 +151,9 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
|
|||
case 0x30F: // SWA_01 Lane Change Assist (SpurWechselAssistent)
|
||||
crc ^= (uint8_t[]){0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C}[counter];
|
||||
break;
|
||||
case 0x324: // ACC_04 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27,0x27}[counter];
|
||||
break;
|
||||
case 0x3C0: // Klemmen_Status_01 ignition and starting status
|
||||
crc ^= (uint8_t[]){0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3}[counter];
|
||||
break;
|
||||
|
|
|
@ -5,8 +5,12 @@
|
|||
#include <unordered_map>
|
||||
|
||||
#include "common_dbc.h"
|
||||
#include <capnp/dynamic.h>
|
||||
#include <capnp/serialize.h>
|
||||
|
||||
#ifndef DYNAMIC_CAPNP
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#endif
|
||||
|
||||
#define MAX_BAD_COUNTER 5
|
||||
|
||||
|
@ -36,6 +40,9 @@ public:
|
|||
uint8_t counter;
|
||||
uint8_t counter_fail;
|
||||
|
||||
bool ignore_checksum = false;
|
||||
bool ignore_counter = false;
|
||||
|
||||
bool parse(uint64_t sec, uint16_t ts_, uint8_t * dat);
|
||||
bool update_counter_generic(int64_t v, int cnt_size);
|
||||
};
|
||||
|
@ -43,6 +50,7 @@ public:
|
|||
class CANParser {
|
||||
private:
|
||||
const int bus;
|
||||
kj::Array<capnp::word> aligned_buf;
|
||||
|
||||
const DBC *dbc = NULL;
|
||||
std::unordered_map<uint32_t, MessageState> message_states;
|
||||
|
@ -54,9 +62,13 @@ public:
|
|||
CANParser(int abus, const std::string& dbc_name,
|
||||
const std::vector<MessageParseOptions> &options,
|
||||
const std::vector<SignalParseOptions> &sigoptions);
|
||||
void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans);
|
||||
void UpdateValid(uint64_t sec);
|
||||
CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter);
|
||||
#ifndef DYNAMIC_CAPNP
|
||||
void update_string(const std::string &data, bool sendcan);
|
||||
void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans);
|
||||
#endif
|
||||
void UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cans);
|
||||
void UpdateValid(uint64_t sec);
|
||||
std::vector<SignalValue> query_latest();
|
||||
};
|
||||
|
||||
|
@ -68,5 +80,6 @@ private:
|
|||
|
||||
public:
|
||||
CANPacker(const std::string& dbc_name);
|
||||
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter);
|
||||
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &values, int counter);
|
||||
Msg* lookup_message(uint32_t address);
|
||||
};
|
||||
|
|
|
@ -3,6 +3,7 @@
|
|||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
|
||||
|
||||
|
@ -74,6 +75,7 @@ struct DBC {
|
|||
size_t num_vals;
|
||||
};
|
||||
|
||||
std::vector<const DBC*>& get_dbcs();
|
||||
const DBC* dbc_lookup(const std::string& dbc_name);
|
||||
|
||||
void dbc_register(const DBC* dbc);
|
||||
|
|
|
@ -2,15 +2,11 @@
|
|||
|
||||
#include "common_dbc.h"
|
||||
|
||||
namespace {
|
||||
|
||||
std::vector<const DBC*>& get_dbcs() {
|
||||
static std::vector<const DBC*> vec;
|
||||
return vec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
const DBC* dbc_lookup(const std::string& dbc_name) {
|
||||
for (const auto& dbci : get_dbcs()) {
|
||||
if (dbc_name == dbci->name) {
|
||||
|
|
|
@ -20,7 +20,7 @@ uint64_t ReverseBytes(uint64_t x) {
|
|||
((x & 0x00000000000000ffull) << 56);
|
||||
}
|
||||
|
||||
uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){
|
||||
static uint64_t set_value(uint64_t ret, const Signal& sig, int64_t ival) {
|
||||
int shift = sig.is_little_endian? sig.b1 : sig.bo;
|
||||
uint64_t mask = ((1ULL << sig.b2)-1) << shift;
|
||||
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift;
|
||||
|
@ -59,7 +59,7 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &s
|
|||
WARN("undefined signal %s - %d\n", name.c_str(), address);
|
||||
continue;
|
||||
}
|
||||
auto sig = sig_it->second;
|
||||
const auto& sig = sig_it->second;
|
||||
|
||||
int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor));
|
||||
if (ival < 0) {
|
||||
|
@ -75,7 +75,7 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &s
|
|||
WARN("COUNTER not defined\n");
|
||||
return ret;
|
||||
}
|
||||
auto sig = sig_it->second;
|
||||
const auto& sig = sig_it->second;
|
||||
|
||||
if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) {
|
||||
WARN("COUNTER signal type not valid\n");
|
||||
|
@ -86,7 +86,7 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &s
|
|||
|
||||
auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
|
||||
if (sig_it_checksum != signal_lookup.end()) {
|
||||
auto sig = sig_it_checksum->second;
|
||||
const auto& sig = sig_it_checksum->second;
|
||||
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
||||
unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
|
||||
ret = set_value(ret, sig, chksm);
|
||||
|
@ -112,3 +112,7 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &s
|
|||
|
||||
return ret;
|
||||
}
|
||||
|
||||
Msg* CANPacker::lookup_message(uint32_t address) {
|
||||
return &message_lookup[address];
|
||||
}
|
||||
|
|
|
@ -13,7 +13,6 @@
|
|||
// #define DEBUG printf
|
||||
#define INFO printf
|
||||
|
||||
|
||||
bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) {
|
||||
uint64_t dat_le = read_u64_le(dat);
|
||||
uint64_t dat_be = read_u64_be(dat);
|
||||
|
@ -34,47 +33,52 @@ bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) {
|
|||
|
||||
DEBUG("parse 0x%X %s -> %lld\n", address, sig.name, tmp);
|
||||
|
||||
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
||||
if (honda_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
if (!ignore_checksum) {
|
||||
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
||||
if (honda_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
||||
if (toyota_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
||||
if (volkswagen_crc(address, dat_le, size) != tmp) {
|
||||
INFO("0x%X CRC FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::SUBARU_CHECKSUM) {
|
||||
if (subaru_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::CHRYSLER_CHECKSUM) {
|
||||
if (chrysler_checksum(address, dat_le, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_CHECKSUM) {
|
||||
if (pedal_checksum(dat_be, size) != tmp) {
|
||||
INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
} else if (sig.type == SignalType::HONDA_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
||||
if (toyota_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
||||
if (volkswagen_crc(address, dat_le, size) != tmp) {
|
||||
INFO("0x%X CRC FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
|
||||
}
|
||||
if (!ignore_counter) {
|
||||
if (sig.type == SignalType::HONDA_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::SUBARU_CHECKSUM) {
|
||||
if (subaru_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::CHRYSLER_CHECKSUM) {
|
||||
if (chrysler_checksum(address, dat_le, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_CHECKSUM) {
|
||||
if (pedal_checksum(dat_be, size) != tmp) {
|
||||
INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -108,26 +112,24 @@ bool MessageState::update_counter_generic(int64_t v, int cnt_size) {
|
|||
CANParser::CANParser(int abus, const std::string& dbc_name,
|
||||
const std::vector<MessageParseOptions> &options,
|
||||
const std::vector<SignalParseOptions> &sigoptions)
|
||||
: bus(abus) {
|
||||
: bus(abus), aligned_buf(kj::heapArray<capnp::word>(1024)) {
|
||||
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
init_crc_lookup_tables();
|
||||
|
||||
for (const auto& op : options) {
|
||||
MessageState state = {
|
||||
.address = op.address,
|
||||
// .check_frequency = op.check_frequency,
|
||||
};
|
||||
MessageState &state = message_states[op.address];
|
||||
state.address = op.address;
|
||||
// state.check_frequency = op.check_frequency,
|
||||
|
||||
// msg is not valid if a message isn't received for 10 consecutive steps
|
||||
if (op.check_frequency > 0) {
|
||||
state.check_threshold = (1000000000ULL / op.check_frequency) * 10;
|
||||
}
|
||||
|
||||
|
||||
const Msg* msg = NULL;
|
||||
for (int i=0; i<dbc->num_msgs; i++) {
|
||||
for (int i = 0; i < dbc->num_msgs; i++) {
|
||||
if (dbc->msgs[i].address == op.address) {
|
||||
msg = &dbc->msgs[i];
|
||||
break;
|
||||
|
@ -141,7 +143,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
|
|||
state.size = msg->size;
|
||||
|
||||
// track checksums and counters for this message
|
||||
for (int i=0; i<msg->num_sigs; i++) {
|
||||
for (int i = 0; i < msg->num_sigs; i++) {
|
||||
const Signal *sig = &msg->sigs[i];
|
||||
if (sig->type != SignalType::DEFAULT) {
|
||||
state.parse_sigs.push_back(*sig);
|
||||
|
@ -153,7 +155,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
|
|||
for (const auto& sigop : sigoptions) {
|
||||
if (sigop.address != op.address) continue;
|
||||
|
||||
for (int i=0; i<msg->num_sigs; i++) {
|
||||
for (int i = 0; i < msg->num_sigs; i++) {
|
||||
const Signal *sig = &msg->sigs[i];
|
||||
if (strcmp(sig->name, sigop.name) == 0
|
||||
&& sig->type == SignalType::DEFAULT) {
|
||||
|
@ -162,37 +164,105 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
|
|||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter)
|
||||
: bus(abus) {
|
||||
// Add all messages and signals
|
||||
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
init_crc_lookup_tables();
|
||||
|
||||
for (int i = 0; i < dbc->num_msgs; i++) {
|
||||
const Msg* msg = &dbc->msgs[i];
|
||||
MessageState state = {
|
||||
.address = msg->address,
|
||||
.size = msg->size,
|
||||
.ignore_checksum = ignore_checksum,
|
||||
.ignore_counter = ignore_counter,
|
||||
};
|
||||
|
||||
for (int j = 0; j < msg->num_sigs; j++) {
|
||||
const Signal *sig = &msg->sigs[j];
|
||||
state.parse_sigs.push_back(*sig);
|
||||
state.vals.push_back(0);
|
||||
}
|
||||
|
||||
message_states[state.address] = state;
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef DYNAMIC_CAPNP
|
||||
void CANParser::update_string(const std::string &data, bool sendcan) {
|
||||
// format for board, make copy due to alignment issues.
|
||||
const size_t buf_size = (data.length() / sizeof(capnp::word)) + 1;
|
||||
if (aligned_buf.size() < buf_size) {
|
||||
aligned_buf = kj::heapArray<capnp::word>(buf_size);
|
||||
}
|
||||
memcpy(aligned_buf.begin(), data.data(), data.length());
|
||||
|
||||
// extract the messages
|
||||
capnp::FlatArrayMessageReader cmsg(aligned_buf.slice(0, buf_size));
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
last_sec = event.getLogMonoTime();
|
||||
|
||||
auto cans = sendcan? event.getSendcan() : event.getCan();
|
||||
UpdateCans(last_sec, cans);
|
||||
|
||||
UpdateValid(last_sec);
|
||||
}
|
||||
|
||||
void CANParser::UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans) {
|
||||
int msg_count = cans.size();
|
||||
int msg_count = cans.size();
|
||||
|
||||
DEBUG("got %d messages\n", msg_count);
|
||||
DEBUG("got %d messages\n", msg_count);
|
||||
|
||||
for (int i = 0; i < msg_count; i++) {
|
||||
auto cmsg = cans[i];
|
||||
// parse the messages
|
||||
for (int i = 0; i < msg_count; i++) {
|
||||
auto cmsg = cans[i];
|
||||
if (cmsg.getSrc() != bus) {
|
||||
// DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
auto state_it = message_states.find(cmsg.getAddress());
|
||||
if (state_it == message_states.end()) {
|
||||
// DEBUG("skip %d: not specified\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmsg.getDat().size() > 8) continue; //shouldn't ever happen
|
||||
uint8_t dat[8] = {0};
|
||||
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
|
||||
|
||||
state_it->second.parse(sec, cmsg.getBusTime(), dat);
|
||||
if (cmsg.getSrc() != bus) {
|
||||
// DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
auto state_it = message_states.find(cmsg.getAddress());
|
||||
if (state_it == message_states.end()) {
|
||||
// DEBUG("skip %d: not specified\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmsg.getDat().size() > 8) continue; //shouldn't ever happen
|
||||
uint8_t dat[8] = {0};
|
||||
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
|
||||
|
||||
state_it->second.parse(sec, cmsg.getBusTime(), dat);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void CANParser::UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cmsg) {
|
||||
// assume message struct is `cereal::CanData` and parse
|
||||
assert(cmsg.has("address") && cmsg.has("src") && cmsg.has("dat") && cmsg.has("busTime"));
|
||||
|
||||
if (cmsg.get("src").as<uint8_t>() != bus) {
|
||||
DEBUG("skip %d: wrong bus\n", cmsg.get("address").as<uint32_t>());
|
||||
return;
|
||||
}
|
||||
|
||||
auto state_it = message_states.find(cmsg.get("address").as<uint32_t>());
|
||||
if (state_it == message_states.end()) {
|
||||
DEBUG("skip %d: not specified\n", cmsg.get("address").as<uint32_t>());
|
||||
return;
|
||||
}
|
||||
|
||||
auto dat = cmsg.get("dat").as<capnp::Data>();
|
||||
if (dat.size() > 8) return; //shouldn't ever happen
|
||||
uint8_t data[8] = {0};
|
||||
memcpy(data, dat.begin(), dat.size());
|
||||
state_it->second.parse(sec, cmsg.get("busTime").as<uint16_t>(), data);
|
||||
}
|
||||
|
||||
void CANParser::UpdateValid(uint64_t sec) {
|
||||
|
@ -208,24 +278,6 @@ void CANParser::UpdateValid(uint64_t sec) {
|
|||
}
|
||||
}
|
||||
|
||||
void CANParser::update_string(const std::string &data, bool sendcan) {
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((data.length() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), data.data(), data.length());
|
||||
|
||||
// extract the messages
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
last_sec = event.getLogMonoTime();
|
||||
|
||||
auto cans = sendcan? event.getSendcan() : event.getCan();
|
||||
UpdateCans(last_sec, cans);
|
||||
|
||||
UpdateValid(last_sec);
|
||||
}
|
||||
|
||||
|
||||
std::vector<SignalValue> CANParser::query_latest() {
|
||||
std::vector<SignalValue> ret;
|
||||
|
||||
|
|
|
@ -106,8 +106,12 @@ def process(in_fn, out_fn):
|
|||
|
||||
parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
|
||||
|
||||
with open(out_fn, "w") as out_f:
|
||||
out_f.write(parser_code)
|
||||
with open(out_fn, "a+") as out_f:
|
||||
out_f.seek(0)
|
||||
if out_f.read() != parser_code:
|
||||
out_f.seek(0)
|
||||
out_f.truncate()
|
||||
out_f.write(parser_code)
|
||||
|
||||
def main():
|
||||
if len(sys.argv) != 3:
|
||||
|
|
|
@ -133,8 +133,8 @@ BO_ 481 ASCMSteeringButton: 7 K124_ASCM
|
|||
SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 485 PSCMSteeringAngle: 8 K43_PSCM
|
||||
SG_ SteeringWheelAngle : 15|16@0- (0.0625,0) [-540|540] "deg" NEO
|
||||
SG_ SteeringWheelRate : 27|12@0- (0.5,0) [-100|100] "deg/s" NEO
|
||||
SG_ SteeringWheelAngle : 15|16@0- (0.0625,0) [-2047|2047] "deg" NEO
|
||||
SG_ SteeringWheelRate : 27|12@0- (1,0) [-2047|2047] "deg/s" NEO
|
||||
|
||||
BO_ 489 EBCMVehicleDynamic: 8 K17_EBCM
|
||||
SG_ BrakePedalPressed : 6|1@0+ (1,0) [0|0] "" NEO
|
||||
|
|
|
@ -1194,18 +1194,18 @@ BO_ 294 HCA_01: 8 XXX
|
|||
SG_ SET_ME_0XFE : 40|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_0X07 : 48|8@1+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 159 EPS_01: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Steering_Wheel_Angle : 16|13@1+ (0.15,0) [0|16383] "Unit_DegreOfArc" XXX
|
||||
SG_ Steering_Wheel_Angle_VZ : 31|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ HCA_Active : 34|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ HCA_Standby : 33|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Unknown_Status2 : 63|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ HCA_Ready : 32|1@1+ (1,0) [0|3] "" XXX
|
||||
SG_ Driver_Strain : 40|13@1+ (1,0) [0|255] "Nm" XXX
|
||||
SG_ Driver_Strain_VZ : 55|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Unknown_Status1 : 62|1@1+ (1,0) [0|1] "" XXX
|
||||
BO_ 159 LH_EPS_03: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ EPS_DSR_Status : 12|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ EPS_Berechneter_LW : 16|12@1+ (0.15,0) [0|613.95] "Unit_DegreOfArc" XXX
|
||||
SG_ EPS_BLW_QBit : 30|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ EPS_VZ_BLW : 31|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ EPS_HCA_Status : 32|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ EPS_Lenkmoment : 40|10@1+ (1,0) [0|8] "Unit_centiNewtoMeter" XXX
|
||||
SG_ EPS_Lenkmoment_QBit : 54|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ EPS_VZ_Lenkmoment : 55|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ EPS_Lenkungstyp : 60|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 286 VehicleSpeed: 8 XXX
|
||||
SG_ VehicleSpeed_CRC : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
|
@ -1213,34 +1213,44 @@ BO_ 286 VehicleSpeed: 8 XXX
|
|||
SG_ Speed : 52|12@1+ (0.125,0) [0|1] "" XXX
|
||||
|
||||
BO_ 919 LDW_02: 8 XXX
|
||||
SG_ LDW_DLC : 40|8@1+ (0.01,0) [0|255] "m" XXX
|
||||
SG_ LDW_TLC : 48|5@1+ (0.05,0) [0|255] "Seconds" XXX
|
||||
SG_ LDW_Unknown : 14|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ Alert_Message : 16|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ LDW_Direction : 20|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Right_Lane_Status : 36|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ Left_Lane_Status : 38|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ Kombi_Lamp_Orange : 61|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Kombi_Lamp_Green : 62|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_Gong : 12|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ LDW_SW_Warnung_links : 14|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_SW_Warnung_rechts : 15|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_Texte : 16|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ LDW_Seite_DLCTLC : 20|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_Lernmodus : 21|3@1+ (1,0) [0|7] "" XXX
|
||||
SG_ LDW_Anlaufsp_VLR : 24|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ LDW_Vib_Amp_VLR : 28|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ LDW_Anlaufzeit_VLR : 32|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ LDW_Lernmodus_rechts : 36|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ LDW_Lernmodus_links : 38|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ LDW_DLC : 40|8@1+ (0.01,-1.25) [-1.25|1.25] "Unit_Meter" XXX
|
||||
SG_ LDW_TLC : 48|5@1+ (0.1,0) [0|3] "Unit_Secon" XXX
|
||||
SG_ LDW_Warnung_links : 56|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_Warnung_rechts : 57|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_Codierinfo_fuer_VLR : 58|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ LDW_Frontscheibenheizung_aktiv : 60|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_Status_LED_gelb : 61|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_Status_LED_gruen : 62|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ LDW_KD_Fehler : 63|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 780 ACC_02: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ SetSpeed : 12|10@1+ (0.08865,0) [0|90.68] "Unit_MeterPerSecond" XXX
|
||||
SG_ Kollision1 : 23|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Abstand : 24|10@1+ (0.1,0) [0|102.3] "m" XXX
|
||||
SG_ ACC_MinusInv : 36|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ ACC_Minus : 38|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ Kollision2 : 40|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ MotorbitB5_1 : 41|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ 1_aktivieren : 42|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Tacho_LED : 43|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Hebelquit : 44|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ 1_aktivieren_inv : 45|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Folgefahrt : 46|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ MotorbitB5_7 : 47|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ SetAbstand : 48|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Hebel : 56|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ ACC_Wunschgeschw : 12|10@1+ (0.32,0) [0.00|326.72] "Unit_KiloMeterPerHour" XXX
|
||||
SG_ ACC_Status_Prim_Anz : 22|2@1+ (1.0,0.0) [0.0|3] "" XXX
|
||||
SG_ ACC_Abstandsindex : 24|10@1+ (1,0) [1|1021] "" XXX
|
||||
SG_ ACC_Akustik : 34|3@1+ (1.0,0.0) [0.0|7] "" XXX
|
||||
SG_ ACC_Gesetzte_Zeitluecke : 37|3@1+ (1.0,0.0) [0.0|7] "" XXX
|
||||
SG_ ACC_Optischer_Fahrerhinweis : 40|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ ACC_Typ_Tachokranz : 41|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ ACC_Anzeige_Zeitluecke : 42|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ ACC_Tachokranz : 43|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ ACC_Display_Prio : 44|2@1+ (1.0,0.0) [0.0|3] "" XXX
|
||||
SG_ ACC_Relevantes_Objekt : 46|2@1+ (1.0,0.0) [0.0|3] "" XXX
|
||||
SG_ ACC_Texte_Primaeranz : 48|7@1+ (1.0,0.0) [0.0|127] "" XXX
|
||||
SG_ ACC_Wunschgeschw_erreicht : 55|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ ACC_Status_Anzeige : 61|3@1+ (1.0,0.0) [0.0|7] "" XXX
|
||||
|
||||
BO_ 302 ACC_07: 8 XXX
|
||||
SG_ ACC_07_BZ : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
|
@ -1276,8 +1286,17 @@ BO_ 783 SWA_01: 8 Gateway_MQB
|
|||
SG_ SWA_KD_Fehler : 59|1@1+ (1,0) [0|1] "" Vector__XXX
|
||||
|
||||
BO_ 804 ACC_04: 8 XXX
|
||||
SG_ ACC_04_CRC : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ ACC_04_BZ : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ ACC_Texte_Zusatzanz : 16|6@1+ (1.0,0.0) [0.0|63] "" XXX
|
||||
SG_ ACC_Status_Zusatzanz : 22|5@1+ (1.0,0.0) [0.0|31] "" XXX
|
||||
SG_ ACC_Texte : 27|5@1+ (1.0,0.0) [0.0|31] "" XXX
|
||||
SG_ ACC_Texte_braking_guard : 32|3@1+ (1.0,0.0) [0.0|7] "" XXX
|
||||
SG_ ACC_Warnhinweis : 35|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ ACC_Geschw_Zielfahrzeug : 40|10@1+ (0.32,0) [0.00|326.72] "Unit_KiloMeterPerHour" XXX
|
||||
SG_ ACC_Charisma_FahrPr : 56|3@1+ (1.0,0.0) [0.0|7] "" XXX
|
||||
SG_ ACC_Charisma_Status : 59|2@1+ (1.0,0.0) [0.0|3] "" XXX
|
||||
SG_ ACC_Charisma_Umschaltung : 61|2@1+ (1.0,0.0) [0.0|3] "" XXX
|
||||
|
||||
BO_ 917 LWR_AFS_01: 8 XXX
|
||||
|
||||
|
|
|
@ -16,3 +16,4 @@ examples/output.csv
|
|||
.vscode*
|
||||
nosetests.xml
|
||||
.mypy_cache/
|
||||
.sconsign.dblite
|
||||
|
|
|
@ -1,3 +1,3 @@
|
|||
# flake8: noqa
|
||||
# pylint: skip-file
|
||||
from .python import Panda, PandaWifiStreaming, PandaDFU, flash_release, BASEDIR, ensure_st_up_to_date, build_st, PandaSerial
|
||||
from .python import Panda, PandaWifiStreaming, PandaDFU, flash_release, BASEDIR, ensure_st_up_to_date, PandaSerial
|
||||
|
|
|
@ -1,8 +0,0 @@
|
|||
PROJ_NAME = panda
|
||||
CFLAGS = -g -Wall -Wextra -Wstrict-prototypes -Werror
|
||||
|
||||
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4
|
||||
CFLAGS += -mhard-float -DSTM32F4 -DSTM32F413xx -mfpu=fpv4-sp-d16 -fsingle-precision-constant
|
||||
STARTUP_FILE = startup_stm32f413xx
|
||||
|
||||
include build.mk
|
|
@ -21,7 +21,8 @@ Programming
|
|||
**Panda**
|
||||
|
||||
```
|
||||
make
|
||||
scons -u # Compile
|
||||
./flash.sh # Compile & Flash
|
||||
```
|
||||
|
||||
Troubleshooting
|
||||
|
@ -29,7 +30,7 @@ Troubleshooting
|
|||
|
||||
If your panda will not flash and is quickly blinking a single Green LED, use:
|
||||
```
|
||||
make recover
|
||||
./recover.sh
|
||||
```
|
||||
|
||||
|
||||
|
|
|
@ -0,0 +1,158 @@
|
|||
import os
|
||||
import subprocess
|
||||
|
||||
EON = os.path.isfile('/EON')
|
||||
TICI = os.path.isfile('/TICI')
|
||||
PC = not (EON or TICI)
|
||||
|
||||
PREFIX = "arm-none-eabi-"
|
||||
BUILDER = "DEV"
|
||||
|
||||
if os.getenv("PEDAL"):
|
||||
PROJECT = "pedal"
|
||||
STARTUP_FILE = "startup_stm32f205xx.s"
|
||||
MAIN = "pedal/main.c"
|
||||
PROJECT_FLAGS = [
|
||||
"-mcpu=cortex-m3",
|
||||
"-msoft-float",
|
||||
"-DSTM32F2",
|
||||
"-DSTM32F205xx",
|
||||
"-O2",
|
||||
"-DPEDAL",
|
||||
]
|
||||
|
||||
else:
|
||||
PROJECT = "panda"
|
||||
STARTUP_FILE = "startup_stm32f413xx.s"
|
||||
MAIN = "main.c"
|
||||
PROJECT_FLAGS = [
|
||||
"-mcpu=cortex-m4",
|
||||
"-mhard-float",
|
||||
"-DSTM32F4",
|
||||
"-DSTM32F413xx",
|
||||
"-mfpu=fpv4-sp-d16",
|
||||
"-fsingle-precision-constant",
|
||||
"-Os",
|
||||
"-g",
|
||||
]
|
||||
|
||||
if not PC:
|
||||
PROJECT_FLAGS += ["-DEON"]
|
||||
BUILDER = "EON"
|
||||
|
||||
|
||||
def get_version(builder, build_type):
|
||||
version_file = File('../VERSION').srcnode().abspath
|
||||
version = open(version_file).read()
|
||||
try:
|
||||
git = subprocess.check_output(["git", "rev-parse", "--short=8", "HEAD"], encoding='utf8').strip()
|
||||
except subprocess.CalledProcessError:
|
||||
git = "unknown"
|
||||
return f"{version}-{builder}-{git}-{build_type}"
|
||||
|
||||
|
||||
def to_c_uint32(x):
|
||||
nums = []
|
||||
for _ in range(0x20):
|
||||
nums.append(x % (2**32))
|
||||
x //= (2**32)
|
||||
return "{" + 'U,'.join(map(str, nums)) + "U}"
|
||||
|
||||
|
||||
def get_key_header(name):
|
||||
from Crypto.PublicKey import RSA
|
||||
|
||||
public_fn = File(f'../certs/{name}.pub').srcnode().abspath
|
||||
rsa = RSA.importKey(open(public_fn).read())
|
||||
assert(rsa.size_in_bits() == 1024)
|
||||
|
||||
rr = pow(2**1024, 2, rsa.n)
|
||||
n0inv = 2**32 - pow(rsa.n, -1, 2**32)
|
||||
|
||||
r = [
|
||||
f"RSAPublicKey {name}_rsa_key = {{",
|
||||
f" .len = 0x20,",
|
||||
f" .n0inv = {n0inv}U,",
|
||||
f" .n = {to_c_uint32(rsa.n)},",
|
||||
f" .rr = {to_c_uint32(rr)},",
|
||||
f" .exponent = {rsa.e},",
|
||||
f"}};",
|
||||
]
|
||||
return r
|
||||
|
||||
|
||||
def objcopy(source, target, env, for_signature):
|
||||
return '$OBJCOPY -O binary %s %s' % (source[0], target[0])
|
||||
|
||||
|
||||
linkerscript_fn = File("stm32_flash.ld").srcnode().abspath
|
||||
|
||||
flags = [
|
||||
"-Wall",
|
||||
"-Wextra",
|
||||
"-Wstrict-prototypes",
|
||||
"-Werror",
|
||||
"-mlittle-endian",
|
||||
"-mthumb",
|
||||
"-nostdlib",
|
||||
"-fno-builtin",
|
||||
f"-T{linkerscript_fn}",
|
||||
"-std=gnu11",
|
||||
] + PROJECT_FLAGS
|
||||
|
||||
|
||||
if os.getenv("RELEASE"):
|
||||
BUILD_TYPE = "RELEASE"
|
||||
cert_fn = os.getenv("CERT")
|
||||
assert cert_fn is not None, 'No certificate file specified. Please set CERT env variable'
|
||||
assert os.path.exists(cert_fn), 'Certificate file not found. Please specify absolute path'
|
||||
else:
|
||||
BUILD_TYPE = "DEBUG"
|
||||
cert_fn = File("../certs/debug").srcnode().abspath
|
||||
flags += ["-DALLOW_DEBUG"]
|
||||
|
||||
includes = [
|
||||
"inc",
|
||||
"..",
|
||||
".",
|
||||
]
|
||||
|
||||
panda_env = Environment(
|
||||
ENV=os.environ,
|
||||
CC=PREFIX + 'gcc',
|
||||
AS=PREFIX + 'gcc',
|
||||
OBJCOPY=PREFIX + 'objcopy',
|
||||
OBJDUMP=PREFIX + 'objdump',
|
||||
ASCOM="$AS $ASFLAGS -o $TARGET -c $SOURCES",
|
||||
CFLAGS=flags,
|
||||
ASFLAGS=flags,
|
||||
LINKFLAGS=flags,
|
||||
CPPPATH=includes,
|
||||
BUILDERS={
|
||||
'Objcopy': Builder(generator=objcopy, suffix='.bin', src_suffix='.elf')
|
||||
}
|
||||
)
|
||||
|
||||
# Common autogenerated includes
|
||||
version = f'const uint8_t gitversion[] = "{get_version(BUILDER, BUILD_TYPE)}";'
|
||||
gitversion = panda_env.Textfile("obj/gitversion.h", [version, ""])
|
||||
certs = [get_key_header(n) for n in ["debug", "release"]]
|
||||
certheader = panda_env.Textfile("obj/cert.h", certs + [""])
|
||||
|
||||
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
|
||||
sign_py = File("../crypto/sign.py").srcnode().abspath
|
||||
panda_bin_signed = panda_env.Command(f"obj/{PROJECT}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}")
|
|
@ -1,91 +0,0 @@
|
|||
CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -Os
|
||||
|
||||
CFLAGS += -Tstm32_flash.ld
|
||||
|
||||
DFU_UTIL = "dfu-util"
|
||||
|
||||
PC = 0
|
||||
|
||||
ifeq (,$(wildcard /EON))
|
||||
ifeq (,$(wildcard /TICI))
|
||||
PC = 1
|
||||
endif
|
||||
endif
|
||||
|
||||
ifeq (1, $(PC))
|
||||
BUILDER = DEV
|
||||
else
|
||||
CFLAGS += "-DEON"
|
||||
BUILDER = EON
|
||||
endif
|
||||
|
||||
#COMPILER_PATH = /home/batman/Downloads/gcc-arm-none-eabi-9-2020-q2-update/bin/
|
||||
CC = $(COMPILER_PATH)arm-none-eabi-gcc
|
||||
OBJCOPY = $(COMPILER_PATH)arm-none-eabi-objcopy
|
||||
OBJDUMP = $(COMPILER_PATH)arm-none-eabi-objdump
|
||||
|
||||
ifeq ($(RELEASE),1)
|
||||
CERT = ../../pandaextra/certs/release
|
||||
else
|
||||
# enable the debug cert
|
||||
CERT = ../certs/debug
|
||||
CFLAGS += "-DALLOW_DEBUG"
|
||||
endif
|
||||
|
||||
|
||||
DEPDIR = generated_dependencies
|
||||
$(shell mkdir -p -m 777 $(DEPDIR) >/dev/null)
|
||||
DEPFLAGS = -MT $@ -MMD -MP -MF $(DEPDIR)/$*.Td
|
||||
POSTCOMPILE = @mv -f $(DEPDIR)/$*.Td $(DEPDIR)/$*.d && touch $@
|
||||
|
||||
# this no longer pushes the bootstub
|
||||
flash: obj/$(PROJ_NAME).bin
|
||||
PYTHONPATH=../ python3 -c "from python import Panda; Panda().flash('obj/$(PROJ_NAME).bin')"
|
||||
|
||||
ota: obj/$(PROJ_NAME).bin
|
||||
curl http://192.168.0.10/stupdate --upload-file $<
|
||||
|
||||
bin: obj/$(PROJ_NAME).bin
|
||||
|
||||
# this flashes everything
|
||||
recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin
|
||||
-PYTHONPATH=../ python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)"
|
||||
sleep 1.0
|
||||
$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin
|
||||
$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin
|
||||
|
||||
include ../common/version.mk
|
||||
|
||||
obj/cert.h: ../crypto/getcertheader.py
|
||||
../crypto/getcertheader.py ../certs/debug.pub ../certs/release.pub > $@
|
||||
|
||||
obj/%.$(PROJ_NAME).o: %.c obj/gitversion.h obj/cert.h $(DEPDIR)/%.d
|
||||
$(CC) $(DEPFLAGS) $(CFLAGS) -o $@ -c $<
|
||||
$(POSTCOMPILE)
|
||||
|
||||
obj/%.$(PROJ_NAME).o: ../crypto/%.c
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/$(STARTUP_FILE).o: $(STARTUP_FILE).s
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.$(PROJ_NAME).o
|
||||
# hack
|
||||
$(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^
|
||||
$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin
|
||||
SETLEN=1 ../crypto/sign.py obj/code.bin $@ $(CERT)
|
||||
@BINSIZE=$$(du -b "obj/$(PROJ_NAME).bin" | cut -f 1) ; \
|
||||
if [ $$BINSIZE -ge 49152 ]; then echo "ERROR obj/$(PROJ_NAME).bin is too big!"; exit 1; fi;
|
||||
|
||||
obj/bootstub.$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/bootstub.$(PROJ_NAME).o obj/sha.$(PROJ_NAME).o obj/rsa.$(PROJ_NAME).o
|
||||
$(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^
|
||||
$(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@
|
||||
|
||||
$(DEPDIR)/%.d: ;
|
||||
.PRECIOUS: $(DEPDIR)/%.d
|
||||
|
||||
include $(wildcard $(patsubst %,$(DEPDIR)/%.d,$(basename $(wildcard *.c))))
|
||||
|
||||
clean:
|
||||
@$(RM) obj/*
|
||||
@rm -rf $(DEPDIR)
|
|
@ -0,0 +1,5 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
scons -u
|
||||
PYTHONPATH=.. python3 -c "from python import Panda; Panda().flash('obj/panda.bin.signed')"
|
|
@ -58,6 +58,7 @@ struct __attribute__((packed)) health_t {
|
|||
uint8_t car_harness_status_pkt;
|
||||
uint8_t usb_power_mode_pkt;
|
||||
uint8_t safety_mode_pkt;
|
||||
int16_t safety_param_pkt;
|
||||
uint8_t fault_status_pkt;
|
||||
uint8_t power_save_enabled_pkt;
|
||||
};
|
||||
|
@ -179,6 +180,7 @@ int get_health_pkt(void *dat) {
|
|||
health->car_harness_status_pkt = car_harness_status;
|
||||
health->usb_power_mode_pkt = usb_power_mode;
|
||||
health->safety_mode_pkt = (uint8_t)(current_safety_mode);
|
||||
health->safety_param_pkt = current_safety_param;
|
||||
health->power_save_enabled_pkt = (uint8_t)(power_save_status == POWER_SAVE_STATUS_ENABLED);
|
||||
|
||||
health->fault_status_pkt = fault_status;
|
||||
|
|
|
@ -1,67 +0,0 @@
|
|||
# :set noet
|
||||
PROJ_NAME = comma
|
||||
|
||||
CFLAGS = -O2 -Wall -Wextra -Wstrict-prototypes -Werror -std=gnu11 -DPEDAL
|
||||
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3
|
||||
CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx
|
||||
CFLAGS += -I ../inc -I ../ -I ../../ -nostdlib -fno-builtin
|
||||
CFLAGS += -T../stm32_flash.ld
|
||||
|
||||
STARTUP_FILE = startup_stm32f205xx
|
||||
|
||||
CC = arm-none-eabi-gcc
|
||||
OBJCOPY = arm-none-eabi-objcopy
|
||||
OBJDUMP = arm-none-eabi-objdump
|
||||
DFU_UTIL = "dfu-util"
|
||||
|
||||
# pedal only uses the debug cert
|
||||
CERT = ../../certs/debug
|
||||
CFLAGS += "-DALLOW_DEBUG"
|
||||
|
||||
canflash: obj/$(PROJ_NAME).bin
|
||||
../../tests/pedal/enter_canloader.py $<
|
||||
|
||||
usbflash: obj/$(PROJ_NAME).bin
|
||||
../../tests/pedal/enter_canloader.py; sleep 0.5
|
||||
PYTHONPATH=../../ python -c "from python import Panda; p = [x for x in [Panda(x) for x in Panda.list()] if x.bootstub]; assert(len(p)==1); p[0].flash('obj/$(PROJ_NAME).bin', reconnect=False)"
|
||||
|
||||
recover: obj/bootstub.bin obj/$(PROJ_NAME).bin
|
||||
../../tests/pedal/enter_canloader.py --recover; sleep 0.5
|
||||
$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin
|
||||
$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.bin
|
||||
|
||||
include ../../common/version.mk
|
||||
|
||||
obj/cert.h: ../../crypto/getcertheader.py
|
||||
../../crypto/getcertheader.py ../../certs/debug.pub ../../certs/release.pub > $@
|
||||
|
||||
obj/main.o: main.c ../*.h
|
||||
mkdir -p obj
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/bootstub.o: ../bootstub.c ../*.h obj/gitversion.h obj/cert.h
|
||||
mkdir -p obj
|
||||
mkdir -p ../obj
|
||||
cp obj/gitversion.h ../obj/gitversion.h
|
||||
cp obj/cert.h ../obj/cert.h
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/$(STARTUP_FILE).o: ../$(STARTUP_FILE).s
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/%.o: ../../crypto/%.c
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.o
|
||||
# hack
|
||||
$(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^
|
||||
$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin
|
||||
SETLEN=1 ../../crypto/sign.py obj/code.bin $@ $(CERT)
|
||||
|
||||
obj/bootstub.bin: obj/$(STARTUP_FILE).o obj/bootstub.o obj/sha.o obj/rsa.o
|
||||
$(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^
|
||||
$(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@
|
||||
|
||||
clean:
|
||||
rm -f obj/*
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
cd ..
|
||||
PEDAL=1 scons -u
|
||||
cd pedal
|
||||
|
||||
../../tests/pedal/enter_canloader.py ../obj/pedal.bin.signed
|
|
@ -0,0 +1,11 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
DFU_UTIL="dfu-util"
|
||||
|
||||
cd ..
|
||||
PEDAL=1 scons -u
|
||||
cd pedal
|
||||
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/pedal.bin.signed
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D ../obj/bootstub.pedal.bin
|
|
@ -0,0 +1,11 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
DFU_UTIL="dfu-util"
|
||||
|
||||
scons -u
|
||||
|
||||
PYTHONPATH=.. python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true
|
||||
sleep 1
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D obj/panda.bin.signed
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.panda.bin
|
|
@ -41,6 +41,7 @@
|
|||
#define SAFETY_HYUNDAI_COMMUNITY 24U
|
||||
|
||||
uint16_t current_safety_mode = SAFETY_SILENT;
|
||||
int16_t current_safety_param = 0;
|
||||
const safety_hooks *current_hooks = &nooutput_hooks;
|
||||
|
||||
int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push){
|
||||
|
@ -291,7 +292,8 @@ int set_safety_hooks(uint16_t mode, int16_t param) {
|
|||
for (int i = 0; i < hook_config_count; i++) {
|
||||
if (safety_hook_registry[i].id == mode) {
|
||||
current_hooks = safety_hook_registry[i].hooks;
|
||||
current_safety_mode = safety_hook_registry[i].id;
|
||||
current_safety_mode = mode;
|
||||
current_safety_param = param;
|
||||
set_status = 0; // set
|
||||
}
|
||||
|
||||
|
|
|
@ -9,7 +9,7 @@ const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
|
|||
|
||||
// Safety-relevant CAN messages for the Volkswagen MQB platform
|
||||
#define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds
|
||||
#define MSG_EPS_01 0x09F // RX from EPS, for driver steering torque
|
||||
#define MSG_LH_EPS_03 0x09F // RX from EPS, for driver steering torque
|
||||
#define MSG_ESP_05 0x106 // RX from ABS, for brake switch state
|
||||
#define MSG_TSK_06 0x120 // RX from ECU, for ACC status from drivetrain coordinator
|
||||
#define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input
|
||||
|
@ -23,7 +23,7 @@ const int VOLKSWAGEN_MQB_TX_MSGS_LEN = sizeof(VOLKSWAGEN_MQB_TX_MSGS) / sizeof(V
|
|||
|
||||
AddrCheckStruct volkswagen_mqb_rx_checks[] = {
|
||||
{.msg = {{MSG_ESP_19, 0, 8, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}}},
|
||||
{.msg = {{MSG_EPS_01, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}}},
|
||||
{.msg = {{MSG_LH_EPS_03, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}}},
|
||||
{.msg = {{MSG_ESP_05, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}}},
|
||||
{.msg = {{MSG_TSK_06, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}}},
|
||||
{.msg = {{MSG_MOTOR_20, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}}},
|
||||
|
@ -86,7 +86,7 @@ static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
|
||||
uint8_t counter = volkswagen_mqb_get_counter(to_push);
|
||||
switch(addr) {
|
||||
case MSG_EPS_01:
|
||||
case MSG_LH_EPS_03:
|
||||
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
|
||||
break;
|
||||
case MSG_ESP_05:
|
||||
|
@ -156,9 +156,9 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
}
|
||||
|
||||
// Update driver input torque samples
|
||||
// Signal: EPS_01.Driver_Strain (absolute torque)
|
||||
// Signal: EPS_01.Driver_Strain_VZ (direction)
|
||||
if (addr == MSG_EPS_01) {
|
||||
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
|
||||
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
|
||||
if (addr == MSG_LH_EPS_03) {
|
||||
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
|
||||
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
|
||||
if (sign == 1) {
|
||||
|
|
|
@ -1,20 +0,0 @@
|
|||
ifeq ($(RELEASE),1)
|
||||
BUILD_TYPE = "RELEASE"
|
||||
else
|
||||
BUILD_TYPE = "DEBUG"
|
||||
endif
|
||||
|
||||
SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
|
||||
ifneq ($(wildcard $(SELF_DIR)/../.git/HEAD),)
|
||||
obj/gitversion.h: $(SELF_DIR)/../VERSION $(SELF_DIR)/../.git/HEAD $(SELF_DIR)/../.git/index
|
||||
echo "const uint8_t gitversion[] = \"$(shell cat $(SELF_DIR)/../VERSION)-$(BUILDER)-$(shell git rev-parse --short=8 HEAD)-$(BUILD_TYPE)\";" > $@
|
||||
else
|
||||
ifneq ($(wildcard $(SELF_DIR)/../../.git/modules/panda/HEAD),)
|
||||
obj/gitversion.h: $(SELF_DIR)/../VERSION $(SELF_DIR)/../../.git/modules/panda/HEAD $(SELF_DIR)/../../.git/modules/panda/index
|
||||
echo "const uint8_t gitversion[] = \"$(shell cat $(SELF_DIR)/../VERSION)-$(BUILDER)-$(shell git rev-parse --short=8 HEAD)-$(BUILD_TYPE)\";" > $@
|
||||
else
|
||||
obj/gitversion.h: $(SELF_DIR)/../VERSION
|
||||
echo "const uint8_t gitversion[] = \"$(shell cat $(SELF_DIR)/../VERSION)-$(BUILDER)-unknown-$(BUILD_TYPE)\";" > $@
|
||||
endif
|
||||
endif
|
|
@ -1,43 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import sys
|
||||
from Crypto.PublicKey import RSA
|
||||
|
||||
def egcd(a, b):
|
||||
if a == 0:
|
||||
return (b, 0, 1)
|
||||
else:
|
||||
g, y, x = egcd(b % a, a)
|
||||
return (g, x - (b // a) * y, y)
|
||||
|
||||
def modinv(a, m):
|
||||
g, x, _ = egcd(a, m)
|
||||
if g != 1:
|
||||
raise Exception('modular inverse does not exist')
|
||||
else:
|
||||
return x % m
|
||||
|
||||
def to_c_string(x):
|
||||
mod = (hex(x)[2:-1].rjust(0x100, '0'))
|
||||
hh = ''.join('\\x' + mod[i:i + 2] for i in range(0, 0x100, 2))
|
||||
return hh
|
||||
|
||||
def to_c_uint32(x):
|
||||
nums = []
|
||||
for _ in range(0x20):
|
||||
nums.append(x % (2**32))
|
||||
x //= (2**32)
|
||||
return "{" + 'U,'.join(map(str, nums)) + "U}"
|
||||
|
||||
for fn in sys.argv[1:]:
|
||||
rsa = RSA.importKey(open(fn).read())
|
||||
rr = pow(2**1024, 2, rsa.n)
|
||||
n0inv = 2**32 - modinv(rsa.n, 2**32)
|
||||
|
||||
cname = fn.split("/")[-1].split(".")[0] + "_rsa_key"
|
||||
|
||||
print('RSAPublicKey ' + cname + ' = {.len = 0x20,')
|
||||
print(' .n0inv = %dU,' % n0inv)
|
||||
print(' .n = %s,' % to_c_uint32(rsa.n))
|
||||
print(' .rr = %s,' % to_c_uint32(rr))
|
||||
print(' .exponent = %d,' % rsa.e)
|
||||
print('};')
|
|
@ -7,7 +7,6 @@ import usb1
|
|||
import os
|
||||
import time
|
||||
import traceback
|
||||
import subprocess
|
||||
import sys
|
||||
from .dfu import PandaDFU # pylint: disable=import-error
|
||||
from .flash_release import flash_release # noqa pylint: disable=import-error
|
||||
|
@ -19,17 +18,10 @@ from .isotp import isotp_send, isotp_recv # pylint: disable=import-error
|
|||
__version__ = '0.0.9'
|
||||
|
||||
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
|
||||
DEFAULT_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
|
||||
|
||||
DEBUG = os.getenv("PANDADEBUG") is not None
|
||||
|
||||
# *** wifi mode ***
|
||||
def build_st(target, mkfile="Makefile", clean=True):
|
||||
from panda import BASEDIR
|
||||
|
||||
clean_cmd = "make -f %s clean" % mkfile if clean else ":"
|
||||
cmd = 'cd %s && %s && make -f %s %s' % (os.path.join(BASEDIR, "board"), clean_cmd, mkfile, target)
|
||||
_ = subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True)
|
||||
|
||||
def parse_can_buffer(dat):
|
||||
ret = []
|
||||
for j in range(0, len(dat), 0x10):
|
||||
|
@ -184,7 +176,6 @@ class Panda(object):
|
|||
if self._serial is None or this_serial == self._serial:
|
||||
self._serial = this_serial
|
||||
print("opening device", self._serial, hex(device.getProductID()))
|
||||
time.sleep(1)
|
||||
self.bootstub = device.getProductID() == 0xddee
|
||||
self.legacy = (device.getbcdDevice() != 0x2300)
|
||||
self._handle = device.open()
|
||||
|
@ -267,23 +258,12 @@ class Panda(object):
|
|||
except Exception:
|
||||
pass
|
||||
|
||||
def flash(self, fn=None, code=None, reconnect=True):
|
||||
def flash(self, fn=DEFAULT_FW_FN, code=None, reconnect=True):
|
||||
print("flash: main version is " + self.get_version())
|
||||
if not self.bootstub:
|
||||
self.reset(enter_bootstub=True)
|
||||
assert(self.bootstub)
|
||||
|
||||
if fn is None and code is None:
|
||||
if self.legacy:
|
||||
fn = "obj/comma.bin"
|
||||
print("building legacy st code")
|
||||
build_st(fn, "Makefile.legacy")
|
||||
else:
|
||||
fn = "obj/panda.bin"
|
||||
print("building panda st code")
|
||||
build_st(fn)
|
||||
fn = os.path.join(BASEDIR, "board", fn)
|
||||
|
||||
if code is None:
|
||||
with open(fn, "rb") as f:
|
||||
code = f.read()
|
||||
|
|
|
@ -92,16 +92,8 @@ class PandaDFU(object):
|
|||
self.reset()
|
||||
|
||||
def recover(self):
|
||||
from panda import BASEDIR, build_st
|
||||
if self.legacy:
|
||||
fn = "obj/bootstub.comma.bin"
|
||||
print("building legacy bootstub")
|
||||
build_st(fn, "Makefile.legacy")
|
||||
else:
|
||||
fn = "obj/bootstub.panda.bin"
|
||||
print("building panda bootstub")
|
||||
build_st(fn)
|
||||
fn = os.path.join(BASEDIR, "board", fn)
|
||||
from panda import BASEDIR
|
||||
fn = os.path.join(BASEDIR, "board", "obj", "bootstub.panda.bin")
|
||||
|
||||
with open(fn, "rb") as f:
|
||||
code = f.read()
|
||||
|
|
|
@ -486,13 +486,14 @@ class IsoTpMessage():
|
|||
|
||||
FUNCTIONAL_ADDRS = [0x7DF, 0x18DB33F1]
|
||||
|
||||
def get_rx_addr_for_tx_addr(tx_addr):
|
||||
def get_rx_addr_for_tx_addr(tx_addr, rx_offset=0x8):
|
||||
if tx_addr in FUNCTIONAL_ADDRS:
|
||||
return None
|
||||
|
||||
if tx_addr < 0xFFF8:
|
||||
# standard 11 bit response addr (add 8)
|
||||
return tx_addr + 8
|
||||
# pseudo-standard 11 bit response addr (add 8) works for most manufacturers
|
||||
# allow override; some manufacturers use other offsets for non-OBD2 access
|
||||
return tx_addr + rx_offset
|
||||
|
||||
if tx_addr > 0x10000000 and tx_addr < 0xFFFFFFFF:
|
||||
# standard 29 bit response addr (flip last two bytes)
|
||||
|
|
|
@ -1,4 +1,5 @@
|
|||
import os
|
||||
import logging
|
||||
from bisect import bisect_right
|
||||
|
||||
import numpy as np
|
||||
|
@ -157,7 +158,7 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
|
|||
|
||||
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):
|
||||
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], 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
|
||||
|
@ -166,6 +167,8 @@ class EKF_sym():
|
|||
self.dim_main = dim_main
|
||||
self.dim_main_err = dim_main_err
|
||||
|
||||
self.logger = logger
|
||||
|
||||
# state
|
||||
x_initial = x_initial.reshape((-1, 1))
|
||||
self.dim_x = x_initial.shape[0]
|
||||
|
@ -381,7 +384,7 @@ class EKF_sym():
|
|||
# rewind
|
||||
if self.filter_time is not None and t < self.filter_time:
|
||||
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - self.max_rewind_age:
|
||||
print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
|
||||
self.logger.error("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
|
||||
return None
|
||||
rewound = self.rewind(t)
|
||||
else:
|
||||
|
@ -502,7 +505,7 @@ class EKF_sym():
|
|||
|
||||
# TODO If nullspace isn't the dimension we want
|
||||
if A.shape[1] + He.shape[1] != A.shape[0]:
|
||||
print('Warning: null space projection failed, measurement ignored')
|
||||
self.logger.warning('Warning: null space projection failed, measurement ignored')
|
||||
return x, P, np.zeros(A.shape[0] - He.shape[1])
|
||||
|
||||
# if using eskf
|
||||
|
|
|
@ -41,13 +41,9 @@ echo "#define COMMA_VERSION \"$VERSION-release\"" > selfdrive/common/version.h
|
|||
git commit -m "openpilot v$VERSION"
|
||||
|
||||
# Build signed panda firmware
|
||||
pushd panda/board/
|
||||
cp -r /tmp/pandaextra /data/openpilot/
|
||||
RELEASE=1 make obj/panda.bin
|
||||
mv obj/panda.bin /tmp/panda.bin
|
||||
make clean
|
||||
mv /tmp/panda.bin obj/panda.bin.signed
|
||||
rm -rf /data/openpilot/pandaextra
|
||||
pushd panda/
|
||||
CERT=/tmp/pandaextra/certs/release RELEASE=1 scons -u .
|
||||
mv board/obj/panda.bin.signed /tmp/panda.bin.signed
|
||||
popd
|
||||
|
||||
# Build stuff
|
||||
|
@ -56,7 +52,7 @@ export PYTHONPATH="/data/openpilot:/data/openpilot/pyextra"
|
|||
SCONS_CACHE=1 scons -j3
|
||||
|
||||
# Run tests
|
||||
python selfdrive/test/test_manager.py
|
||||
python selfdrive/manager/test/test_manager.py
|
||||
selfdrive/car/tests/test_car_interfaces.py
|
||||
|
||||
# Cleanup
|
||||
|
@ -65,7 +61,13 @@ find . -name '*.o' -delete
|
|||
find . -name '*.os' -delete
|
||||
find . -name '*.pyc' -delete
|
||||
find . -name '__pycache__' -delete
|
||||
rm -rf panda/board panda/certs panda/crypto
|
||||
rm -rf .sconsign.dblite Jenkinsfile release/
|
||||
rm models/supercombo.dlc
|
||||
|
||||
# Move back signed panda fw
|
||||
mkdir -p panda/board/obj
|
||||
mv /tmp/panda.bin.signed panda/board/obj/panda.bin.signed
|
||||
|
||||
# Restore phonelibs
|
||||
git checkout phonelibs/
|
||||
|
|
|
@ -5,12 +5,7 @@
|
|||
<title>openpilot Terms of Service</title>
|
||||
<style type="text/css">
|
||||
body {
|
||||
color: #ffffff;
|
||||
font-size: 50px;
|
||||
font-family: 'Inter', sans-serif;
|
||||
line-height: 1.7;
|
||||
background: #000000;
|
||||
touch-action: pan-y;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
|
|
Before Width: | Height: | Size: 183 KiB |
After Width: | Height: | Size: 1.5 MiB |
Before Width: | Height: | Size: 246 KiB |
After Width: | Height: | Size: 47 KiB |
Before Width: | Height: | Size: 257 KiB |
After Width: | Height: | Size: 50 KiB |
Before Width: | Height: | Size: 216 KiB |
After Width: | Height: | Size: 743 KiB |
Before Width: | Height: | Size: 290 KiB |
After Width: | Height: | Size: 33 KiB |
Before Width: | Height: | Size: 368 KiB |
After Width: | Height: | Size: 1.1 MiB |
Before Width: | Height: | Size: 229 KiB |
After Width: | Height: | Size: 708 KiB |
After Width: | Height: | Size: 33 KiB |
After Width: | Height: | Size: 39 KiB |
After Width: | Height: | Size: 1.9 MiB |
Before Width: | Height: | Size: 194 KiB |
After Width: | Height: | Size: 33 KiB |
Before Width: | Height: | Size: 253 KiB |
After Width: | Height: | Size: 556 KiB |
Before Width: | Height: | Size: 263 KiB |
After Width: | Height: | Size: 524 KiB |
Before Width: | Height: | Size: 174 KiB |
After Width: | Height: | Size: 39 KiB |
Before Width: | Height: | Size: 252 KiB |
After Width: | Height: | Size: 1.0 MiB |
Before Width: | Height: | Size: 288 KiB |
After Width: | Height: | Size: 718 KiB |
Before Width: | Height: | Size: 211 KiB |
After Width: | Height: | Size: 645 KiB |
Before Width: | Height: | Size: 196 KiB |
After Width: | Height: | Size: 688 KiB |
|
@ -4,6 +4,7 @@ import hashlib
|
|||
import io
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import queue
|
||||
import random
|
||||
import select
|
||||
|
@ -24,18 +25,24 @@ from common.api import Api
|
|||
from common.basedir import PERSIST
|
||||
from common.params import Params
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.hardware import HARDWARE
|
||||
from selfdrive.hardware import HARDWARE, PC
|
||||
from selfdrive.loggerd.config import ROOT
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
|
||||
from selfdrive.swaglog import cloudlog, SWAGLOG_DIR
|
||||
|
||||
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
|
||||
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
|
||||
LOCAL_PORT_WHITELIST = set([8022])
|
||||
|
||||
LOG_ATTR_NAME = 'user.upload'
|
||||
LOG_ATTR_VALUE_MAX_UNIX_TIME = int.to_bytes(2147483647, 4, sys.byteorder)
|
||||
|
||||
dispatcher["echo"] = lambda s: s
|
||||
payload_queue: Any = queue.Queue()
|
||||
response_queue: Any = queue.Queue()
|
||||
recv_queue: Any = queue.Queue()
|
||||
send_queue: Any = queue.Queue()
|
||||
upload_queue: Any = queue.Queue()
|
||||
log_send_queue: Any = queue.Queue()
|
||||
log_recv_queue: Any = queue.Queue()
|
||||
cancelled_uploads: Any = set()
|
||||
UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id'])
|
||||
|
||||
|
@ -46,7 +53,8 @@ def handle_long_poll(ws):
|
|||
threads = [
|
||||
threading.Thread(target=ws_recv, args=(ws, end_event)),
|
||||
threading.Thread(target=ws_send, args=(ws, end_event)),
|
||||
threading.Thread(target=upload_handler, args=(end_event,))
|
||||
threading.Thread(target=upload_handler, args=(end_event,)),
|
||||
threading.Thread(target=log_handler, args=(end_event,)),
|
||||
] + [
|
||||
threading.Thread(target=jsonrpc_handler, args=(end_event,))
|
||||
for x in range(HANDLER_THREADS)
|
||||
|
@ -64,19 +72,23 @@ def handle_long_poll(ws):
|
|||
for thread in threads:
|
||||
thread.join()
|
||||
|
||||
|
||||
def jsonrpc_handler(end_event):
|
||||
dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event)
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
data = payload_queue.get(timeout=1)
|
||||
response = JSONRPCResponseManager.handle(data, dispatcher)
|
||||
response_queue.put_nowait(response)
|
||||
data = recv_queue.get(timeout=1)
|
||||
if "method" in data:
|
||||
response = JSONRPCResponseManager.handle(data, dispatcher)
|
||||
send_queue.put_nowait(response.json)
|
||||
elif "result" in data and "id" in data:
|
||||
log_recv_queue.put_nowait(data)
|
||||
else:
|
||||
raise Exception("not a valid request or response")
|
||||
except queue.Empty:
|
||||
pass
|
||||
except Exception as e:
|
||||
cloudlog.exception("athena jsonrpc handler failed")
|
||||
response_queue.put_nowait(json.dumps({"error": str(e)}))
|
||||
send_queue.put_nowait(json.dumps({"error": str(e)}))
|
||||
|
||||
|
||||
def upload_handler(end_event):
|
||||
|
@ -244,6 +256,82 @@ def takeSnapshot():
|
|||
raise Exception("not available while camerad is started")
|
||||
|
||||
|
||||
def get_logs_to_send_sorted():
|
||||
# TODO: scan once then use inotify to detect file creation/deletion
|
||||
curr_time = int(time.time())
|
||||
logs = []
|
||||
for log_entry in os.listdir(SWAGLOG_DIR):
|
||||
log_path = os.path.join(SWAGLOG_DIR, log_entry)
|
||||
try:
|
||||
time_sent = int.from_bytes(getxattr(log_path, LOG_ATTR_NAME), sys.byteorder)
|
||||
except (ValueError, TypeError):
|
||||
time_sent = 0
|
||||
# assume send failed and we lost the response if sent more than one hour ago
|
||||
if not time_sent or curr_time - time_sent > 3600:
|
||||
logs.append(log_entry)
|
||||
# return logs in order they should be sent
|
||||
# excluding most recent (active) log file
|
||||
return sorted(logs[:-1])
|
||||
|
||||
|
||||
def log_handler(end_event):
|
||||
if PC:
|
||||
return
|
||||
|
||||
log_files = []
|
||||
last_scan = 0
|
||||
log_retries = 0
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
try:
|
||||
result = json.loads(log_recv_queue.get(timeout=1))
|
||||
log_success = result.get("success")
|
||||
log_entry = result.get("id")
|
||||
log_path = os.path.join(SWAGLOG_DIR, log_entry)
|
||||
if log_entry and log_success:
|
||||
try:
|
||||
setxattr(log_path, LOG_ATTR_NAME, LOG_ATTR_VALUE_MAX_UNIX_TIME)
|
||||
except OSError:
|
||||
pass # file could be deleted by log rotation
|
||||
except queue.Empty:
|
||||
pass
|
||||
|
||||
curr_scan = sec_since_boot()
|
||||
if curr_scan - last_scan > 10:
|
||||
log_files = get_logs_to_send_sorted()
|
||||
last_scan = curr_scan
|
||||
|
||||
# never send last log file because it is the active log
|
||||
# and only send one log file at a time (most recent first)
|
||||
if not len(log_files) or not log_send_queue.empty():
|
||||
continue
|
||||
|
||||
log_entry = log_files.pop()
|
||||
try:
|
||||
curr_time = int(time.time())
|
||||
log_path = os.path.join(SWAGLOG_DIR, log_entry)
|
||||
setxattr(log_path, LOG_ATTR_NAME, int.to_bytes(curr_time, 4, sys.byteorder))
|
||||
with open(log_path, "r") as f:
|
||||
jsonrpc = {
|
||||
"method": "forwardLogs",
|
||||
"params": {
|
||||
"logs": f.read()
|
||||
},
|
||||
"jsonrpc": "2.0",
|
||||
"id": log_entry
|
||||
}
|
||||
log_send_queue.put_nowait(json.dumps(jsonrpc))
|
||||
except OSError:
|
||||
pass # file could be deleted by log rotation
|
||||
log_retries = 0
|
||||
except Exception:
|
||||
cloudlog.exception("athena.log_handler.exception")
|
||||
log_retries += 1
|
||||
|
||||
if log_retries != 0:
|
||||
time.sleep(backoff(log_retries))
|
||||
|
||||
|
||||
def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event):
|
||||
while not (end_event.is_set() or global_end_event.is_set()):
|
||||
try:
|
||||
|
@ -290,7 +378,7 @@ def ws_recv(ws, end_event):
|
|||
if opcode in (ABNF.OPCODE_TEXT, ABNF.OPCODE_BINARY):
|
||||
if opcode == ABNF.OPCODE_TEXT:
|
||||
data = data.decode("utf-8")
|
||||
payload_queue.put_nowait(data)
|
||||
recv_queue.put_nowait(data)
|
||||
elif opcode == ABNF.OPCODE_PING:
|
||||
Params().put("LastAthenaPingTime", str(int(sec_since_boot() * 1e9)))
|
||||
except WebSocketTimeoutException:
|
||||
|
@ -303,8 +391,11 @@ def ws_recv(ws, end_event):
|
|||
def ws_send(ws, end_event):
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
response = response_queue.get(timeout=1)
|
||||
ws.send(response.json)
|
||||
try:
|
||||
data = send_queue.get_nowait()
|
||||
except queue.Empty:
|
||||
data = log_send_queue.get(timeout=1)
|
||||
ws.send(data)
|
||||
except queue.Empty:
|
||||
pass
|
||||
except Exception:
|
||||
|
|
|
@ -5,12 +5,13 @@ from multiprocessing import Process
|
|||
|
||||
import selfdrive.crash as crash
|
||||
from common.params import Params
|
||||
from selfdrive.launcher import launcher
|
||||
from selfdrive.manager.process import launcher
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.version import version, dirty
|
||||
|
||||
ATHENA_MGR_PID_PARAM = "AthenadPid"
|
||||
|
||||
|
||||
def main():
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId").decode('utf-8')
|
||||
|
@ -32,5 +33,6 @@ def main():
|
|||
finally:
|
||||
params.delete(ATHENA_MGR_PID_PARAM)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
|
@ -15,6 +15,7 @@
|
|||
#include <bitset>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <unordered_map>
|
||||
|
||||
#include <libusb-1.0/libusb.h>
|
||||
|
||||
|
@ -25,6 +26,7 @@
|
|||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "messaging.hpp"
|
||||
#include "locationd/ublox_msg.h"
|
||||
|
||||
#include "panda.h"
|
||||
#include "pigeon.h"
|
||||
|
@ -97,11 +99,8 @@ void safety_setter_thread() {
|
|||
}
|
||||
LOGW("got %d bytes CarParams", params.size());
|
||||
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((params.size() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), params.data(), params.size());
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
AlignedBuffer aligned_buf;
|
||||
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size()));
|
||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
|
||||
cereal::CarParams::SafetyModel safety_model = car_params.getSafetyModel();
|
||||
|
||||
|
@ -199,6 +198,7 @@ void can_recv(PubMaster &pm) {
|
|||
void can_send_thread(bool fake_send) {
|
||||
LOGD("start send thread");
|
||||
|
||||
AlignedBuffer aligned_buf;
|
||||
Context * context = Context::create();
|
||||
SubSocket * subscriber = SubSocket::create(context, "sendcan");
|
||||
assert(subscriber != NULL);
|
||||
|
@ -215,10 +215,7 @@ void can_send_thread(bool fake_send) {
|
|||
continue;
|
||||
}
|
||||
|
||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg));
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
//Dont send if older than 1 second
|
||||
|
@ -358,7 +355,7 @@ void panda_state_thread(bool spoofing_started) {
|
|||
ps.setIgnitionCan(pandaState.ignition_can);
|
||||
ps.setControlsAllowed(pandaState.controls_allowed);
|
||||
ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
|
||||
ps.setHasGps(panda->is_pigeon);
|
||||
ps.setHasGps(true);
|
||||
ps.setCanRxErrs(pandaState.can_rx_errs);
|
||||
ps.setCanSendErrs(pandaState.can_send_errs);
|
||||
ps.setCanFwdErrs(pandaState.can_fwd_errs);
|
||||
|
@ -366,6 +363,7 @@ void panda_state_thread(bool spoofing_started) {
|
|||
ps.setPandaType(panda->hw_type);
|
||||
ps.setUsbPowerMode(cereal::PandaState::UsbPowerMode(pandaState.usb_power_mode));
|
||||
ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
|
||||
ps.setSafetyParam(pandaState.safety_param);
|
||||
ps.setFanSpeedRpm(fan_speed_rpm);
|
||||
ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
|
||||
ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
|
||||
|
@ -467,36 +465,70 @@ static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) {
|
|||
}
|
||||
|
||||
void pigeon_thread() {
|
||||
if (!panda->is_pigeon) { return; };
|
||||
|
||||
// ubloxRaw = 8042
|
||||
PubMaster pm({"ubloxRaw"});
|
||||
bool ignition_last = false;
|
||||
|
||||
#ifdef QCOM2
|
||||
Pigeon * pigeon = Pigeon::connect("/dev/ttyHS0");
|
||||
Pigeon *pigeon = Pigeon::connect("/dev/ttyHS0");
|
||||
#else
|
||||
Pigeon * pigeon = Pigeon::connect(panda);
|
||||
Pigeon *pigeon = Pigeon::connect(panda);
|
||||
#endif
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
while (!do_exit && panda->connected) {
|
||||
bool need_reset = false;
|
||||
std::string recv = pigeon->receive();
|
||||
if (recv.length() > 0) {
|
||||
if (recv[0] == (char)0x00){
|
||||
if (ignition) {
|
||||
LOGW("received invalid ublox message while onroad, resetting panda GPS");
|
||||
need_reset = true;
|
||||
|
||||
// Parse message header
|
||||
if (ignition && recv.length() >= 3) {
|
||||
if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2){
|
||||
const char msg_cls = recv[2];
|
||||
uint64_t t = nanos_since_boot();
|
||||
if (t > last_recv_time[msg_cls]){
|
||||
last_recv_time[msg_cls] = t;
|
||||
}
|
||||
} else {
|
||||
pigeon_publish_raw(pm, recv);
|
||||
}
|
||||
}
|
||||
|
||||
// Check based on message frequency
|
||||
for (const auto& [msg_cls, max_dt] : cls_max_dt) {
|
||||
int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls];
|
||||
if (ignition_last && ignition && dt > max_dt) {
|
||||
LOG("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt);
|
||||
// TODO: turn on reset after verification of logs
|
||||
// need_reset = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Check based on null bytes
|
||||
if (ignition && recv.length() > 0 && recv[0] == (char)0x00){
|
||||
need_reset = true;
|
||||
LOGW("received invalid ublox message while onroad, resetting panda GPS");
|
||||
}
|
||||
|
||||
if (recv.length() > 0){
|
||||
pigeon_publish_raw(pm, recv);
|
||||
}
|
||||
|
||||
// init pigeon on rising ignition edge
|
||||
// since it was turned off in low power mode
|
||||
if((ignition && !ignition_last) || need_reset) {
|
||||
pigeon->init();
|
||||
|
||||
// Set receive times to current time
|
||||
uint64_t t = nanos_since_boot() + 10000000000ULL; // Give ublox 10 seconds to start
|
||||
for (const auto& [msg_cls, dt] : cls_max_dt) {
|
||||
last_recv_time[msg_cls] = t;
|
||||
}
|
||||
} else if (!ignition && ignition_last) {
|
||||
// power off on falling edge of ignition
|
||||
LOGD("powering off pigeon\n");
|
||||
pigeon->set_power(false);
|
||||
}
|
||||
|
||||
ignition_last = ignition;
|
||||
|
|
|
@ -52,13 +52,12 @@ Panda::Panda(){
|
|||
if (err != 0) { goto fail; }
|
||||
|
||||
hw_type = get_hw_type();
|
||||
is_pigeon =
|
||||
(hw_type == cereal::PandaState::PandaType::GREY_PANDA) ||
|
||||
(hw_type == cereal::PandaState::PandaType::BLACK_PANDA) ||
|
||||
(hw_type == cereal::PandaState::PandaType::UNO) ||
|
||||
(hw_type == cereal::PandaState::PandaType::DOS);
|
||||
|
||||
assert((hw_type != cereal::PandaState::PandaType::WHITE_PANDA) &&
|
||||
(hw_type != cereal::PandaState::PandaType::GREY_PANDA));
|
||||
|
||||
has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) ||
|
||||
(hw_type == cereal::PandaState::PandaType::DOS);
|
||||
(hw_type == cereal::PandaState::PandaType::DOS);
|
||||
|
||||
return;
|
||||
|
||||
|
|
|
@ -33,6 +33,7 @@ struct __attribute__((packed)) health_t {
|
|||
uint8_t car_harness_status;
|
||||
uint8_t usb_power_mode;
|
||||
uint8_t safety_model;
|
||||
int16_t safety_param;
|
||||
uint8_t fault_status;
|
||||
uint8_t power_save_enabled;
|
||||
};
|
||||
|
@ -54,7 +55,6 @@ class Panda {
|
|||
|
||||
std::atomic<bool> connected = true;
|
||||
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
|
||||
bool is_pigeon = false;
|
||||
bool has_rtc = false;
|
||||
|
||||
// HW communication
|
||||
|
|
|
@ -133,7 +133,6 @@ void TTYPigeon::connect(const char * tty) {
|
|||
if (pigeon_tty_fd < 0){
|
||||
handle_tty_issue(errno, __func__);
|
||||
assert(pigeon_tty_fd >= 0);
|
||||
|
||||
}
|
||||
int err = tcgetattr(pigeon_tty_fd, &pigeon_tty);
|
||||
assert(err == 0);
|
||||
|
|
|
@ -35,3 +35,10 @@ env.Program('camerad', [
|
|||
'imgproc/utils.cc',
|
||||
cameras,
|
||||
], LIBS=libs)
|
||||
|
||||
if GetOption("test"):
|
||||
env.Program('test/ae_gray_test', [
|
||||
'test/ae_gray_test.cc',
|
||||
'cameras/camera_common.cc',
|
||||
'transforms/rgb_to_yuv.cc',
|
||||
], LIBS=libs)
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
#include "modeldata.h"
|
||||
#include "imgproc/utils.h"
|
||||
|
||||
static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b, const CameraState *s) {
|
||||
|
@ -69,16 +70,8 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
|
|||
rgb_width = ci->frame_width / 2;
|
||||
rgb_height = ci->frame_height / 2;
|
||||
}
|
||||
float db_s = 0.5;
|
||||
#else
|
||||
float db_s = 1.0;
|
||||
#endif
|
||||
const mat3 transform = (mat3){{
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0
|
||||
}};
|
||||
yuv_transform = ci->bayer ? transform_scale_buffer(transform, db_s) : transform;
|
||||
yuv_transform = get_model_yuv_transform(ci->bayer);
|
||||
|
||||
vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height);
|
||||
rgb_stride = vipc_server->get_buffer(rgb_type)->stride;
|
||||
|
@ -106,25 +99,21 @@ CameraBuf::~CameraBuf() {
|
|||
camera_bufs[i].free();
|
||||
}
|
||||
|
||||
rgb_to_yuv_destroy(&rgb_to_yuv_state);
|
||||
|
||||
if (krnl_debayer) {
|
||||
CL_CHECK(clReleaseKernel(krnl_debayer));
|
||||
}
|
||||
CL_CHECK(clReleaseCommandQueue(q));
|
||||
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));
|
||||
}
|
||||
|
||||
bool CameraBuf::acquire() {
|
||||
if (!safe_queue.try_pop(cur_buf_idx, 1)) return false;
|
||||
|
||||
const FrameMetadata &frame_data = camera_bufs_metadata[cur_buf_idx];
|
||||
if (frame_data.frame_id == -1) {
|
||||
if (camera_bufs_metadata[cur_buf_idx].frame_id == -1) {
|
||||
LOGE("no frame data? wtf");
|
||||
release();
|
||||
return false;
|
||||
}
|
||||
|
||||
cur_frame_data = frame_data;
|
||||
cur_frame_data = camera_bufs_metadata[cur_buf_idx];
|
||||
|
||||
cur_rgb_buf = vipc_server->get_buffer(rgb_type);
|
||||
|
||||
|
@ -162,13 +151,12 @@ bool CameraBuf::acquire() {
|
|||
CL_CHECK(clReleaseEvent(debayer_event));
|
||||
|
||||
cur_yuv_buf = vipc_server->get_buffer(yuv_type);
|
||||
yuv_metas[cur_yuv_buf->idx] = frame_data;
|
||||
rgb_to_yuv_queue(&rgb_to_yuv_state, q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
|
||||
|
||||
VisionIpcBufExtra extra = {
|
||||
frame_data.frame_id,
|
||||
frame_data.timestamp_sof,
|
||||
frame_data.timestamp_eof,
|
||||
cur_frame_data.frame_id,
|
||||
cur_frame_data.timestamp_sof,
|
||||
cur_frame_data.timestamp_eof,
|
||||
};
|
||||
vipc_server->send(cur_rgb_buf, &extra);
|
||||
vipc_server->send(cur_yuv_buf, &extra);
|
||||
|
@ -290,19 +278,16 @@ static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) {
|
|||
free(thumbnail_buffer);
|
||||
}
|
||||
|
||||
void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) {
|
||||
const CameraBuf *b = &c->buf;
|
||||
|
||||
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted) {
|
||||
const uint8_t *pix_ptr = b->cur_yuv_buf->y;
|
||||
uint32_t lum_binning[256] = {0};
|
||||
unsigned int lum_total = 0;
|
||||
for (int y = y_start; y < y_end; y += y_skip) {
|
||||
for (int x = x_start; x < x_end; x += x_skip) {
|
||||
uint8_t lum = pix_ptr[(y * b->rgb_width) + x];
|
||||
#ifdef QCOM2
|
||||
if (lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) {
|
||||
if (hist_ceil && lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) {
|
||||
continue;
|
||||
}
|
||||
#endif
|
||||
lum_binning[lum]++;
|
||||
lum_total += 1;
|
||||
}
|
||||
|
@ -313,20 +298,21 @@ void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, in
|
|||
int lum_med_alt = 0;
|
||||
for (lum_med=255; lum_med>=0; lum_med--) {
|
||||
lum_cur += lum_binning[lum_med];
|
||||
#ifdef QCOM2
|
||||
int lum_med_tmp = 0;
|
||||
int hb = HLC_THRESH;
|
||||
if (lum_cur > 0 && lum_med > hb) {
|
||||
lum_med_tmp = 4 * (lum_med - hb) + 100;
|
||||
if (hl_weighted) {
|
||||
int lum_med_tmp = 0;
|
||||
int hb = HLC_THRESH + (10 - analog_gain);
|
||||
if (lum_cur > 0 && lum_med > hb) {
|
||||
lum_med_tmp = (lum_med - hb) + 100;
|
||||
}
|
||||
lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp;
|
||||
}
|
||||
lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp;
|
||||
#endif
|
||||
if (lum_cur >= lum_total / 2) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*(lum_med_alt - lum_med)/lum_total/2:lum_med;
|
||||
camera_autoexposure(c, lum_med / 256.0);
|
||||
lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*abs(lum_med_alt - lum_med)/lum_total:lum_med;
|
||||
|
||||
return lum_med / 256.0;
|
||||
}
|
||||
|
||||
extern ExitHandler do_exit;
|
||||
|
@ -342,7 +328,8 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
|
|||
}
|
||||
set_thread_name(thread_name);
|
||||
|
||||
for (int cnt = 0; !do_exit; cnt++) {
|
||||
uint32_t cnt = 0;
|
||||
while (!do_exit) {
|
||||
if (!cs->buf.acquire()) continue;
|
||||
|
||||
callback(cameras, cs, cnt);
|
||||
|
@ -352,6 +339,7 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
|
|||
publish_thumbnail(cameras->pm, &(cs->buf));
|
||||
}
|
||||
cs->buf.release();
|
||||
++cnt;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
@ -414,7 +402,11 @@ void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c,
|
|||
#endif
|
||||
}
|
||||
|
||||
set_exposure_target(c, (const uint8_t *)b->cur_yuv_buf->y, x_min, x_max, 2, y_min, y_max, skip);
|
||||
#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
|
||||
}
|
||||
|
||||
MessageBuilder msg;
|
||||
|
|
|
@ -10,7 +10,6 @@
|
|||
#include "common/queue.h"
|
||||
#include "visionbuf.h"
|
||||
#include "common/visionimg.h"
|
||||
#include "imgproc/utils.h"
|
||||
#include "messaging.hpp"
|
||||
#include "transforms/rgb_to_yuv.h"
|
||||
|
||||
|
@ -36,6 +35,10 @@
|
|||
#define LOG_CAMERA_ID_QCAMERA 3
|
||||
#define LOG_CAMERA_ID_MAX 4
|
||||
|
||||
#define HLC_THRESH 222
|
||||
#define HLC_A 80
|
||||
#define HISTO_CEIL_K 5
|
||||
|
||||
const bool env_send_driver = getenv("SEND_DRIVER") != NULL;
|
||||
const bool env_send_road = getenv("SEND_ROAD") != NULL;
|
||||
const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL;
|
||||
|
@ -82,8 +85,6 @@ typedef struct CameraExpInfo {
|
|||
float grey_frac;
|
||||
} CameraExpInfo;
|
||||
|
||||
extern CameraInfo cameras_supported[CAMERA_ID_MAX];
|
||||
|
||||
struct MultiCameraState;
|
||||
struct CameraState;
|
||||
|
||||
|
@ -95,9 +96,8 @@ private:
|
|||
|
||||
RGBToYUVState rgb_to_yuv_state;
|
||||
|
||||
FrameMetadata yuv_metas[YUV_COUNT];
|
||||
VisionStreamType rgb_type, yuv_type;
|
||||
|
||||
|
||||
int cur_buf_idx;
|
||||
|
||||
SafeQueue<int> safe_queue;
|
||||
|
@ -113,7 +113,7 @@ public:
|
|||
std::unique_ptr<VisionBuf[]> camera_bufs;
|
||||
std::unique_ptr<FrameMetadata[]> camera_bufs_metadata;
|
||||
int rgb_width, rgb_height, rgb_stride;
|
||||
|
||||
|
||||
mat3 yuv_transform;
|
||||
|
||||
CameraBuf() = default;
|
||||
|
@ -128,6 +128,12 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt);
|
|||
|
||||
void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data);
|
||||
kj::Array<uint8_t> get_frame_image(const CameraBuf *b);
|
||||
void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
|
||||
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted);
|
||||
std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
|
||||
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt);
|
||||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx);
|
||||
void cameras_open(MultiCameraState *s);
|
||||
void cameras_run(MultiCameraState *s);
|
||||
void cameras_close(MultiCameraState *s);
|
||||
void camera_autoexposure(CameraState *s, float grey_frac);
|
||||
|
|
|
@ -15,6 +15,25 @@ extern ExitHandler do_exit;
|
|||
|
||||
namespace {
|
||||
|
||||
// TODO: make this more generic
|
||||
CameraInfo cameras_supported[CAMERA_ID_MAX] = {
|
||||
[CAMERA_ID_IMX298] = {
|
||||
.frame_width = FRAME_WIDTH,
|
||||
.frame_height = FRAME_HEIGHT,
|
||||
.frame_stride = FRAME_WIDTH*3,
|
||||
.bayer = false,
|
||||
.bayer_flip = false,
|
||||
},
|
||||
[CAMERA_ID_OV8865] = {
|
||||
.frame_width = 1632,
|
||||
.frame_height = 1224,
|
||||
.frame_stride = 2040, // seems right
|
||||
.bayer = false,
|
||||
.bayer_flip = 3,
|
||||
.hdr = false
|
||||
},
|
||||
};
|
||||
|
||||
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));
|
||||
s->ci = cameras_supported[camera_id];
|
||||
|
@ -54,25 +73,6 @@ void run_frame_stream(CameraState &camera, const char* frame_pkt) {
|
|||
|
||||
} // namespace
|
||||
|
||||
// TODO: make this more generic
|
||||
CameraInfo cameras_supported[CAMERA_ID_MAX] = {
|
||||
[CAMERA_ID_IMX298] = {
|
||||
.frame_width = FRAME_WIDTH,
|
||||
.frame_height = FRAME_HEIGHT,
|
||||
.frame_stride = FRAME_WIDTH*3,
|
||||
.bayer = false,
|
||||
.bayer_flip = false,
|
||||
},
|
||||
[CAMERA_ID_OV8865] = {
|
||||
.frame_width = 1632,
|
||||
.frame_height = 1224,
|
||||
.frame_stride = 2040, // seems right
|
||||
.bayer = false,
|
||||
.bayer_flip = 3,
|
||||
.hdr = false
|
||||
},
|
||||
};
|
||||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
|
||||
camera_init(v, &s->road_cam, CAMERA_ID_IMX298, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
|
||||
|
|
|
@ -14,29 +14,19 @@
|
|||
#define FRAME_BUF_COUNT 16
|
||||
|
||||
typedef struct CameraState {
|
||||
int camera_id;
|
||||
int camera_num;
|
||||
CameraInfo ci;
|
||||
|
||||
int fps;
|
||||
float digital_gain;
|
||||
float cur_gain_frac;
|
||||
|
||||
CameraBuf buf;
|
||||
} CameraState;
|
||||
|
||||
typedef struct MultiCameraState {
|
||||
int ispif_fd;
|
||||
|
||||
CameraState road_cam;
|
||||
CameraState driver_cam;
|
||||
|
||||
SubMaster *sm;
|
||||
PubMaster *pm;
|
||||
} MultiCameraState;
|
||||
|
||||
void cameras_init(VisionIpcServer * v, MultiCameraState *s, cl_device_id device_id, cl_context ctx);
|
||||
void cameras_open(MultiCameraState *s);
|
||||
void cameras_run(MultiCameraState *s);
|
||||
void cameras_close(MultiCameraState *s);
|
||||
void camera_autoexposure(CameraState *s, float grey_frac);
|
||||
|
|
|
@ -16,26 +16,19 @@
|
|||
|
||||
#include "common/mat.h"
|
||||
#include "common/util.h"
|
||||
#include "imgproc/utils.h"
|
||||
|
||||
#include "camera_common.h"
|
||||
|
||||
#define FRAME_BUF_COUNT 4
|
||||
#define METADATA_BUF_COUNT 4
|
||||
|
||||
#define DEVICE_OP3 0
|
||||
#define DEVICE_OP3T 1
|
||||
#define DEVICE_LP3 2
|
||||
|
||||
#define NUM_FOCUS 8
|
||||
|
||||
#define LP3_AF_DAC_DOWN 366
|
||||
#define LP3_AF_DAC_UP 634
|
||||
#define LP3_AF_DAC_M 440
|
||||
#define LP3_AF_DAC_3SIG 52
|
||||
#define OP3T_AF_DAC_DOWN 224
|
||||
#define OP3T_AF_DAC_UP 456
|
||||
#define OP3T_AF_DAC_M 300
|
||||
#define OP3T_AF_DAC_3SIG 96
|
||||
|
||||
#define FOCUS_RECOVER_PATIENCE 50 // 2.5 seconds of complete blur
|
||||
#define FOCUS_RECOVER_STEPS 240 // 6 seconds
|
||||
|
@ -54,92 +47,62 @@ typedef struct StreamState {
|
|||
typedef struct CameraState {
|
||||
int camera_num;
|
||||
int camera_id;
|
||||
|
||||
int fps;
|
||||
CameraInfo ci;
|
||||
|
||||
int device;
|
||||
|
||||
uint32_t pixel_clock;
|
||||
uint32_t line_length_pclk;
|
||||
unsigned int max_gain;
|
||||
|
||||
unique_fd csid_fd;
|
||||
unique_fd csiphy_fd;
|
||||
unique_fd sensor_fd;
|
||||
unique_fd isp_fd;
|
||||
unique_fd eeprom_fd;
|
||||
// rear only
|
||||
unique_fd ois_fd, actuator_fd;
|
||||
uint16_t infinity_dac;
|
||||
|
||||
struct msm_vfe_axi_stream_cfg_cmd stream_cfg;
|
||||
|
||||
size_t eeprom_size;
|
||||
uint8_t *eeprom;
|
||||
StreamState ss[3];
|
||||
CameraBuf buf;
|
||||
|
||||
// uint32_t camera_bufs_ids[FRAME_BUF_COUNT];
|
||||
|
||||
pthread_mutex_t frame_info_lock;
|
||||
std::mutex frame_info_lock;
|
||||
FrameMetadata frame_metadata[METADATA_BUF_COUNT];
|
||||
int frame_metadata_idx;
|
||||
float cur_exposure_frac;
|
||||
float cur_gain_frac;
|
||||
int cur_gain;
|
||||
|
||||
// exposure
|
||||
uint32_t pixel_clock, line_length_pclk;
|
||||
uint32_t max_gain;
|
||||
float cur_exposure_frac, cur_gain_frac;
|
||||
int cur_gain, cur_integ_lines;
|
||||
int cur_frame_length;
|
||||
int cur_integ_lines;
|
||||
|
||||
std::atomic<float> digital_gain;
|
||||
|
||||
StreamState ss[3];
|
||||
|
||||
camera_apply_exposure_func apply_exposure;
|
||||
|
||||
int16_t focus[NUM_FOCUS];
|
||||
uint8_t confidence[NUM_FOCUS];
|
||||
|
||||
// rear camera only,used for focusing
|
||||
unique_fd actuator_fd;
|
||||
std::atomic<float> focus_err;
|
||||
|
||||
uint16_t cur_step_pos;
|
||||
uint16_t cur_lens_pos;
|
||||
std::atomic<float> last_sag_acc_z;
|
||||
std::atomic<float> lens_true_pos;
|
||||
|
||||
std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
|
||||
|
||||
int fps;
|
||||
|
||||
CameraBuf buf;
|
||||
uint16_t cur_step_pos;
|
||||
uint16_t cur_lens_pos;
|
||||
int16_t focus[NUM_FOCUS];
|
||||
uint8_t confidence[NUM_FOCUS];
|
||||
} CameraState;
|
||||
|
||||
|
||||
typedef struct MultiCameraState {
|
||||
int device;
|
||||
|
||||
unique_fd ispif_fd;
|
||||
unique_fd msmcfg_fd;
|
||||
unique_fd v4l_fd;
|
||||
|
||||
cl_mem rgb_conv_roi_cl, rgb_conv_result_cl, rgb_conv_filter_cl;
|
||||
uint16_t lapres[(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)];
|
||||
|
||||
VisionBuf focus_bufs[FRAME_BUF_COUNT];
|
||||
VisionBuf stats_bufs[FRAME_BUF_COUNT];
|
||||
|
||||
cl_program prg_rgb_laplacian;
|
||||
cl_kernel krnl_rgb_laplacian;
|
||||
|
||||
CameraState road_cam;
|
||||
CameraState driver_cam;
|
||||
|
||||
SubMaster *sm;
|
||||
PubMaster *pm;
|
||||
|
||||
LapConv *lap_conv;
|
||||
} MultiCameraState;
|
||||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx);
|
||||
void cameras_open(MultiCameraState *s);
|
||||
void cameras_run(MultiCameraState *s);
|
||||
void cameras_close(MultiCameraState *s);
|
||||
|
||||
void camera_autoexposure(CameraState *s, float grey_frac);
|
||||
void actuator_move(CameraState *s, uint16_t target);
|
||||
int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type);
|
||||
|
|
|
@ -29,6 +29,7 @@
|
|||
//#define FRAME_STRIDE 1936 // for 8 bit output
|
||||
#define FRAME_STRIDE 2416 // for 10 bit output
|
||||
|
||||
#define MIPI_SETTLE_CNT 33 // Calculated by camera_freqs.py
|
||||
|
||||
extern ExitHandler do_exit;
|
||||
|
||||
|
@ -79,7 +80,8 @@ int device_control(int fd, int op_code, int session_handle, int dev_handle) {
|
|||
return cam_control(fd, op_code, &release_dev_cmd, sizeof(release_dev_cmd));
|
||||
}
|
||||
|
||||
void *alloc_w_mmu_hdl(int video0_fd, int len, int align, int flags, uint32_t *handle, int mmu_hdl, int mmu_hdl2) {
|
||||
void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
|
||||
int mmu_hdl = 0, int mmu_hdl2 = 0) {
|
||||
struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0};
|
||||
mem_mgr_alloc_cmd.len = len;
|
||||
mem_mgr_alloc_cmd.align = align;
|
||||
|
@ -108,10 +110,6 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, int align, int flags, uint32_t *ha
|
|||
return ptr;
|
||||
}
|
||||
|
||||
void *alloc(int video0_fd, int len, int align, int flags, uint32_t *handle) {
|
||||
return alloc_w_mmu_hdl(video0_fd, len, align, flags, handle, 0, 0);
|
||||
}
|
||||
|
||||
void release(int video0_fd, uint32_t handle) {
|
||||
int ret;
|
||||
struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
|
||||
|
@ -142,8 +140,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(s->video0_fd, size, 8,
|
||||
CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 0;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
|
@ -167,8 +164,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
|
|||
// 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(s->video0_fd, size, 8,
|
||||
CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 1;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
|
@ -178,7 +174,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(s->video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (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->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;
|
||||
|
@ -204,8 +200,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
|
|||
void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc(video0_fd, size, 8,
|
||||
CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 2;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.op_code = 0x1000003;
|
||||
|
@ -214,7 +209,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
|||
|
||||
buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe);
|
||||
buf_desc[0].type = CAM_CMD_BUF_LEGACY;
|
||||
struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc(video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
struct cam_cmd_probe *probe = (struct cam_cmd_probe *)((uint8_t *)i2c_info) + sizeof(struct cam_cmd_i2c_info);
|
||||
|
||||
switch (camera_num) {
|
||||
|
@ -251,7 +246,7 @@ 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(video0_fd, buf_desc[1].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[1].mem_handle);
|
||||
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;
|
||||
|
||||
|
@ -377,8 +372,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(s->video0_fd, size, 8,
|
||||
CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 2;
|
||||
pkt->kmd_cmd_buf_index = 0;
|
||||
|
||||
|
@ -414,7 +408,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(s->video0_fd, buf_desc[1].size, 0x20, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[1].mem_handle);
|
||||
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
|
||||
|
||||
// cam_isp_packet_generic_blob_handler
|
||||
uint32_t tmp[] = {
|
||||
|
@ -716,7 +710,7 @@ static void camera_open(CameraState *s) {
|
|||
// acquires done
|
||||
|
||||
// config ISP
|
||||
alloc_w_mmu_hdl(s->video0_fd, 984480, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&s->buf0_handle, s->device_iommu, s->cdm_iommu);
|
||||
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);
|
||||
config_isp(s, 0, 0, 1, s->buf0_handle, 0);
|
||||
|
||||
LOG("-- Configuring sensor");
|
||||
|
@ -732,8 +726,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(s->video0_fd, size, 8,
|
||||
CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 1;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
|
@ -742,15 +735,15 @@ 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(s->video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
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);
|
||||
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
|
||||
csiphy_info->combo_mode = 0x0;
|
||||
csiphy_info->lane_cnt = 0x4;
|
||||
csiphy_info->secure_mode = 0x0;
|
||||
csiphy_info->settle_time = 2800000000;
|
||||
csiphy_info->data_rate = 44000000;
|
||||
csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
|
||||
csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py
|
||||
|
||||
static struct cam_config_dev_cmd config_dev_cmd = {};
|
||||
config_dev_cmd.session_handle = s->session_handle;
|
||||
|
@ -905,7 +898,7 @@ static void camera_close(CameraState *s) {
|
|||
LOGD("destroyed session: %d", ret);
|
||||
}
|
||||
|
||||
static void cameras_close(MultiCameraState *s) {
|
||||
void cameras_close(MultiCameraState *s) {
|
||||
camera_close(&s->road_cam);
|
||||
camera_close(&s->wide_road_cam);
|
||||
camera_close(&s->driver_cam);
|
||||
|
@ -945,11 +938,15 @@ void handle_camera_event(CameraState *s, void *evdat) {
|
|||
// metas
|
||||
s->frame_id_last = main_id;
|
||||
s->request_id_last = real_id;
|
||||
s->buf.camera_bufs_metadata[buf_idx].frame_id = main_id - s->idx_offset;
|
||||
s->buf.camera_bufs_metadata[buf_idx].timestamp_sof = timestamp;
|
||||
s->buf.camera_bufs_metadata[buf_idx].global_gain = s->analog_gain + (100*s->dc_gain_enabled);
|
||||
s->buf.camera_bufs_metadata[buf_idx].gain_frac = s->analog_gain_frac;
|
||||
s->buf.camera_bufs_metadata[buf_idx].integ_lines = s->exposure_time;
|
||||
|
||||
auto &meta_data = s->buf.camera_bufs_metadata[buf_idx];
|
||||
meta_data.frame_id = main_id - s->idx_offset;
|
||||
meta_data.timestamp_sof = timestamp;
|
||||
s->exp_lock.lock();
|
||||
meta_data.global_gain = s->analog_gain + (100*s->dc_gain_enabled);
|
||||
meta_data.gain_frac = s->analog_gain_frac;
|
||||
meta_data.integ_lines = s->exposure_time;
|
||||
s->exp_lock.unlock();
|
||||
|
||||
// dispatch
|
||||
enqueue_req_multi(s, real_id + FRAME_BUF_COUNT, 1, 1);
|
||||
|
@ -1002,13 +999,14 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
|||
// TODO: get stats from sensor?
|
||||
float target_grey = 0.4 - ((float)(s->analog_gain + 4*s->dc_gain_enabled) / 48.0f);
|
||||
float exposure_factor = 1 + 30 * pow((target_grey - grey_frac), 3);
|
||||
exposure_factor = std::max(exposure_factor, 0.4f);
|
||||
exposure_factor = std::max(exposure_factor, 0.56f);
|
||||
|
||||
if (s->camera_num != 1) {
|
||||
s->ef_filtered = (1 - EF_LOWPASS_K) * s->ef_filtered + EF_LOWPASS_K * exposure_factor;
|
||||
exposure_factor = s->ef_filtered;
|
||||
}
|
||||
|
||||
s->exp_lock.lock();
|
||||
// always prioritize exposure time adjust
|
||||
s->exposure_time *= exposure_factor;
|
||||
|
||||
|
@ -1048,14 +1046,15 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
|||
AG = 0xFF00 + AG * 16 + AG;
|
||||
s->analog_gain_frac = sensor_analog_gains[s->analog_gain];
|
||||
|
||||
s->exp_lock.unlock();
|
||||
// printf("cam %d, min %d, max %d \n", s->camera_num, s->exposure_time_min, s->exposure_time_max);
|
||||
// printf("cam %d, set AG to 0x%X, S to %d, dc %d \n", s->camera_num, AG, s->exposure_time, s->dc_gain_enabled);
|
||||
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x3366, AG}, // analog gain
|
||||
{0x3362, (uint16_t)(s->dc_gain_enabled?0x1:0x0)}, // DC_GAIN
|
||||
{0x305A, 0x00D8}, // red gain
|
||||
{0x3058, 0x011B}, // blue gain
|
||||
{0x305A, 0x00F8}, // red gain
|
||||
{0x3058, 0x0122}, // blue gain
|
||||
{0x3056, 0x009A}, // g1 gain
|
||||
{0x305C, 0x009A}, // g2 gain
|
||||
{0x3012, (uint16_t)s->exposure_time}, // integ time
|
||||
|
@ -1115,7 +1114,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
|
|||
if (cnt % 3 == 0) {
|
||||
const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
|
||||
const int skip = 2;
|
||||
set_exposure_target(c, (const uint8_t *)b->cur_yuv_buf->y, x, x + w, skip, y, y + h, skip);
|
||||
camera_autoexposure(c, set_exposure_target(b, x, x + w, skip, y, y + h, skip, (int)c->analog_gain, true, true));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -11,11 +11,7 @@
|
|||
|
||||
#define ANALOG_GAIN_MAX_IDX 10 // 0xF is bypass
|
||||
#define EXPOSURE_TIME_MIN 2 // with HDR, fastest ss
|
||||
#define EXPOSURE_TIME_MAX 1757 // with HDR, slowest ss
|
||||
|
||||
#define HLC_THRESH 222
|
||||
#define HLC_A 80
|
||||
#define HISTO_CEIL_K 5
|
||||
#define EXPOSURE_TIME_MAX 1904 // with HDR, slowest ss
|
||||
|
||||
#define EF_LOWPASS_K 0.35
|
||||
|
||||
|
@ -23,7 +19,8 @@
|
|||
|
||||
typedef struct CameraState {
|
||||
CameraInfo ci;
|
||||
|
||||
|
||||
std::mutex exp_lock;
|
||||
float analog_gain_frac;
|
||||
uint16_t analog_gain;
|
||||
bool dc_gain_enabled;
|
||||
|
@ -83,8 +80,3 @@ typedef struct MultiCameraState {
|
|||
SubMaster *sm;
|
||||
PubMaster *pm;
|
||||
} MultiCameraState;
|
||||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx);
|
||||
void cameras_open(MultiCameraState *s);
|
||||
void cameras_run(MultiCameraState *s);
|
||||
void camera_autoexposure(CameraState *s, float grey_frac);
|
||||
|
|