diff --git a/.gitignore b/.gitignore
index b25a8f8b..f1f1d431 100644
--- a/.gitignore
+++ b/.gitignore
@@ -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
diff --git a/Jenkinsfile b/Jenkinsfile
index 32bdc496..78a3a0fa 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -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"],
+ ])
+ }
+ }
+
}
}
}
diff --git a/README.md b/README.md
index 92545d0a..52c06169 100644
--- a/README.md
+++ b/README.md
@@ -66,7 +66,7 @@ Supported Cars
| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
| Acura | ILX 2016-19 | AcuraWatch Plus | openpilot | 25mph1 | 25mph |
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 12mph |
-| Acura | RDX 2020 | 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 | 25mph1 | 0mph |
| Honda | Passport 2019 | All | openpilot | 25mph1 | 12mph |
| Honda | Pilot 2016-19 | Honda Sensing | openpilot | 25mph1 | 12mph |
-| Honda | Ridgeline 2017-20 | Honda Sensing | openpilot | 25mph1 | 12mph |
+| Honda | Ridgeline 2017-21 | Honda Sensing | openpilot | 25mph1 | 12mph |
| Hyundai | Palisade 2020-21 | All | Stock | 0mph | 0mph |
| Hyundai | Sonata 2020-21 | All | Stock | 0mph | 0mph |
| Lexus | CT Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph |
@@ -97,7 +97,7 @@ Supported Cars
| Lexus | RX 2020-21 | All | openpilot | 0mph | 0mph |
| Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph |
| Lexus | RX Hybrid 2020 | All | openpilot | 0mph | 0mph |
-| Toyota | Avalon 2016-18, 2021 | TSS-P | Stock3| 20mph1 | 0mph |
+| Toyota | Avalon 2016-18, 2020-21 | TSS-P | Stock3| 20mph1 | 0mph |
| Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph |
| Toyota | Camry 2021 | All | openpilot | 0mph | 0mph |
| Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph4 | 0mph |
@@ -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-172 | Driver Assistance | Stock | 0mph | 0mph |
+| Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph |
1Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
+2Only includes the MQB Passat sold outside of North America. The NMS Passat made in Chattanooga TN is not yet supported.
Although it's not upstream, there's a community of people getting openpilot to run on Tesla's [here](https://tinkla.us/)
@@ -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
diff --git a/RELEASES.md b/RELEASES.md
index 035958c8..42b6379b 100644
--- a/RELEASES.md
+++ b/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
diff --git a/SConstruct b/SConstruct
index 4330052d..fc1ddfd6 100644
--- a/SConstruct
+++ b/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:
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
deleted file mode 100644
index a99da3ca..00000000
Binary files a/apk/ai.comma.plus.offroad.apk and /dev/null differ
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 9a8743f8..d5726f7c 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -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
diff --git a/cereal/legacy.capnp b/cereal/legacy.capnp
index 6474f441..0534dabc 100644
--- a/cereal/legacy.capnp
+++ b/cereal/legacy.capnp
@@ -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);
diff --git a/cereal/log.capnp b/cereal/log.capnp
index e717de08..aca7a9b3 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -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;
}
}
diff --git a/cereal/messaging/messaging.hpp b/cereal/messaging/messaging.hpp
index 6531de62..e461f06d 100644
--- a/cereal/messaging/messaging.hpp
+++ b/cereal/messaging/messaging.hpp
@@ -120,3 +120,21 @@ public:
private:
std::map sockets_;
};
+
+class AlignedBuffer {
+public:
+ kj::ArrayPtr 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(words_size < 512 ? 512 : words_size);
+ }
+ memcpy(aligned_buf.begin(), data, size);
+ return aligned_buf.slice(0, words_size);
+ }
+ inline kj::ArrayPtr align(Message *m) {
+ return align(m->getData(), m->getSize());
+ }
+private:
+ kj::Array aligned_buf;
+ size_t words_size;
+};
diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc
index 53821aa2..1240009f 100644
--- a/cereal/messaging/socketmaster.cc
+++ b/cereal/messaging/socketmaster.cc
@@ -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 buf;
+ AlignedBuffer aligned_buf;
cereal::Event::Reader event;
};
@@ -56,8 +56,7 @@ SubMaster::SubMaster(const std::initializer_list &service_list, co
.socket = socket,
.freq = serv->frequency,
.ignore_alive = inList(ignore_alive, name),
- .allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)),
- .buf = kj::heapArray(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(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(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();
m->updated = true;
m->rcv_time = current_time;
diff --git a/cereal/services.py b/cereal/services.py
index daf6d6de..98f80acd 100755
--- a/cereal/services.py
+++ b/cereal/services.py
@@ -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.),
diff --git a/common/dict_helpers.py b/common/dict_helpers.py
new file mode 100644
index 00000000..62cff63b
--- /dev/null
+++ b/common/dict_helpers.py
@@ -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
diff --git a/common/logging_extra.py b/common/logging_extra.py
index ce8889b4..9471b441 100644
--- a/common/logging_extra.py
+++ b/common/logging_extra.py
@@ -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)
diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx
index cae66468..3d366b08 100755
--- a/common/params_pyx.pyx
+++ b/common/params_pyx.pyx
@@ -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],
diff --git a/common/spinner.py b/common/spinner.py
index cde33420..27b76519 100644
--- a/common/spinner.py
+++ b/common/spinner.py
@@ -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:
diff --git a/installer/updater/update.json b/installer/updater/update.json
index ef02bfae..faff0c52 100644
--- a/installer/updater/update.json
+++ b/installer/updater/update.json
@@ -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"
}
diff --git a/launch.sh b/launch.sh
deleted file mode 100755
index eeebfc9e..00000000
--- a/launch.sh
+++ /dev/null
@@ -1,4 +0,0 @@
-#!/usr/bin/bash
-
-export PASSIVE="0"
-exec ./launch_chffrplus.sh
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index a5298d9d..c9605cba 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -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
diff --git a/launch_env.sh b/launch_env.sh
index d75f8d0d..da81aa41 100755
--- a/launch_env.sh
+++ b/launch_env.sh
@@ -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
diff --git a/models/supercombo.dlc b/models/supercombo.dlc
index faef0b3d..dc905d59 100644
Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ
diff --git a/opendbc/can/common.cc b/opendbc/can/common.cc
index d0ace8c8..b374467a 100644
--- a/opendbc/can/common.cc
+++ b/opendbc/can/common.cc
@@ -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;
diff --git a/opendbc/can/common.h b/opendbc/can/common.h
index 4daf8164..95833402 100644
--- a/opendbc/can/common.h
+++ b/opendbc/can/common.h
@@ -5,8 +5,12 @@
#include
#include "common_dbc.h"
+#include
#include
+
+#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 aligned_buf;
const DBC *dbc = NULL;
std::unordered_map message_states;
@@ -54,9 +62,13 @@ public:
CANParser(int abus, const std::string& dbc_name,
const std::vector &options,
const std::vector &sigoptions);
- void UpdateCans(uint64_t sec, const capnp::List::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::Reader& cans);
+ #endif
+ void UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cans);
+ void UpdateValid(uint64_t sec);
std::vector query_latest();
};
@@ -68,5 +80,6 @@ private:
public:
CANPacker(const std::string& dbc_name);
- uint64_t pack(uint32_t address, const std::vector &signals, int counter);
+ uint64_t pack(uint32_t address, const std::vector &values, int counter);
+ Msg* lookup_message(uint32_t address);
};
diff --git a/opendbc/can/common_dbc.h b/opendbc/can/common_dbc.h
index e5c12ba6..dc49f315 100644
--- a/opendbc/can/common_dbc.h
+++ b/opendbc/can/common_dbc.h
@@ -3,6 +3,7 @@
#include
#include
#include
+#include
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
@@ -74,6 +75,7 @@ struct DBC {
size_t num_vals;
};
+std::vector& get_dbcs();
const DBC* dbc_lookup(const std::string& dbc_name);
void dbc_register(const DBC* dbc);
diff --git a/opendbc/can/dbc.cc b/opendbc/can/dbc.cc
index 6587de7f..5747c7c6 100644
--- a/opendbc/can/dbc.cc
+++ b/opendbc/can/dbc.cc
@@ -2,15 +2,11 @@
#include "common_dbc.h"
-namespace {
-
std::vector& get_dbcs() {
static std::vector vec;
return vec;
}
-}
-
const DBC* dbc_lookup(const std::string& dbc_name) {
for (const auto& dbci : get_dbcs()) {
if (dbc_name == dbci->name) {
diff --git a/opendbc/can/packer.cc b/opendbc/can/packer.cc
index ba675a5b..146b4dee 100644
--- a/opendbc/can/packer.cc
+++ b/opendbc/can/packer.cc
@@ -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 &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 &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 &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 &s
return ret;
}
+
+Msg* CANPacker::lookup_message(uint32_t address) {
+ return &message_lookup[address];
+}
diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc
index 44baee5e..9e23311c 100644
--- a/opendbc/can/parser.cc
+++ b/opendbc/can/parser.cc
@@ -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 &options,
const std::vector &sigoptions)
- : bus(abus) {
+ : bus(abus), aligned_buf(kj::heapArray(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; inum_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; inum_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; inum_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(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();
+
+ 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::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() != bus) {
+ DEBUG("skip %d: wrong bus\n", cmsg.get("address").as());
+ return;
+ }
+
+ auto state_it = message_states.find(cmsg.get("address").as());
+ if (state_it == message_states.end()) {
+ DEBUG("skip %d: not specified\n", cmsg.get("address").as());
+ return;
+ }
+
+ auto dat = cmsg.get("dat").as();
+ 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(), 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((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();
-
- last_sec = event.getLogMonoTime();
-
- auto cans = sendcan? event.getSendcan() : event.getCan();
- UpdateCans(last_sec, cans);
-
- UpdateValid(last_sec);
-}
-
-
std::vector CANParser::query_latest() {
std::vector ret;
diff --git a/opendbc/can/process_dbc.py b/opendbc/can/process_dbc.py
index 9025331d..811e59b2 100755
--- a/opendbc/can/process_dbc.py
+++ b/opendbc/can/process_dbc.py
@@ -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:
diff --git a/opendbc/gm_global_a_powertrain.dbc b/opendbc/gm_global_a_powertrain.dbc
index 948a519b..e11c769f 100644
--- a/opendbc/gm_global_a_powertrain.dbc
+++ b/opendbc/gm_global_a_powertrain.dbc
@@ -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
diff --git a/opendbc/vw_mqb_2010.dbc b/opendbc/vw_mqb_2010.dbc
index c542b31f..f68564fa 100644
--- a/opendbc/vw_mqb_2010.dbc
+++ b/opendbc/vw_mqb_2010.dbc
@@ -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
diff --git a/panda/.gitignore b/panda/.gitignore
index 2c453216..2cd2eb8e 100644
--- a/panda/.gitignore
+++ b/panda/.gitignore
@@ -16,3 +16,4 @@ examples/output.csv
.vscode*
nosetests.xml
.mypy_cache/
+.sconsign.dblite
diff --git a/panda/__init__.py b/panda/__init__.py
index 09466ddd..98ed364e 100644
--- a/panda/__init__.py
+++ b/panda/__init__.py
@@ -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
diff --git a/panda/board/Makefile b/panda/board/Makefile
deleted file mode 100644
index adeba047..00000000
--- a/panda/board/Makefile
+++ /dev/null
@@ -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
diff --git a/panda/board/README.md b/panda/board/README.md
index 7151bfbf..add2e923 100644
--- a/panda/board/README.md
+++ b/panda/board/README.md
@@ -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
```
diff --git a/panda/board/SConscript b/panda/board/SConscript
new file mode 100644
index 00000000..902498ac
--- /dev/null
+++ b/panda/board/SConscript
@@ -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}")
diff --git a/panda/board/build.mk b/panda/board/build.mk
deleted file mode 100644
index c53f8860..00000000
--- a/panda/board/build.mk
+++ /dev/null
@@ -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)
diff --git a/panda/board/flash.sh b/panda/board/flash.sh
new file mode 100755
index 00000000..f6e5a4b7
--- /dev/null
+++ b/panda/board/flash.sh
@@ -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')"
diff --git a/panda/board/main.c b/panda/board/main.c
index 0ce54f83..9b80830e 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -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;
diff --git a/panda/board/pedal/Makefile b/panda/board/pedal/Makefile
deleted file mode 100644
index 19937ec7..00000000
--- a/panda/board/pedal/Makefile
+++ /dev/null
@@ -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/*
-
diff --git a/panda/board/pedal/flash_can.sh b/panda/board/pedal/flash_can.sh
new file mode 100755
index 00000000..11253727
--- /dev/null
+++ b/panda/board/pedal/flash_can.sh
@@ -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
diff --git a/panda/board/pedal/recover.sh b/panda/board/pedal/recover.sh
new file mode 100755
index 00000000..607a32ce
--- /dev/null
+++ b/panda/board/pedal/recover.sh
@@ -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
diff --git a/panda/board/recover.sh b/panda/board/recover.sh
new file mode 100755
index 00000000..9cf68e5d
--- /dev/null
+++ b/panda/board/recover.sh
@@ -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
diff --git a/panda/board/safety.h b/panda/board/safety.h
index 108832dc..a3aa6bc1 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -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
}
diff --git a/panda/board/safety/safety_volkswagen.h b/panda/board/safety/safety_volkswagen.h
index 2a2c4474..3df1fcbd 100644
--- a/panda/board/safety/safety_volkswagen.h
+++ b/panda/board/safety/safety_volkswagen.h
@@ -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) {
diff --git a/panda/common/version.mk b/panda/common/version.mk
deleted file mode 100644
index cc66efaa..00000000
--- a/panda/common/version.mk
+++ /dev/null
@@ -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
diff --git a/panda/crypto/getcertheader.py b/panda/crypto/getcertheader.py
deleted file mode 100755
index a43bc02e..00000000
--- a/panda/crypto/getcertheader.py
+++ /dev/null
@@ -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('};')
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index b22b0102..1c02f751 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -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()
diff --git a/panda/python/dfu.py b/panda/python/dfu.py
index a7f51ecd..f0e05587 100644
--- a/panda/python/dfu.py
+++ b/panda/python/dfu.py
@@ -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()
diff --git a/panda/python/uds.py b/panda/python/uds.py
index f83c11fc..243dd5ac 100644
--- a/panda/python/uds.py
+++ b/panda/python/uds.py
@@ -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)
diff --git a/rednose/helpers/ekf_sym.py b/rednose/helpers/ekf_sym.py
index 6df607e5..9f635c48 100644
--- a/rednose/helpers/ekf_sym.py
+++ b/rednose/helpers/ekf_sym.py
@@ -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
diff --git a/release/build_release2.sh b/release/build_release2.sh
index 656eb20f..fe493691 100755
--- a/release/build_release2.sh
+++ b/release/build_release2.sh
@@ -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/
diff --git a/selfdrive/assets/offroad/tc.html b/selfdrive/assets/offroad/tc.html
index 4ab74907..35e791f7 100644
--- a/selfdrive/assets/offroad/tc.html
+++ b/selfdrive/assets/offroad/tc.html
@@ -5,12 +5,7 @@
openpilot Terms of Service
diff --git a/selfdrive/assets/training/step0.jpg b/selfdrive/assets/training/step0.jpg
deleted file mode 100644
index 65917358..00000000
Binary files a/selfdrive/assets/training/step0.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step0.png b/selfdrive/assets/training/step0.png
new file mode 100644
index 00000000..badeff6c
Binary files /dev/null and b/selfdrive/assets/training/step0.png differ
diff --git a/selfdrive/assets/training/step1.jpg b/selfdrive/assets/training/step1.jpg
deleted file mode 100644
index d03e1f3b..00000000
Binary files a/selfdrive/assets/training/step1.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step1.png b/selfdrive/assets/training/step1.png
new file mode 100644
index 00000000..9fa6cf68
Binary files /dev/null and b/selfdrive/assets/training/step1.png differ
diff --git a/selfdrive/assets/training/step10.jpg b/selfdrive/assets/training/step10.jpg
deleted file mode 100644
index c720acc5..00000000
Binary files a/selfdrive/assets/training/step10.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step10.png b/selfdrive/assets/training/step10.png
new file mode 100644
index 00000000..10d216c8
Binary files /dev/null and b/selfdrive/assets/training/step10.png differ
diff --git a/selfdrive/assets/training/step11.jpg b/selfdrive/assets/training/step11.jpg
deleted file mode 100644
index 776a389b..00000000
Binary files a/selfdrive/assets/training/step11.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step11.png b/selfdrive/assets/training/step11.png
new file mode 100644
index 00000000..71d98864
Binary files /dev/null and b/selfdrive/assets/training/step11.png differ
diff --git a/selfdrive/assets/training/step12.jpg b/selfdrive/assets/training/step12.jpg
deleted file mode 100644
index 2a364b3c..00000000
Binary files a/selfdrive/assets/training/step12.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step12.png b/selfdrive/assets/training/step12.png
new file mode 100644
index 00000000..627f235c
Binary files /dev/null and b/selfdrive/assets/training/step12.png differ
diff --git a/selfdrive/assets/training/step13.jpg b/selfdrive/assets/training/step13.jpg
deleted file mode 100644
index 8f958f3b..00000000
Binary files a/selfdrive/assets/training/step13.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step13.png b/selfdrive/assets/training/step13.png
new file mode 100644
index 00000000..2430c6c8
Binary files /dev/null and b/selfdrive/assets/training/step13.png differ
diff --git a/selfdrive/assets/training/step14.jpg b/selfdrive/assets/training/step14.jpg
deleted file mode 100644
index 9b1044ae..00000000
Binary files a/selfdrive/assets/training/step14.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step14.png b/selfdrive/assets/training/step14.png
new file mode 100644
index 00000000..835d6e24
Binary files /dev/null and b/selfdrive/assets/training/step14.png differ
diff --git a/selfdrive/assets/training/step15.png b/selfdrive/assets/training/step15.png
new file mode 100644
index 00000000..c65b0e2a
Binary files /dev/null and b/selfdrive/assets/training/step15.png differ
diff --git a/selfdrive/assets/training/step16.png b/selfdrive/assets/training/step16.png
new file mode 100644
index 00000000..bff55984
Binary files /dev/null and b/selfdrive/assets/training/step16.png differ
diff --git a/selfdrive/assets/training/step17.png b/selfdrive/assets/training/step17.png
new file mode 100644
index 00000000..81d214d0
Binary files /dev/null and b/selfdrive/assets/training/step17.png differ
diff --git a/selfdrive/assets/training/step2.jpg b/selfdrive/assets/training/step2.jpg
deleted file mode 100644
index 1b1aa83b..00000000
Binary files a/selfdrive/assets/training/step2.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step2.png b/selfdrive/assets/training/step2.png
new file mode 100644
index 00000000..ab1498bc
Binary files /dev/null and b/selfdrive/assets/training/step2.png differ
diff --git a/selfdrive/assets/training/step3.jpg b/selfdrive/assets/training/step3.jpg
deleted file mode 100644
index 556a36b0..00000000
Binary files a/selfdrive/assets/training/step3.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step3.png b/selfdrive/assets/training/step3.png
new file mode 100644
index 00000000..d19e388e
Binary files /dev/null and b/selfdrive/assets/training/step3.png differ
diff --git a/selfdrive/assets/training/step4.jpg b/selfdrive/assets/training/step4.jpg
deleted file mode 100644
index 4e981f1b..00000000
Binary files a/selfdrive/assets/training/step4.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step4.png b/selfdrive/assets/training/step4.png
new file mode 100644
index 00000000..874b1939
Binary files /dev/null and b/selfdrive/assets/training/step4.png differ
diff --git a/selfdrive/assets/training/step5.jpg b/selfdrive/assets/training/step5.jpg
deleted file mode 100644
index 880374aa..00000000
Binary files a/selfdrive/assets/training/step5.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step5.png b/selfdrive/assets/training/step5.png
new file mode 100644
index 00000000..9ee3388c
Binary files /dev/null and b/selfdrive/assets/training/step5.png differ
diff --git a/selfdrive/assets/training/step6.jpg b/selfdrive/assets/training/step6.jpg
deleted file mode 100644
index 0201bc8e..00000000
Binary files a/selfdrive/assets/training/step6.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step6.png b/selfdrive/assets/training/step6.png
new file mode 100644
index 00000000..14463ea0
Binary files /dev/null and b/selfdrive/assets/training/step6.png differ
diff --git a/selfdrive/assets/training/step7.jpg b/selfdrive/assets/training/step7.jpg
deleted file mode 100644
index 4198df06..00000000
Binary files a/selfdrive/assets/training/step7.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step7.png b/selfdrive/assets/training/step7.png
new file mode 100644
index 00000000..68233321
Binary files /dev/null and b/selfdrive/assets/training/step7.png differ
diff --git a/selfdrive/assets/training/step8.jpg b/selfdrive/assets/training/step8.jpg
deleted file mode 100644
index 7e07ee8a..00000000
Binary files a/selfdrive/assets/training/step8.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step8.png b/selfdrive/assets/training/step8.png
new file mode 100644
index 00000000..1a5bb136
Binary files /dev/null and b/selfdrive/assets/training/step8.png differ
diff --git a/selfdrive/assets/training/step9.jpg b/selfdrive/assets/training/step9.jpg
deleted file mode 100644
index 7e8114f8..00000000
Binary files a/selfdrive/assets/training/step9.jpg and /dev/null differ
diff --git a/selfdrive/assets/training/step9.png b/selfdrive/assets/training/step9.png
new file mode 100644
index 00000000..f9b53cf7
Binary files /dev/null and b/selfdrive/assets/training/step9.png differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index c3645ed8..5191e322 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -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:
diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py
index cd8bc582..c16bef07 100755
--- a/selfdrive/athena/manage_athenad.py
+++ b/selfdrive/athena/manage_athenad.py
@@ -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()
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 5b82b023..a93e05d2 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -15,6 +15,7 @@
#include
#include
#include
+#include
#include
@@ -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((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::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((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();
//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 last_recv_time;
+ std::unordered_map cls_max_dt = {
+ {(char)ublox::CLASS_NAV, int64_t(250000000ULL)}, // 0.25s
+ {(char)ublox::CLASS_RXM, int64_t(250000000ULL)}, // 0.25s
+ };
+
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;
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 84697965..e8cb23e1 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -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;
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 26bd2636..d34a01c9 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -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 connected = true;
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
- bool is_pigeon = false;
bool has_rtc = false;
// HW communication
diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc
index 8cd07904..3f2588f2 100644
--- a/selfdrive/boardd/pigeon.cc
+++ b/selfdrive/boardd/pigeon.cc
@@ -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);
diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript
index 9fb5da90..a26c20dd 100644
--- a/selfdrive/camerad/SConscript
+++ b/selfdrive/camerad/SConscript
@@ -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)
diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc
index df472d5b..b431eeca 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -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;
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index 9e26ef4d..55c25214 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -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 safe_queue;
@@ -113,7 +113,7 @@ public:
std::unique_ptr camera_bufs;
std::unique_ptr 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 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);
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc
index 7fbd3d34..d6d44748 100644
--- a/selfdrive/camerad/cameras/camera_frame_stream.cc
+++ b/selfdrive/camerad/cameras/camera_frame_stream.cc
@@ -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);
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h
index fe03c117..4a851265 100644
--- a/selfdrive/camerad/cameras/camera_frame_stream.h
+++ b/selfdrive/camerad/cameras/camera_frame_stream.h
@@ -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);
diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc
index 051854b3..40628e52 100644
--- a/selfdrive/camerad/cameras/camera_qcom.cc
+++ b/selfdrive/camerad/cameras/camera_qcom.cc
@@ -28,6 +28,9 @@
#include "sensor_i2c.h"
#include "camera_qcom.h"
+// leeco actuator (DW9800W H-Bridge Driver IC)
+// from sniff
+const uint16_t INFINITY_DAC = 364;
extern ExitHandler do_exit;
@@ -41,25 +44,9 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
.frame_height = 1748,
.frame_stride = 2912,
.bayer = true,
- .bayer_flip = 0,
+ .bayer_flip = 3,
.hdr = true
},
- [CAMERA_ID_IMX179] = {
- .frame_width = 3280,
- .frame_height = 2464,
- .frame_stride = 4104,
- .bayer = true,
- .bayer_flip = 0,
- .hdr = false
- },
- [CAMERA_ID_S5K3P8SP] = {
- .frame_width = 2304,
- .frame_height = 1728,
- .frame_stride = 2880,
- .bayer = true,
- .bayer_flip = 1,
- .hdr = false
- },
[CAMERA_ID_OV8865] = {
.frame_width = 1632,
.frame_height = 1224,
@@ -94,29 +81,6 @@ static void camera_release_buffer(void* cookie, int buf_idx) {
ioctl(s->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &s->ss[0].qbuf_info[buf_idx]);
}
-static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int camera_num,
- uint32_t pixel_clock, uint32_t line_length_pclk,
- unsigned int max_gain, unsigned int fps, cl_device_id device_id, cl_context ctx,
- VisionStreamType rgb_type, VisionStreamType yuv_type) {
- s->camera_num = camera_num;
- s->camera_id = camera_id;
-
- assert(camera_id < ARRAYSIZE(cameras_supported));
- s->ci = cameras_supported[camera_id];
- assert(s->ci.frame_width != 0);
-
- s->pixel_clock = pixel_clock;
- s->line_length_pclk = line_length_pclk;
- s->max_gain = max_gain;
- s->fps = fps;
-
- s->self_recover = 0;
-
- s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type, camera_release_buffer);
-
- pthread_mutex_init(&s->frame_info_lock, NULL);
-}
-
int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, msm_camera_i2c_data_type data_type) {
struct msm_camera_i2c_reg_setting out_settings = {
.reg_setting = arr,
@@ -156,12 +120,7 @@ static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int
// REG_HOLD
{0x104,0x0,0},
};
-
- int err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
- if (err != 0) {
- LOGE("apply_exposure err %d", err);
- }
- return err;
+ return sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
}
static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) {
@@ -196,70 +155,38 @@ static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int
//{0x104,0x0,0},
};
- int err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
- if (err != 0) {
- LOGE("apply_exposure err %d", err);
- }
- return err;
+ return sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
}
-static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) {
- //printf("driver camera: %d %d %d\n", gain, integ_lines, frame_length);
- struct msm_camera_i2c_reg_array reg_array[] = {
- {0x104,0x1,0},
+static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int camera_num,
+ uint32_t pixel_clock, uint32_t line_length_pclk,
+ uint32_t max_gain, uint32_t fps, cl_device_id device_id, cl_context ctx,
+ VisionStreamType rgb_type, VisionStreamType yuv_type) {
+ s->camera_num = camera_num;
+ s->camera_id = camera_id;
- // FRM_LENGTH
- {0x340, (uint16_t)(frame_length >> 8), 0}, {0x341, (uint16_t)(frame_length & 0xff), 0},
- // coarse_int_time
- {0x202, (uint16_t)(integ_lines >> 8), 0}, {0x203, (uint16_t)(integ_lines & 0xff),0},
- // global_gain
- {0x204, (uint16_t)(gain >> 8), 0}, {0x205, (uint16_t)(gain & 0xff),0},
+ assert(camera_id < ARRAYSIZE(cameras_supported));
+ s->ci = cameras_supported[camera_id];
+ assert(s->ci.frame_width != 0);
- {0x104,0x0,0},
- };
- int err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
- if (err != 0) {
- LOGE("apply_exposure err %d", err);
- }
- return err;
-}
+ s->pixel_clock = pixel_clock;
+ s->line_length_pclk = line_length_pclk;
+ s->max_gain = max_gain;
+ s->fps = fps;
+ s->self_recover = 0;
-cl_program build_conv_program(cl_device_id device_id, cl_context context, int image_w, int image_h, int filter_size) {
- char args[4096];
- snprintf(args, sizeof(args),
- "-cl-fast-relaxed-math -cl-denorms-are-zero "
- "-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d "
- "-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d",
- image_w, image_h, 1,
- filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w);
- return cl_program_from_file(context, device_id, "imgproc/conv.cl", args);
+ s->apply_exposure = (camera_id == CAMERA_ID_IMX298) ? imx298_apply_exposure : ov8865_apply_exposure;
+ s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type, camera_release_buffer);
}
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
char project_name[1024] = {0};
property_get("ro.boot.project_name", project_name, "");
+ assert(strlen(project_name) == 0);
- char product_name[1024] = {0};
- property_get("ro.product.name", product_name, "");
-
- if (strlen(project_name) == 0) {
- LOGD("LePro 3 op system detected");
- s->device = DEVICE_LP3;
-
- // sensor is flipped in LP3
- // IMAGE_ORIENT = 3
- init_array_imx298[0].reg_data = 3;
- cameras_supported[CAMERA_ID_IMX298].bayer_flip = 3;
- } else if (strcmp(product_name, "OnePlus3") == 0 && strcmp(project_name, "15811") != 0) {
- // no more OP3 support
- s->device = DEVICE_OP3;
- assert(false);
- } else if (strcmp(product_name, "OnePlus3") == 0 && strcmp(project_name, "15811") == 0) {
- // only OP3T support
- s->device = DEVICE_OP3T;
- } else {
- assert(false);
- }
+ // sensor is flipped in LP3
+ // IMAGE_ORIENT = 3
+ init_array_imx298[0].reg_data = 3;
// 0 = ISO 100
// 256 = ISO 200
@@ -281,30 +208,11 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
#endif
device_id, ctx,
VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
- s->road_cam.apply_exposure = imx298_apply_exposure;
- if (s->device == DEVICE_OP3T) {
- camera_init(v, &s->driver_cam, CAMERA_ID_S5K3P8SP, 1,
- /*pixel_clock=*/560000000, /*line_length_pclk=*/5120,
- /*max_gain=*/510, 10, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
- s->driver_cam.apply_exposure = imx179_s5k3p8sp_apply_exposure;
- } else if (s->device == DEVICE_LP3) {
- camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
- /*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
- /*max_gain=*/510, 10, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
- s->driver_cam.apply_exposure = ov8865_apply_exposure;
- } else {
- camera_init(v, &s->driver_cam, CAMERA_ID_IMX179, 1,
- /*pixel_clock=*/251200000, /*line_length_pclk=*/3440,
- /*max_gain=*/224, 20, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
- s->driver_cam.apply_exposure = imx179_s5k3p8sp_apply_exposure;
- }
-
- s->road_cam.device = s->device;
- s->driver_cam.device = s->device;
+ camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
+ /*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
+ /*max_gain=*/510, 10, device_id, ctx,
+ VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
s->sm = new SubMaster({"driverState"});
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
@@ -314,28 +222,17 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
s->focus_bufs[i].allocate(0xb80);
s->stats_bufs[i].allocate(0xb80);
}
- const int width = s->road_cam.buf.rgb_width/NUM_SEGMENTS_X;
- const int height = s->road_cam.buf.rgb_height/NUM_SEGMENTS_Y;
- s->prg_rgb_laplacian = build_conv_program(device_id, ctx, width, height, 3);
- s->krnl_rgb_laplacian = CL_CHECK_ERR(clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err));
- // TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter
- s->rgb_conv_roi_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE,
- width * height * 3 * sizeof(uint8_t), NULL, &err));
- s->rgb_conv_result_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE,
- width * height * sizeof(int16_t), NULL, &err));
- s->rgb_conv_filter_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
- 9 * sizeof(int16_t), (void*)&lapl_conv_krnl, &err));
-
std::fill_n(s->lapres, std::size(s->lapres), 16160);
+ s->lap_conv = new LapConv(device_id, ctx, s->road_cam.buf.rgb_width, s->road_cam.buf.rgb_height, 3);
}
static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
int err = 0;
- unsigned int frame_length = s->pixel_clock / s->line_length_pclk / s->fps;
+ uint32_t frame_length = s->pixel_clock / s->line_length_pclk / s->fps;
- unsigned int gain = s->cur_gain;
- unsigned int integ_lines = s->cur_integ_lines;
+ uint32_t gain = s->cur_gain;
+ uint32_t integ_lines = s->cur_integ_lines;
if (exposure_frac >= 0) {
exposure_frac = std::clamp(exposure_frac, 2.0f / frame_length, 1.0f);
@@ -365,25 +262,22 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
if (s->apply_exposure == ov8865_apply_exposure) {
gain = 800 * gain_frac; // ISO
- err = s->apply_exposure(s, gain, integ_lines, frame_length);
- } else if (s->apply_exposure) {
- err = s->apply_exposure(s, gain, integ_lines, frame_length);
}
-
+ err = s->apply_exposure(s, gain, integ_lines, frame_length);
if (err == 0) {
- pthread_mutex_lock(&s->frame_info_lock);
+ std::lock_guard lk(s->frame_info_lock);
s->cur_gain = gain;
s->cur_integ_lines = integ_lines;
s->cur_frame_length = frame_length;
- pthread_mutex_unlock(&s->frame_info_lock);
+ } else {
+ LOGE("camera %d apply_exposure err: %d", s->camera_num, err);
}
}
if (err == 0) {
s->cur_exposure_frac = exposure_frac;
- pthread_mutex_lock(&s->frame_info_lock);
+ std::lock_guard lk(s->frame_info_lock);
s->cur_gain_frac = gain_frac;
- pthread_mutex_unlock(&s->frame_info_lock);
}
//LOGD("set exposure: %f %f - %d", exposure_frac, gain_frac, err);
@@ -396,9 +290,9 @@ static void do_autoexposure(CameraState *s, float grey_frac) {
const float gain_frac_min = 0.015625;
const float gain_frac_max = 1.0;
// exposure time limits
- unsigned int frame_length = s->pixel_clock / s->line_length_pclk / s->fps;
- const unsigned int exposure_time_min = 16;
- const unsigned int exposure_time_max = frame_length - 11; // copied from set_exposure()
+ uint32_t frame_length = s->pixel_clock / s->line_length_pclk / s->fps;
+ const uint32_t exposure_time_min = 16;
+ const uint32_t exposure_time_max = frame_length - 11; // copied from set_exposure()
float cur_gain_frac = s->cur_gain_frac;
float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.05);
@@ -409,9 +303,9 @@ static void do_autoexposure(CameraState *s, float grey_frac) {
} else if (cur_gain_frac * exposure_factor <= gain_frac_max && cur_gain_frac * exposure_factor >= gain_frac_min) {
cur_gain_frac *= exposure_factor;
}
- pthread_mutex_lock(&s->frame_info_lock);
+ s->frame_info_lock.lock();
s->cur_gain_frac = cur_gain_frac;
- pthread_mutex_unlock(&s->frame_info_lock);
+ s->frame_info_lock.unlock();
set_exposure(s, s->cur_exposure_frac, cur_gain_frac);
} else { // keep the old for others
@@ -430,314 +324,106 @@ static void do_autoexposure(CameraState *s, float grey_frac) {
}
}
-static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) {
- msm_eeprom_cfg_data cfg = {.cfgtype = CFG_EEPROM_GET_CAL_DATA};
- int err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg);
- assert(err >= 0);
-
- uint32_t num_bytes = cfg.cfg.get_data.num_bytes;
- assert(num_bytes > 100);
-
- uint8_t* buffer = (uint8_t*)malloc(num_bytes);
- assert(buffer);
- memset(buffer, 0, num_bytes);
-
- cfg.cfgtype = CFG_EEPROM_READ_CAL_DATA;
- cfg.cfg.read_data.num_bytes = num_bytes;
- cfg.cfg.read_data.dbuffer = buffer;
- err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg);
- assert(err >= 0);
-
- *out_len = num_bytes;
- return buffer;
-}
-
-static void imx298_ois_calibration(int ois_fd, uint8_t* eeprom) {
- const int ois_registers[][2] = {
- // == SET_FADJ_PARAM() == (factory adjustment)
-
- // Set Hall Current DAC
- {0x8230, *(uint16_t*)(eeprom+0x102)}, //_P_30_ADC_CH0 (CURDAT)
-
- // Set Hall PreAmp Offset
- {0x8231, *(uint16_t*)(eeprom+0x104)}, //_P_31_ADC_CH1 (HALOFS_X)
- {0x8232, *(uint16_t*)(eeprom+0x106)}, //_P_32_ADC_CH2 (HALOFS_Y)
-
- // Set Hall-X/Y PostAmp Offset
- {0x841e, *(uint16_t*)(eeprom+0x108)}, //_M_X_H_ofs
- {0x849e, *(uint16_t*)(eeprom+0x10a)}, //_M_Y_H_ofs
-
- // Set Residual Offset
- {0x8239, *(uint16_t*)(eeprom+0x10c)}, //_P_39_Ch3_VAL_1 (PSTXOF)
- {0x823b, *(uint16_t*)(eeprom+0x10e)}, //_P_3B_Ch3_VAL_3 (PSTYOF)
-
- // DIGITAL GYRO OFFSET
- {0x8406, *(uint16_t*)(eeprom+0x110)}, //_M_Kgx00
- {0x8486, *(uint16_t*)(eeprom+0x112)}, //_M_Kgy00
- {0x846a, *(uint16_t*)(eeprom+0x120)}, //_M_TMP_X_
- {0x846b, *(uint16_t*)(eeprom+0x122)}, //_M_TMP_Y_
-
- // HALLSENSE
- // Set Hall Gain
- {0x8446, *(uint16_t*)(eeprom+0x114)}, //_M_KgxHG
- {0x84c6, *(uint16_t*)(eeprom+0x116)}, //_M_KgyHG
- // Set Cross Talk Canceller
- {0x8470, *(uint16_t*)(eeprom+0x124)}, //_M_KgxH0
- {0x8472, *(uint16_t*)(eeprom+0x126)}, //_M_KgyH0
-
- // LOOPGAIN
- {0x840f, *(uint16_t*)(eeprom+0x118)}, //_M_KgxG
- {0x848f, *(uint16_t*)(eeprom+0x11a)}, //_M_KgyG
-
- // Position Servo ON ( OIS OFF )
- {0x847f, 0x0c0c}, //_M_EQCTL
- };
-
-
- struct msm_camera_i2c_seq_reg_array ois_reg_settings[ARRAYSIZE(ois_registers)] = {{0}};
- for (int i=0; i> 8) & 0xff;
- ois_reg_settings[i].reg_data_size = 2;
- }
- struct msm_camera_i2c_seq_reg_setting ois_reg_setting = {
- .reg_setting = &ois_reg_settings[0],
- .size = ARRAYSIZE(ois_reg_settings),
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .delay = 0,
- };
- msm_ois_cfg_data cfg = {.cfgtype = CFG_OIS_I2C_WRITE_SEQ_TABLE, .cfg.settings = &ois_reg_setting};
- int err = ioctl(ois_fd, VIDIOC_MSM_OIS_CFG, &cfg);
- LOG("ois reg calibration: %d", err);
-}
-
static void sensors_init(MultiCameraState *s) {
- int err;
-
unique_fd sensorinit_fd;
- if (s->device == DEVICE_LP3) {
- sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK);
- } else {
- sensorinit_fd = open("/dev/v4l-subdev12", O_RDWR | O_NONBLOCK);
- }
+ sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK);
assert(sensorinit_fd >= 0);
// init road camera sensor
struct msm_camera_sensor_slave_info slave_info = {0};
- if (s->device == DEVICE_LP3) {
- slave_info = (struct msm_camera_sensor_slave_info){
- .sensor_name = "imx298",
- .eeprom_name = "sony_imx298",
- .actuator_name = "dw9800w",
- .ois_name = "",
- .flash_name = "pmic",
- .camera_id = CAMERA_0,
- .slave_addr = 32,
- .i2c_freq_mode = I2C_FAST_MODE,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .sensor_id_info = {.sensor_id_reg_addr = 22, .sensor_id = 664, .module_id = 9, .vcm_id = 6},
- .power_setting_array = {
- .power_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- {.seq_type = SENSOR_GPIO, .seq_val = 5, .config_val = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1},
- {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1},
- {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 10},
- },
- .size = 7,
- .power_down_setting_a = {
- {.seq_type = SENSOR_CLK, .delay = 1},
- {.seq_type = SENSOR_GPIO, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_GPIO, .seq_val = 5},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1},
- },
- .size_down = 6,
+ slave_info = (struct msm_camera_sensor_slave_info){
+ .sensor_name = "imx298",
+ .eeprom_name = "sony_imx298",
+ .actuator_name = "dw9800w",
+ .ois_name = "",
+ .flash_name = "pmic",
+ .camera_id = CAMERA_0,
+ .slave_addr = 32,
+ .i2c_freq_mode = I2C_FAST_MODE,
+ .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
+ .sensor_id_info = {.sensor_id_reg_addr = 22, .sensor_id = 664, .module_id = 9, .vcm_id = 6},
+ .power_setting_array = {
+ .power_setting_a = {
+ {.seq_type = SENSOR_GPIO, .delay = 1},
+ {.seq_type = SENSOR_VREG, .seq_val = 2},
+ {.seq_type = SENSOR_GPIO, .seq_val = 5, .config_val = 2},
+ {.seq_type = SENSOR_VREG, .seq_val = 1},
+ {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1},
+ {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1},
+ {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 10},
},
- .is_init_params_valid = 0,
- .sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 90},
- .output_format = MSM_SENSOR_BAYER,
- };
- } else {
- slave_info = (struct msm_camera_sensor_slave_info){
- .sensor_name = "imx298",
- .eeprom_name = "sony_imx298",
- .actuator_name = "rohm_bu63165gwl",
- .ois_name = "rohm_bu63165gwl",
- .camera_id = CAMERA_0,
- .slave_addr = 52,
- .i2c_freq_mode = I2C_CUSTOM_MODE,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .sensor_id_info = {.sensor_id_reg_addr = 22, .sensor_id = 664},
- .power_setting_array = {
- .power_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 2},
- {.seq_type = SENSOR_VREG, .delay = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 1, .delay = 2},
- {.seq_type = SENSOR_GPIO, .seq_val = 6, .config_val = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 5},
- {.seq_type = SENSOR_VREG, .seq_val = 4, .delay = 5},
- {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 2},
- {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 2},
- },
- .size = 9,
- .power_down_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 10},
- {.seq_type = SENSOR_CLK, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 4},
- {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1},
- {.seq_type = SENSOR_GPIO, .seq_val = 6},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- },
- .size_down = 8,
+ .size = 7,
+ .power_down_setting_a = {
+ {.seq_type = SENSOR_CLK, .delay = 1},
+ {.seq_type = SENSOR_GPIO, .delay = 1},
+ {.seq_type = SENSOR_VREG, .seq_val = 1},
+ {.seq_type = SENSOR_GPIO, .seq_val = 5},
+ {.seq_type = SENSOR_VREG, .seq_val = 2},
+ {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1},
},
- .is_init_params_valid = 0,
- .sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 360},
- .output_format = MSM_SENSOR_BAYER,
- };
- }
- slave_info.power_setting_array.power_setting =
- (struct msm_sensor_power_setting *)&slave_info.power_setting_array.power_setting_a[0];
- slave_info.power_setting_array.power_down_setting =
- (struct msm_sensor_power_setting *)&slave_info.power_setting_array.power_down_setting_a[0];
+ .size_down = 6,
+ },
+ .is_init_params_valid = 0,
+ .sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 90},
+ .output_format = MSM_SENSOR_BAYER,
+ };
+ slave_info.power_setting_array.power_setting = &slave_info.power_setting_array.power_setting_a[0];
+ slave_info.power_setting_array.power_down_setting = &slave_info.power_setting_array.power_down_setting_a[0];
sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &slave_info};
- err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg);
+ int err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg);
LOG("sensor init cfg (road camera): %d", err);
assert(err >= 0);
struct msm_camera_sensor_slave_info slave_info2 = {0};
- if (s->device == DEVICE_LP3) {
- slave_info2 = (struct msm_camera_sensor_slave_info){
- .sensor_name = "ov8865_sunny",
- .eeprom_name = "ov8865_plus",
- .actuator_name = "",
- .ois_name = "",
- .flash_name = "",
- .camera_id = CAMERA_2,
- .slave_addr = 108,
- .i2c_freq_mode = I2C_FAST_MODE,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .sensor_id_info = {.sensor_id_reg_addr = 12299, .sensor_id = 34917, .module_id = 2},
- .power_setting_array = {
- .power_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 5},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- {.seq_type = SENSOR_VREG},
- {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1},
- {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 1},
- },
- .size = 6,
- .power_down_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 5},
- {.seq_type = SENSOR_CLK, .delay = 1},
- {.seq_type = SENSOR_VREG},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1},
- },
- .size_down = 5,
+ slave_info2 = (struct msm_camera_sensor_slave_info){
+ .sensor_name = "ov8865_sunny",
+ .eeprom_name = "ov8865_plus",
+ .actuator_name = "",
+ .ois_name = "",
+ .flash_name = "",
+ .camera_id = CAMERA_2,
+ .slave_addr = 108,
+ .i2c_freq_mode = I2C_FAST_MODE,
+ .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
+ .sensor_id_info = {.sensor_id_reg_addr = 12299, .sensor_id = 34917, .module_id = 2},
+ .power_setting_array = {
+ .power_setting_a = {
+ {.seq_type = SENSOR_GPIO, .delay = 5},
+ {.seq_type = SENSOR_VREG, .seq_val = 1},
+ {.seq_type = SENSOR_VREG, .seq_val = 2},
+ {.seq_type = SENSOR_VREG},
+ {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1},
+ {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 1},
},
- .is_init_params_valid = 0,
- .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270},
- .output_format = MSM_SENSOR_BAYER,
- };
- } else if (s->driver_cam.camera_id == CAMERA_ID_S5K3P8SP) {
- // init driver camera
- slave_info2 = (struct msm_camera_sensor_slave_info){
- .sensor_name = "s5k3p8sp",
- .eeprom_name = "s5k3p8sp_m24c64s",
- .actuator_name = "",
- .ois_name = "",
- .camera_id = CAMERA_1,
- .slave_addr = 32,
- .i2c_freq_mode = I2C_FAST_MODE,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .sensor_id_info = {.sensor_id = 12552},
- .power_setting_array = {
- .power_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 1, .delay = 1},
- {.seq_type = SENSOR_VREG, .delay = 1},
- {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1},
- {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 1},
- },
- .size = 6,
- .power_down_setting_a = {
- {.seq_type = SENSOR_CLK, .delay = 1},
- {.seq_type = SENSOR_GPIO, .delay = 1},
- {.seq_type = SENSOR_VREG, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 1, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1},
- },
- .size_down = 5,
+ .size = 6,
+ .power_down_setting_a = {
+ {.seq_type = SENSOR_GPIO, .delay = 5},
+ {.seq_type = SENSOR_CLK, .delay = 1},
+ {.seq_type = SENSOR_VREG},
+ {.seq_type = SENSOR_VREG, .seq_val = 1},
+ {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1},
},
- .is_init_params_valid = 0,
- .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270},
- .output_format = MSM_SENSOR_BAYER,
- };
- } else {
- // init driver camera
- slave_info2 = (struct msm_camera_sensor_slave_info){
- .sensor_name = "imx179",
- .eeprom_name = "sony_imx179",
- .actuator_name = "",
- .ois_name = "",
- .camera_id = CAMERA_1,
- .slave_addr = 32,
- .i2c_freq_mode = I2C_FAST_MODE,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .sensor_id_info = {.sensor_id_reg_addr = 2, .sensor_id = 377, .sensor_id_mask = 4095},
- .power_setting_array = {
- .power_setting_a = {
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG},
- {.seq_type = SENSOR_GPIO, .config_val = 2},
- {.seq_type = SENSOR_CLK, .config_val = 24000000},
- },
- .size = 5,
- .power_down_setting_a = {
- {.seq_type = SENSOR_CLK},
- {.seq_type = SENSOR_GPIO, .delay = 1},
- {.seq_type = SENSOR_VREG, .delay = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- },
- .size_down = 5,
- },
- .is_init_params_valid = 0,
- .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270},
- .output_format = MSM_SENSOR_BAYER,
- };
- }
- slave_info2.power_setting_array.power_setting =
- (struct msm_sensor_power_setting *)&slave_info2.power_setting_array.power_setting_a[0];
- slave_info2.power_setting_array.power_down_setting =
- (struct msm_sensor_power_setting *)&slave_info2.power_setting_array.power_down_setting_a[0];
+ .size_down = 5,
+ },
+ .is_init_params_valid = 0,
+ .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270},
+ .output_format = MSM_SENSOR_BAYER,
+ };
+ slave_info2.power_setting_array.power_setting = &slave_info2.power_setting_array.power_setting_a[0];
+ slave_info2.power_setting_array.power_down_setting = &slave_info2.power_setting_array.power_down_setting_a[0];
sensor_init_cfg.cfgtype = CFG_SINIT_PROBE;
sensor_init_cfg.cfg.setting = &slave_info2;
err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg);
- LOG("sensor init cfg (driver): %d", err);
+ LOG("sensor init cfg (driver camera): %d", err);
assert(err >= 0);
}
static void camera_open(CameraState *s, bool is_road_cam) {
- int err;
-
struct csid_cfg_data csid_cfg_data = {};
struct v4l2_event_subscription sub = {};
struct msm_actuator_cfg_data actuator_cfg_data = {};
- struct msm_ois_cfg_data ois_cfg_data = {};
// open devices
const char *sensor_dev;
@@ -746,45 +432,19 @@ static void camera_open(CameraState *s, bool is_road_cam) {
assert(s->csid_fd >= 0);
s->csiphy_fd = open("/dev/v4l-subdev0", O_RDWR | O_NONBLOCK);
assert(s->csiphy_fd >= 0);
- if (s->device == DEVICE_LP3) {
- sensor_dev = "/dev/v4l-subdev17";
- } else {
- sensor_dev = "/dev/v4l-subdev18";
- }
- if (s->device == DEVICE_LP3) {
- s->isp_fd = open("/dev/v4l-subdev13", O_RDWR | O_NONBLOCK);
- } else {
- s->isp_fd = open("/dev/v4l-subdev14", O_RDWR | O_NONBLOCK);
- }
+ sensor_dev = "/dev/v4l-subdev17";
+ s->isp_fd = open("/dev/v4l-subdev13", O_RDWR | O_NONBLOCK);
assert(s->isp_fd >= 0);
- s->eeprom_fd = open("/dev/v4l-subdev8", O_RDWR | O_NONBLOCK);
- assert(s->eeprom_fd >= 0);
-
s->actuator_fd = open("/dev/v4l-subdev7", O_RDWR | O_NONBLOCK);
assert(s->actuator_fd >= 0);
-
- if (s->device != DEVICE_LP3) {
- s->ois_fd = open("/dev/v4l-subdev10", O_RDWR | O_NONBLOCK);
- assert(s->ois_fd >= 0);
- }
} else {
s->csid_fd = open("/dev/v4l-subdev5", O_RDWR | O_NONBLOCK);
assert(s->csid_fd >= 0);
s->csiphy_fd = open("/dev/v4l-subdev2", O_RDWR | O_NONBLOCK);
assert(s->csiphy_fd >= 0);
- if (s->device == DEVICE_LP3) {
- sensor_dev = "/dev/v4l-subdev18";
- } else {
- sensor_dev = "/dev/v4l-subdev19";
- }
- if (s->device == DEVICE_LP3) {
- s->isp_fd = open("/dev/v4l-subdev14", O_RDWR | O_NONBLOCK);
- } else {
- s->isp_fd = open("/dev/v4l-subdev15", O_RDWR | O_NONBLOCK);
- }
+ sensor_dev = "/dev/v4l-subdev18";
+ s->isp_fd = open("/dev/v4l-subdev14", O_RDWR | O_NONBLOCK);
assert(s->isp_fd >= 0);
- s->eeprom_fd = open("/dev/v4l-subdev9", O_RDWR | O_NONBLOCK);
- assert(s->eeprom_fd >= 0);
}
// wait for sensor device
@@ -803,7 +463,7 @@ static void camera_open(CameraState *s, bool is_road_cam) {
struct msm_camera_csi_lane_params csi_lane_params = {0};
csi_lane_params.csi_lane_mask = 0x1f;
csiphy_cfg_data csiphy_cfg_data = { .cfg.csi_lane_params = &csi_lane_params, .cfgtype = CSIPHY_RELEASE};
- err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data);
+ int err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data);
LOG("release csiphy: %d", err);
// CSID: release csid
@@ -816,13 +476,6 @@ static void camera_open(CameraState *s, bool is_road_cam) {
err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data);
LOG("sensor power down: %d", err);
- if (is_road_cam && s->device != DEVICE_LP3) {
- // ois powerdown
- ois_cfg_data.cfgtype = CFG_OIS_POWERDOWN;
- err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data);
- LOG("ois powerdown: %d", err);
- }
-
// actuator powerdown
actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERDOWN;
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
@@ -853,14 +506,6 @@ static void camera_open(CameraState *s, bool is_road_cam) {
// **** GO GO GO ****
LOG("******************** GO GO GO ************************");
- s->eeprom = get_eeprom(s->eeprom_fd, &s->eeprom_size);
-
- // printf("eeprom:\n");
- // for (int i=0; ieeprom_size; i++) {
- // printf("%02x", s->eeprom[i]);
- // }
- // printf("\n");
-
// CSID: init csid
csid_cfg_data.cfgtype = CSID_INIT;
err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data);
@@ -894,10 +539,6 @@ static void camera_open(CameraState *s, bool is_road_cam) {
// SENSOR: send i2c configuration
if (s->camera_id == CAMERA_ID_IMX298) {
err = sensor_write_regs(s, init_array_imx298, ARRAYSIZE(init_array_imx298), MSM_CAMERA_I2C_BYTE_DATA);
- } else if (s->camera_id == CAMERA_ID_S5K3P8SP) {
- err = sensor_write_regs(s, init_array_s5k3p8sp, ARRAYSIZE(init_array_s5k3p8sp), MSM_CAMERA_I2C_WORD_DATA);
- } else if (s->camera_id == CAMERA_ID_IMX179) {
- err = sensor_write_regs(s, init_array_imx179, ARRAYSIZE(init_array_imx179), MSM_CAMERA_I2C_BYTE_DATA);
} else if (s->camera_id == CAMERA_ID_OV8865) {
err = sensor_write_regs(s, init_array_ov8865, ARRAYSIZE(init_array_ov8865), MSM_CAMERA_I2C_BYTE_DATA);
} else {
@@ -915,143 +556,57 @@ static void camera_open(CameraState *s, bool is_road_cam) {
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
LOG("actuator init: %d", err);
+ struct msm_actuator_reg_params_t actuator_reg_params[] = {
+ {
+ .reg_write_type = MSM_ACTUATOR_WRITE_DAC,
+ // MSB here at address 3
+ .reg_addr = 3,
+ .data_type = 9,
+ .addr_type = 4,
+ },
+ };
- // no OIS in LP3
- if (s->device != DEVICE_LP3) {
- // see sony_imx298_eeprom_format_afdata in libmmcamera_sony_imx298_eeprom.so
- const float far_margin = -0.28;
- uint16_t macro_dac = *(uint16_t*)(s->eeprom + 0x24);
- s->infinity_dac = *(uint16_t*)(s->eeprom + 0x26);
- LOG("macro_dac: %d infinity_dac: %d", macro_dac, s->infinity_dac);
+ struct reg_settings_t actuator_init_settings[] = {
+ { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // PD = power down
+ { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // 0 = power up
+ { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // RING = SAC mode
+ { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // 0x40 = SAC3 mode
+ { .reg_addr=7, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=113, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },
+ // 0x71 = DIV1 | DIV0 | SACT0 -- Tvib x 1/4 (quarter)
+ // SAC Tvib = 6.3 ms + 0.1 ms = 6.4 ms / 4 = 1.6 ms
+ // LSC 1-step = 252 + 1*4 = 256 ms / 4 = 64 ms
+ };
- int dac_range = macro_dac - s->infinity_dac;
- s->infinity_dac += far_margin * dac_range;
+ struct region_params_t region_params[] = {
+ {.step_bound = {238, 0,}, .code_per_step = 235, .qvalue = 128}
+ };
- LOG(" -> macro_dac: %d infinity_dac: %d", macro_dac, s->infinity_dac);
+ actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO;
+ actuator_cfg_data.cfg.set_info = (struct msm_actuator_set_info_t){
+ .actuator_params = {
+ .act_type = ACTUATOR_BIVCM,
+ .reg_tbl_size = 1,
+ .data_size = 10,
+ .init_setting_size = 5,
+ .i2c_freq_mode = I2C_STANDARD_MODE,
+ .i2c_addr = 24,
+ .i2c_addr_type = MSM_ACTUATOR_BYTE_ADDR,
+ .i2c_data_type = MSM_ACTUATOR_WORD_DATA,
+ .reg_tbl_params = &actuator_reg_params[0],
+ .init_settings = &actuator_init_settings[0],
+ .park_lens = {.damping_step = 1023, .damping_delay = 14000, .hw_params = 11, .max_step = 20},
+ },
+ .af_tuning_params = {
+ .initial_code = INFINITY_DAC,
+ .pwd_step = 0,
+ .region_size = 1,
+ .total_steps = 238,
+ .region_params = ®ion_params[0],
+ },
+ };
- struct msm_actuator_reg_params_t actuator_reg_params[] = {
- {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 240, .data_type = 10, .addr_type = 4},
- {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 241, .data_type = 10, .addr_type = 4},
- {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 242, .data_type = 10, .addr_type = 4},
- {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 243, .data_type = 10, .addr_type = 4},
- };
-
- //...
- struct reg_settings_t actuator_init_settings[1] = {0};
-
- struct region_params_t region_params[] = {
- {.step_bound = {512, 0,}, .code_per_step = 118, .qvalue = 128}
- };
-
- actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO;
- actuator_cfg_data.cfg.set_info = (struct msm_actuator_set_info_t){
- .actuator_params = {
- .act_type = ACTUATOR_VCM,
- .reg_tbl_size = 4,
- .data_size = 10,
- .init_setting_size = 0,
- .i2c_freq_mode = I2C_CUSTOM_MODE,
- .i2c_addr = 28,
- .i2c_addr_type = MSM_ACTUATOR_BYTE_ADDR,
- .i2c_data_type = MSM_ACTUATOR_BYTE_DATA,
- .reg_tbl_params = &actuator_reg_params[0],
- .init_settings = &actuator_init_settings[0],
- .park_lens = {
- .damping_step = 1023,
- .damping_delay = 15000,
- .hw_params = 58404,
- .max_step = 20,
- }
- },
- .af_tuning_params = {
- .initial_code = (int16_t)s->infinity_dac,
- .pwd_step = 0,
- .region_size = 1,
- .total_steps = 512,
- .region_params = ®ion_params[0],
- },
- };
- err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
- LOG("actuator set info: %d", err);
-
- // power up ois
- ois_cfg_data.cfgtype = CFG_OIS_POWERUP;
- err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data);
- LOG("ois powerup: %d", err);
-
- ois_cfg_data.cfgtype = CFG_OIS_INIT;
- err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data);
- LOG("ois init: %d", err);
-
- ois_cfg_data.cfgtype = CFG_OIS_CONTROL;
- ois_cfg_data.cfg.set_info.ois_params = (struct msm_ois_params_t){
- // .data_size = 26312,
- .setting_size = 120,
- .i2c_addr = 28,
- .i2c_freq_mode = I2C_CUSTOM_MODE,
- // .i2c_addr_type = wtf
- // .i2c_data_type = wtf
- .settings = &ois_init_settings[0],
- };
- err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data);
- LOG("ois init settings: %d", err);
- } else {
- // leeco actuator (DW9800W H-Bridge Driver IC)
- // from sniff
- s->infinity_dac = 364;
-
- struct msm_actuator_reg_params_t actuator_reg_params[] = {
- {
- .reg_write_type = MSM_ACTUATOR_WRITE_DAC,
- // MSB here at address 3
- .reg_addr = 3,
- .data_type = 9,
- .addr_type = 4,
- },
- };
-
- struct reg_settings_t actuator_init_settings[] = {
- { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // PD = power down
- { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // 0 = power up
- { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // RING = SAC mode
- { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // 0x40 = SAC3 mode
- { .reg_addr=7, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=113, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },
- // 0x71 = DIV1 | DIV0 | SACT0 -- Tvib x 1/4 (quarter)
- // SAC Tvib = 6.3 ms + 0.1 ms = 6.4 ms / 4 = 1.6 ms
- // LSC 1-step = 252 + 1*4 = 256 ms / 4 = 64 ms
- };
-
- struct region_params_t region_params[] = {
- {.step_bound = {238, 0,}, .code_per_step = 235, .qvalue = 128}
- };
-
- actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO;
- actuator_cfg_data.cfg.set_info = (struct msm_actuator_set_info_t){
- .actuator_params = {
- .act_type = ACTUATOR_BIVCM,
- .reg_tbl_size = 1,
- .data_size = 10,
- .init_setting_size = 5,
- .i2c_freq_mode = I2C_STANDARD_MODE,
- .i2c_addr = 24,
- .i2c_addr_type = MSM_ACTUATOR_BYTE_ADDR,
- .i2c_data_type = MSM_ACTUATOR_WORD_DATA,
- .reg_tbl_params = &actuator_reg_params[0],
- .init_settings = &actuator_init_settings[0],
- .park_lens = {.damping_step = 1023, .damping_delay = 14000, .hw_params = 11, .max_step = 20},
- },
- .af_tuning_params = {
- .initial_code = (int16_t)s->infinity_dac,
- .pwd_step = 0,
- .region_size = 1,
- .total_steps = 238,
- .region_params = ®ion_params[0],
- },
- };
-
- err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
- LOG("actuator set info: %d", err);
- }
+ err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
+ LOG("actuator set info: %d", err);
}
if (s->camera_id == CAMERA_ID_IMX298) {
@@ -1063,10 +618,6 @@ static void camera_open(CameraState *s, bool is_road_cam) {
struct msm_camera_csiphy_params csiphy_params = {};
if (s->camera_id == CAMERA_ID_IMX298) {
csiphy_params = {.lane_cnt = 4, .settle_cnt = 14, .lane_mask = 0x1f, .csid_core = 0};
- } else if (s->camera_id == CAMERA_ID_S5K3P8SP) {
- csiphy_params = {.lane_cnt = 4, .settle_cnt = 24, .lane_mask = 0x1f, .csid_core = 0};
- } else if (s->camera_id == CAMERA_ID_IMX179) {
- csiphy_params = {.lane_cnt = 4, .settle_cnt = 11, .lane_mask = 0x1f, .csid_core = 2};
} else if (s->camera_id == CAMERA_ID_OV8865) {
// guess!
csiphy_params = {.lane_cnt = 4, .settle_cnt = 24, .lane_mask = 0x1f, .csid_core = 2};
@@ -1204,49 +755,24 @@ static void camera_open(CameraState *s, bool is_road_cam) {
LOG("isp start stream: %d", err);
}
-
-static struct damping_params_t actuator_ringing_params = {
- .damping_step = 1023,
- .damping_delay = 15000,
- .hw_params = 0x0000e422,
-};
-
static void road_camera_start(CameraState *s) {
- struct msm_actuator_cfg_data actuator_cfg_data = {0};
-
set_exposure(s, 1.0, 1.0);
int err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA);
LOG("sensor start regs: %d", err);
- // focus on infinity assuming phone is perpendicular
- int inf_step;
+ int inf_step = 512 - INFINITY_DAC;
- if (s->device != DEVICE_LP3) {
- imx298_ois_calibration(s->ois_fd, s->eeprom);
- inf_step = 332 - s->infinity_dac;
-
- // initial guess
- s->lens_true_pos = 300;
- } else {
- // default is OP3, this is for LeEco
- actuator_ringing_params.damping_step = 1023;
- actuator_ringing_params.damping_delay = 20000;
- actuator_ringing_params.hw_params = 13;
-
- inf_step = 512 - s->infinity_dac;
-
- // initial guess
- s->lens_true_pos = 400;
- }
+ // initial guess
+ s->lens_true_pos = 400;
// reset lens position
- memset(&actuator_cfg_data, 0, sizeof(actuator_cfg_data));
+ struct msm_actuator_cfg_data actuator_cfg_data = {};
actuator_cfg_data.cfgtype = CFG_SET_POSITION;
actuator_cfg_data.cfg.setpos = (struct msm_actuator_set_position_t){
.number_of_steps = 1,
- .hw_params = (uint32_t)((s->device != DEVICE_LP3) ? 0x0000e424 : 7),
- .pos = {s->infinity_dac, 0},
+ .hw_params = (uint32_t)7,
+ .pos = {INFINITY_DAC, 0},
.delay = {0,}
};
err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
@@ -1271,16 +797,20 @@ static void road_camera_start(CameraState *s) {
s->cur_step_pos = inf_step;
actuator_move(s, s->cur_lens_pos);
-
LOG("init lens pos: %d", s->cur_lens_pos);
}
void actuator_move(CameraState *s, uint16_t target) {
- int step = target - s->cur_lens_pos;
// LP3 moves only on even positions. TODO: use proper sensor params
- if (s->device == DEVICE_LP3) {
- step /= 2;
- }
+
+ // focus on infinity assuming phone is perpendicular
+ static struct damping_params_t actuator_ringing_params = {
+ .damping_step = 1023,
+ .damping_delay = 20000,
+ .hw_params = 13,
+ };
+
+ int step = (target - s->cur_lens_pos) / 2;
int dest_step_pos = s->cur_step_pos + step;
dest_step_pos = std::clamp(dest_step_pos, 0, 255);
@@ -1288,19 +818,17 @@ void actuator_move(CameraState *s, uint16_t target) {
struct msm_actuator_cfg_data actuator_cfg_data = {0};
actuator_cfg_data.cfgtype = CFG_MOVE_FOCUS;
actuator_cfg_data.cfg.move = (struct msm_actuator_move_params_t){
- .dir = (int8_t)((step > 0) ? 0 : 1),
- .sign_dir = (int8_t)((step > 0) ? 1 : -1),
+ .dir = (int8_t)((step > 0) ? MOVE_NEAR : MOVE_FAR),
+ .sign_dir = (int8_t)((step > 0) ? MSM_ACTUATOR_MOVE_SIGNED_NEAR : MSM_ACTUATOR_MOVE_SIGNED_FAR),
.dest_step_pos = (int16_t)dest_step_pos,
.num_steps = abs(step),
.curr_lens_pos = s->cur_lens_pos,
.ringing_params = &actuator_ringing_params,
};
- int err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
- LOG("actuator move focus: %d", err);
+ ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data);
s->cur_step_pos = dest_step_pos;
s->cur_lens_pos = actuator_cfg_data.cfg.move.curr_lens_pos;
-
//LOGD("step %d target: %d lens pos: %d", dest_step_pos, target, s->cur_lens_pos);
}
@@ -1366,10 +894,6 @@ static std::optional get_accel_z(SubMaster *sm) {
}
static void do_autofocus(CameraState *s, SubMaster *sm) {
- // params for focus PI controller
- const int dac_up = s->device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP;
- const int dac_down = s->device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN;
-
float lens_true_pos = s->lens_true_pos.load();
if (!isnan(s->focus_err)) {
// learn lens_true_pos
@@ -1382,8 +906,8 @@ static void do_autofocus(CameraState *s, SubMaster *sm) {
}
const float sag = (s->last_sag_acc_z / 9.8) * 128;
// stay off the walls
- lens_true_pos = std::clamp(lens_true_pos, float(dac_down), float(dac_up));
- int target = std::clamp(lens_true_pos - sag, float(dac_down), float(dac_up));
+ lens_true_pos = std::clamp(lens_true_pos, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
+ int target = std::clamp(lens_true_pos - sag, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
s->lens_true_pos.store(lens_true_pos);
/*char debug[4096];
@@ -1425,7 +949,7 @@ void cameras_open(MultiCameraState *s) {
// driver camera
{.vfe_intf = VFE1, .intftype = RDI0, .num_cids = 1, .cids[0] = CID0, .csid = CSID2},
// road camera (focus)
- {.vfe_intf = VFE0, .intftype = RDI1, .num_cids = CID1, .cids[0] = CID1, .csid = CSID0},
+ {.vfe_intf = VFE0, .intftype = RDI1, .num_cids = 1, .cids[0] = CID1, .csid = CSID0},
// road camera (stats, for AE)
{.vfe_intf = VFE0, .intftype = RDI2, .num_cids = 1, .cids[0] = CID2, .csid = CSID0},
},
@@ -1438,11 +962,7 @@ void cameras_open(MultiCameraState *s) {
s->v4l_fd = open("/dev/video0", O_RDWR | O_NONBLOCK);
assert(s->v4l_fd >= 0);
- if (s->device == DEVICE_LP3) {
- s->ispif_fd = open("/dev/v4l-subdev15", O_RDWR | O_NONBLOCK);
- } else {
- s->ispif_fd = open("/dev/v4l-subdev16", O_RDWR | O_NONBLOCK);
- }
+ s->ispif_fd = open("/dev/v4l-subdev15", O_RDWR | O_NONBLOCK);
assert(s->ispif_fd >= 0);
// ISPIF: stop
@@ -1509,12 +1029,9 @@ static void camera_close(CameraState *s) {
LOG("isp release stream: %d", err);
}
}
-
- free(s->eeprom);
}
-
-const char* get_isp_event_name(unsigned int type) {
+const char* get_isp_event_name(uint32_t type) {
switch (type) {
case ISP_EVENT_REG_UPDATE: return "ISP_EVENT_REG_UPDATE";
case ISP_EVENT_EPOCH_0: return "ISP_EVENT_EPOCH_0";
@@ -1542,15 +1059,12 @@ const char* get_isp_event_name(unsigned int type) {
}
static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) {
- pthread_mutex_lock(&s->frame_info_lock);
+ std::lock_guard lk(s->frame_info_lock);
for (auto &i : s->frame_metadata) {
if (i.frame_id == frame_id) {
- pthread_mutex_unlock(&s->frame_info_lock);
return i;
}
}
- pthread_mutex_unlock(&s->frame_info_lock);
-
// should never happen
return (FrameMetadata){
.frame_id = (uint32_t)-1,
@@ -1584,59 +1098,20 @@ static void ops_thread(MultiCameraState *s) {
}
}
-static void update_lapmap(MultiCameraState *s, const CameraBuf *b, const int cnt) {
- const size_t width = b->rgb_width / NUM_SEGMENTS_X;
- const size_t height = b->rgb_height / NUM_SEGMENTS_Y;
- static std::unique_ptr rgb_roi_buf = std::make_unique(width * height * 3);
- static std::unique_ptr conv_result = std::make_unique(width * height);
-
- // sharpness scores
- const int roi_id = cnt % std::size(s->lapres); // rolling roi
- const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1);
- const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1);
-
- const uint8_t *rgb_addr_offset = (uint8_t *)b->cur_rgb_buf->addr + y_offset * height * FULL_STRIDE_X * 3 + x_offset * width * 3;
- for (int i = 0; i < height; ++i) {
- memcpy(rgb_roi_buf.get() + i * width * 3, rgb_addr_offset + i * FULL_STRIDE_X * 3, width * 3);
- }
-
- constexpr int conv_cl_localMemSize = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t));
- CL_CHECK(clEnqueueWriteBuffer(b->q, s->rgb_conv_roi_cl, true, 0, width * height * 3 * sizeof(uint8_t), rgb_roi_buf.get(), 0, 0, 0));
- CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 0, sizeof(cl_mem), (void *)&s->rgb_conv_roi_cl));
- CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 1, sizeof(cl_mem), (void *)&s->rgb_conv_result_cl));
- CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 2, sizeof(cl_mem), (void *)&s->rgb_conv_filter_cl));
- CL_CHECK(clSetKernelArg(s->krnl_rgb_laplacian, 3, conv_cl_localMemSize, 0));
- cl_event conv_event;
- CL_CHECK(clEnqueueNDRangeKernel(b->q, s->krnl_rgb_laplacian, 2, NULL,
- (size_t[]){width, height}, (size_t[]){CONV_LOCAL_WORKSIZE, CONV_LOCAL_WORKSIZE}, 0, 0, &conv_event));
- clWaitForEvents(1, &conv_event);
- CL_CHECK(clReleaseEvent(conv_event));
-
- CL_CHECK(clEnqueueReadBuffer(b->q, s->rgb_conv_result_cl, true, 0,
- width * height * sizeof(int16_t), conv_result.get(), 0, 0, 0));
-
- s->lapres[roi_id] = get_lapmap_one(conv_result.get(), width, height);
-}
-
static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t lapres_size) {
- const int dac_down = c->device == DEVICE_LP3 ? LP3_AF_DAC_DOWN : OP3T_AF_DAC_DOWN;
- const int dac_up = c->device == DEVICE_LP3 ? LP3_AF_DAC_UP : OP3T_AF_DAC_UP;
- const int dac_m = c->device == DEVICE_LP3 ? LP3_AF_DAC_M : OP3T_AF_DAC_M;
- const int dac_3sig = c->device == DEVICE_LP3 ? LP3_AF_DAC_3SIG : OP3T_AF_DAC_3SIG;
-
const float lens_true_pos = c->lens_true_pos.load();
int self_recover = c->self_recover.load();
- if (self_recover < 2 && (lens_true_pos < (dac_down + 1) || lens_true_pos > (dac_up - 1)) && is_blur(lapres, lapres_size)) {
+ if (self_recover < 2 && (lens_true_pos < (LP3_AF_DAC_DOWN + 1) || lens_true_pos > (LP3_AF_DAC_UP - 1)) && is_blur(lapres, lapres_size)) {
// truly stuck, needs help
if (--self_recover < -FOCUS_RECOVER_PATIENCE) {
LOGD("road camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, self_recover);
// parity determined by which end is stuck at
- self_recover = FOCUS_RECOVER_STEPS + (lens_true_pos < dac_m ? 1 : 0);
+ self_recover = FOCUS_RECOVER_STEPS + (lens_true_pos < LP3_AF_DAC_M ? 1 : 0);
}
- } else if (self_recover < 2 && (lens_true_pos < (dac_m - dac_3sig) || lens_true_pos > (dac_m + dac_3sig))) {
+ } else if (self_recover < 2 && (lens_true_pos < (LP3_AF_DAC_M - LP3_AF_DAC_3SIG) || lens_true_pos > (LP3_AF_DAC_M + LP3_AF_DAC_3SIG))) {
// in suboptimal position with high prob, but may still recover by itself
if (--self_recover < -(FOCUS_RECOVER_PATIENCE * 3)) {
- self_recover = FOCUS_RECOVER_STEPS / 2 + (lens_true_pos < dac_m ? 1 : 0);
+ self_recover = FOCUS_RECOVER_STEPS / 2 + (lens_true_pos < LP3_AF_DAC_M ? 1 : 0);
}
} else if (self_recover < 0) {
self_recover += 1; // reset if fine
@@ -1651,7 +1126,8 @@ void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
// called by processing_thread
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
- update_lapmap(s, b, cnt);
+ const int roi_id = cnt % std::size(s->lapres); // rolling roi
+ s->lapres[roi_id] = s->lap_conv->Update(b->q, (uint8_t *)b->cur_rgb_buf->addr, roi_id);
setup_self_recover(c, &s->lapres[0], std::size(s->lapres));
MessageBuilder msg;
@@ -1670,7 +1146,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
if (cnt % 3 == 0) {
const int x = 290, y = 322, width = 560, height = 314;
const int skip = 1;
- set_exposure_target(c, (const uint8_t *)b->cur_yuv_buf->y, x, x + width, skip, y, y + height, skip);
+ camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip, -1, false, false));
}
}
@@ -1719,13 +1195,13 @@ void cameras_run(MultiCameraState *s) {
} else if (ev.type == ISP_EVENT_EOF) {
const uint64_t timestamp = (isp_event_data->mono_timestamp.tv_sec * 1000000000ULL + isp_event_data->mono_timestamp.tv_usec * 1000);
- pthread_mutex_lock(&c->frame_info_lock);
+ std::lock_guard lk(c->frame_info_lock);
c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){
.frame_id = isp_event_data->frame_id,
.timestamp_eof = timestamp,
- .frame_length = (unsigned int)c->cur_frame_length,
- .integ_lines = (unsigned int)c->cur_integ_lines,
- .global_gain = (unsigned int)c->cur_gain,
+ .frame_length = (uint32_t)c->cur_frame_length,
+ .integ_lines = (uint32_t)c->cur_integ_lines,
+ .global_gain = (uint32_t)c->cur_gain,
.lens_pos = c->cur_lens_pos,
.lens_sag = c->last_sag_acc_z,
.lens_err = c->focus_err,
@@ -1733,7 +1209,6 @@ void cameras_run(MultiCameraState *s) {
.gain_frac = c->cur_gain_frac,
};
c->frame_metadata_idx = (c->frame_metadata_idx + 1) % METADATA_BUF_COUNT;
- pthread_mutex_unlock(&c->frame_info_lock);
} else if (ev.type == ISP_EVENT_ERROR) {
LOGE("ISP_EVENT_ERROR! err type: 0x%08x", isp_event_data->u.error_info.err_type);
@@ -1755,12 +1230,8 @@ void cameras_close(MultiCameraState *s) {
s->focus_bufs[i].free();
s->stats_bufs[i].free();
}
- CL_CHECK(clReleaseMemObject(s->rgb_conv_roi_cl));
- CL_CHECK(clReleaseMemObject(s->rgb_conv_result_cl));
- CL_CHECK(clReleaseMemObject(s->rgb_conv_filter_cl));
- CL_CHECK(clReleaseKernel(s->krnl_rgb_laplacian));
- CL_CHECK(clReleaseProgram(s->prg_rgb_laplacian));
+ delete s->lap_conv;
delete s->sm;
delete s->pm;
}
diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h
index 82934ef9..1ab817f4 100644
--- a/selfdrive/camerad/cameras/camera_qcom.h
+++ b/selfdrive/camerad/cameras/camera_qcom.h
@@ -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 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 focus_err;
-
- uint16_t cur_step_pos;
- uint16_t cur_lens_pos;
std::atomic last_sag_acc_z;
std::atomic lens_true_pos;
-
std::atomic 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);
diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc
index 3fef9267..ae81db6b 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/selfdrive/camerad/cameras/camera_qcom2.cc
@@ -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));
}
}
diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h
index 104ba49d..ada2744e 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.h
+++ b/selfdrive/camerad/cameras/camera_qcom2.h
@@ -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);
diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h
index 8abec280..6fb771b0 100644
--- a/selfdrive/camerad/cameras/sensor2_i2c.h
+++ b/selfdrive/camerad/cameras/sensor2_i2c.h
@@ -1,79 +1,29 @@
-struct i2c_random_wr_payload start_reg_array[] = {{0x301a, 0x91c}};
-struct i2c_random_wr_payload stop_reg_array[] = {{0x301a, 0x918}};;
+struct i2c_random_wr_payload start_reg_array[] = {{0x301A, 0x91C}};
+struct i2c_random_wr_payload stop_reg_array[] = {{0x301A, 0x918}};
struct i2c_random_wr_payload init_array_ar0231[] = {
{0x301A, 0x0018}, // RESET_REGISTER
- {0x3092, 0x0C24}, // ROW_NOISE_CONTROL
- {0x337A, 0x0C80}, // DBLC_SCALE0
- {0x3520, 0x1288}, // RESERVED_MFR_3520
- {0x3522, 0x880C}, // RESERVED_MFR_3522
- {0x3524, 0x0C12}, // RESERVED_MFR_3524
- {0x352C, 0x1212}, // RESERVED_MFR_352C
- {0x354A, 0x007F}, // RESERVED_MFR_354A
- {0x350C, 0x055C}, // RESERVED_MFR_350C
- {0x3506, 0x3333}, // RESERVED_MFR_3506
- {0x3508, 0x3333}, // RESERVED_MFR_3508
- {0x3100, 0x4000}, // DLO_CONTROL0
- {0x3280, 0x0CCC}, // RESERVED_MFR_3280
- {0x3282, 0x0CCC}, // RESERVED_MFR_3282
- {0x3284, 0x0CCC}, // RESERVED_MFR_3284
- {0x3286, 0x0CCC}, // RESERVED_MFR_3286
- {0x3288, 0x0FA0}, // RESERVED_MFR_3288
- {0x328A, 0x0FA0}, // RESERVED_MFR_328A
- {0x328C, 0x0FA0}, // RESERVED_MFR_328C
- {0x328E, 0x0FA0}, // RESERVED_MFR_328E
- {0x3290, 0x0FA0}, // RESERVED_MFR_3290
- {0x3292, 0x0FA0}, // RESERVED_MFR_3292
- {0x3294, 0x0FA0}, // RESERVED_MFR_3294
- {0x3296, 0x0FA0}, // RESERVED_MFR_3296
- {0x3298, 0x0FA0}, // RESERVED_MFR_3298
- {0x329A, 0x0FA0}, // RESERVED_MFR_329A
- {0x329C, 0x0FA0}, // RESERVED_MFR_329C
- {0x329E, 0x0FA0}, // RESERVED_MFR_329E
- {0x32E6, 0x00E0}, // RESERVED_MFR_32E6
- {0x1008, 0x036F}, // RESERVED_PARAM_1008
- {0x100C, 0x058F}, // RESERVED_PARAM_100C
- {0x100E, 0x07AF}, // RESERVED_PARAM_100E
- {0x1010, 0x014F}, // RESERVED_PARAM_1010
- {0x3230, 0x0312}, // FINE_CORRECTION
- {0x3232, 0x0532}, // FINE_CORRECTION2
- {0x3234, 0x0752}, // FINE_CORRECTION3
- {0x3236, 0x00F2}, // FINE_CORRECTION4
- {0x3566, 0x3328}, // RESERVED_MFR_3566
- {0x32D0, 0x3A02}, // RESERVED_MFR_32D0
- {0x32D2, 0x3508}, // RESERVED_MFR_32D2
- {0x32D4, 0x3702}, // RESERVED_MFR_32D4
- {0x32D6, 0x3C04}, // RESERVED_MFR_32D6
- {0x32DC, 0x370A}, // RESERVED_MFR_32DC
- {0x30B0, 0x0800}, // DIGITAL_TEST
+
+ // CLOCK Settings
{0x302A, 0x0006}, // VT_PIX_CLK_DIV
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
{0x3030, 0x0028}, // PLL_MULTIPLIER
{0x3036, 0x000A}, // OP_WORD_CLK_DIV
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
- {0x30B0, 0x0800}, // DIGITAL_TEST
+
+ // FORMAT
+ {0x3040, 0xC000}, // READ_MODE
+ {0x3004, 0x0000}, // X_ADDR_START_
+ {0x3008, 0x0787}, // X_ADDR_END_
+ {0x3002, 0x0000}, // Y_ADDR_START_
+ {0x3006, 0x04B7}, // Y_ADDR_END_
+ {0x3032, 0x0000}, // SCALING_MODE
{0x30A2, 0x0001}, // X_ODD_INC_
{0x30A6, 0x0001}, // Y_ODD_INC_
- {0x3040, 0xC000}, // READ_MODE C000
- {0x30BA, 0x11F2}, // DIGITAL_CTRL
- {0x3044, 0x0400}, // DARK_CONTROL
- {0x3064, 0x1802}, // SMIA_TEST
- {0x33E0, 0x0C80}, // TEST_ASIL_ROWS
- {0x3180, 0x0080}, // RESERVED_MFR_3180
- {0x33E4, 0x0080}, // RESERVED_MFR_33E4
- {0x33E0, 0x0C80}, // TEST_ASIL_ROWS
- {0x33E0, 0x0C80}, // TEST_ASIL_ROWS
- {0x3004, 0x0000}, // X_ADDR_START_
- {0x3008, 0x0787}, // X_ADDR_END_ 787
- {0x3002, 0x0000}, // Y_ADDR_START_
- {0x3006, 0x04B7}, // Y_ADDR_END_ 4B7
- {0x3032, 0x0000}, // SCALING_MODE
- {0x3400, 0x0010}, // RESERVED_MFR_3400
- {0x3402, 0x0788}, // X_OUTPUT_CONTROL
{0x3402, 0x0F10}, // X_OUTPUT_CONTROL
- {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
{0x3404, 0x0970}, // Y_OUTPUT_CONTROL
+ {0x3064, 0x1802}, // SMIA_TEST
{0x30BA, 0x11F2}, // DIGITAL_CTRL
// SLAV* MODE
@@ -82,8 +32,8 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
{0x340C, 0x802}, // 2 // 0000 0000 0010
// Readout timing
- {0x300C, 0x074B}, // LINE_LENGTH_PCK: min for 2-exposure HDR
- {0x300A, 0x085E}, // FRAME_LENGTH_LINES_ 6EB
+ {0x300C, 0x07B9}, // LINE_LENGTH_PCK
+ {0x300A, 0x07E7}, // FRAME_LENGTH_LINES
{0x3042, 0x0000}, // EXTRA_DELAY
// Readout Settings
@@ -97,36 +47,44 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
{0x3348, 0x0111}, // MIPI_F2_VDT_VC
{0x334C, 0x0211}, // MIPI_F3_VDT_VC
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
- {0x31B0, 0x0049}, // FRAME_PREAMBLE
- {0x31B2, 0x0033}, // LINE_PREAMBLE
- {0x31B4, 0x2185}, // RESERVED_MFR_31B4
- {0x31B6, 0x1146}, // RESERVED_MFR_31B6
- {0x31B8, 0x3047}, // RESERVED_MFR_31B8
- {0x31BA, 0x0186}, // RESERVED_MFR_31BA
- {0x31BC, 0x0805}, // RESERVED_MFR_31BC
+ {0x31B0, 0x0053}, // FRAME_PREAMBLE
+ {0x31B2, 0x003B}, // LINE_PREAMBLE
{0x301A, 0x01C}, // RESET_REGISTER
+ // Noise Corrections
+ {0x3092, 0x0C24}, // ROW_NOISE_CONTROL
+ {0x337A, 0x0C80}, // DBLC_SCALE0
+ {0x3370, 0x03B1}, // DBLC
+ {0x3044, 0x0400}, // DARK_CONTROL
+ {0x31E0, 0x0001}, // PDC
+
// HDR Settings
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
{0x3238, 0x0004}, // EXPOSURE_RATIO
{0x3014, 0x098E}, // FINE_INTEGRATION_TIME_
{0x321E, 0x098E}, // FINE_INTEGRATION_TIME2
- {0x30B0, 0x0800}, // DIGITAL_TEST
- {0x32EA, 0x3C0E}, // RESERVED_MFR_32EA
- {0x32EC, 0x72A1}, // RESERVED_MFR_32EC
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
{0x33DA, 0x0000}, // COMPANDING
- {0x3370, 0x03B1}, // DBLC
- {0x31E0, 0x0001}, // PDC
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
+ // DLO Settings
+ {0x3100, 0x4000}, // DLO_CONTROL0
+ {0x3280, 0x0CCC}, // T1 G1
+ {0x3282, 0x0CCC}, // T1 R
+ {0x3284, 0x0CCC}, // T1 B
+ {0x3286, 0x0CCC}, // T1 G2
+ {0x3288, 0x0FA0}, // T2 G1
+ {0x328A, 0x0FA0}, // T2 R
+ {0x328C, 0x0FA0}, // T2 B
+ {0x328E, 0x0FA0}, // T2 G2
+
// Initial Gains
{0x3022, 0x01}, // GROUPED_PARAMETER_HOLD_
{0x3366, 0x5555}, // ANALOG_GAIN
{0x3060, 0x3333}, // ANALOG_COLOR_GAIN
{0x3362, 0x0000}, // DC GAIN
- {0x305A, 0x00D8}, // RED_GAIN
- {0x3058, 0x011B}, // BLUE_GAIN
+ {0x305A, 0x0108}, // RED_GAIN
+ {0x3058, 0x00FB}, // BLUE_GAIN
{0x3056, 0x009A}, // GREEN1_GAIN
{0x305C, 0x009A}, // GREEN2_GAIN
{0x3022, 0x00}, // GROUPED_PARAMETER_HOLD_
@@ -134,4 +92,3 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
// Initial Integration Time
{0x3012, 0x256},
};
-
diff --git a/selfdrive/camerad/cameras/sensor_i2c.h b/selfdrive/camerad/cameras/sensor_i2c.h
index 74f56b02..b46ebb37 100644
--- a/selfdrive/camerad/cameras/sensor_i2c.h
+++ b/selfdrive/camerad/cameras/sensor_i2c.h
@@ -594,1165 +594,6 @@ static struct msm_camera_i2c_reg_array start_reg_array[] = {{0x100,0x1,0}};
// stop, enable standby mode
static struct msm_camera_i2c_reg_array stop_reg_array[] = {{0x100,0x0,0}};
-
-///////////////////
-
-
-static struct msm_camera_i2c_reg_array init_array_imx179[] = {
- { 0x100, 0x0, 0}, // MODE_SELECT
- { 0x101, 0x0, 0}, // IMAGE_ORIENT
- { 0x202, 0x9, 0}, { 0x203, 0xd2, 0}, // COARSE_INTEGRATION_TIME
- { 0x301, 0x5, 0}, // vt_pix_clk_div
- { 0x303, 0x1, 0}, // vt_sys_clk_div
- { 0x305, 0x6, 0}, // pre_pll_clk_div
- { 0x309, 0x5, 0}, // op_pix_clk_div
- { 0x30b, 0x1, 0}, // op_sys_clk_div
- { 0x30c, 0x0, 0}, { 0x30d, 0x9d, 0},
-
- { 0x340, 0x9, 0}, { 0x341, 0xd6, 0}, // frame_length_lines
- { 0x342, 0xd, 0}, { 0x343, 0x70, 0}, // line_length_pclk
- { 0x344, 0x0, 0}, { 0x345, 0x0, 0}, // x_addr_start
- { 0x346, 0x0, 0}, { 0x347, 0x0, 0}, // y_addr_start
- { 0x348, 0xc, 0}, { 0x349, 0xcf, 0}, // last_pixel / x_addr_end
- { 0x34a, 0x9, 0}, { 0x34b, 0x9f, 0}, // last_line / y_addr_end
- { 0x34c, 0xc, 0}, { 0x34d, 0xd0, 0}, // pixels_per_line / x_output_size
- { 0x34e, 0x9, 0}, { 0x34f, 0xa0, 0}, // lines_per_frame / y_output_size
- { 0x383, 0x1, 0}, // x_odd_inc
- { 0x387, 0x1, 0}, // y_odd_inc
- { 0x390, 0x0, 0}, // binning_mode
- { 0x401, 0x0, 0}, // SCALING_MODE
- { 0x405, 0x10, 0}, // SCALE_M
-
- {0x3020, 0x10, 0},
- {0x3041, 0x15, 0}, // READ_MODE?
- {0x3042, 0x87, 0},
- {0x3089, 0x4f, 0},
- {0x3309, 0x9a, 0},
- {0x3344, 0x57, 0},
- {0x3345, 0x1f, 0},
- {0x3362, 0xa, 0},
- {0x3363, 0xa, 0},
- {0x3364, 0x0, 0},
- {0x3368, 0x18, 0},
- {0x3369, 0x0, 0},
- {0x3370, 0x6f, 0},
- {0x3371, 0x27, 0},
- {0x3372, 0x4f, 0},
- {0x3373, 0x2f, 0},
- {0x3374, 0x27, 0},
- {0x3375, 0x2f, 0},
- {0x3376, 0x97, 0},
- {0x3377, 0x37, 0},
- {0x33c8, 0x0, 0},
- {0x33d4, 0xc, 0},
- {0x33d5, 0xd0, 0},
- {0x33d6, 0x9, 0},
- {0x33d7, 0xa0, 0},
- // znr
- {0x4100, 0xe, 0},
- {0x4108, 0x1, 0},
- {0x4109, 0x7c, 0},
-};
-
-
-
-/////////////// ois stuff ///////////////
-
-/*
-#define _OP_FIRM_DWNLD 0x80
-#define _OP_Periphe_RW 0x82
-#define _OP_Memory__RW 0x84
-#define _OP_AD_TRNSFER 0x86
-#define _OP_COEF_DWNLD 0x88
-#define _OP_PrgMem__RD 0x8A
-#define _OP_SpecialCMD 0x8C
-*/
-
-static struct reg_settings_ois_t ois_init_settings[] = {
- {
- .reg_addr = 0x8262,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0xbf03,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8263,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x9f05,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8264,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x6040,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8260,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x1130,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8265,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x8000,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8261,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0280,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8261,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0380,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8261,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0988,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x34\x84\x00\x03\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x03\x00\x10\x7e\x84\x50\x00\x08\x40\x7e\xa0\x00\x03\x00\x10\x7e\x84\x60\x00\x08\x40\x7e\xa0\x00\x03\x00\x90\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x10\x08\x80\x00\xa0\x10\x00\x08\x7f\xff\x11\x8f\x02\x07\x80\x00\x11\x40\xff\xa0\x90\x01\x84\x20\x8f\x08\x40\xfe\x90\x40\xf5",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x08\x80\x01\xa0\x00\x01\x11\x8f\x02\x07\xff\xff\x11\x08\x00\x20\x50\x12\x07\x00\x10\x08\x80\x00\xa0\x10\xff\x84\x20\x0a",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x7f\xff\x21\x08\xfe\x84\x00\x04\x07\x20\x0c\x08\x7f\xff\x21\x08\xfe\x84\x00\x03\x00\x90\x17\x84\x20\x1f\x08\x80\x17\xa0\x10\x10",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x7f\xff\x21\x10\x00\x08\x01\x00\x11\x40\x51\xa0\x90\x17\x84\x20\x0f\x08\x80\x47\xa0\x8d\x0c\x07\x00\x00\x11\x30\x03\x07\x80\x41",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8090,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x50\x00\x08\x40\xfc\x90\x88\x2f\x84\x00\x00\x11\x30\x02\x07\x40\xff\x90\x50\x00\x08\x40\xfd\x90\x40\x7f\xa0\x10\xff\x84\x20\x2c",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x7f\xff\x11\x20\x0d\x08\x80\x0f\x90\x80\x26\xa0\x90\x2e\x84\x00\x10\x08\x90\x26\x84\x00\x10\x08\x80\x1f\xa0\x20\x2e\x08\x40\xed",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8090,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x20\x0f\x08\x80\x0e\x90\x00\x00\x21\x30\x02\x07\x40\xeb\xa0\x50\x00\x08\x40\xfe\x90\x40\x7f\xa0\x04\xeb\x84\x10\x00\x20\x20\x0f",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x80\x00\x21\x60\x04\x07\x40\xff\xa0\x10\x00\x08\x40\xea\x90\x10\x00\x20\x20\x0f\x08\x80\x00\x11\x00\x0b\x07\x08\x00\x20\x60\x0d",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x00\x08\x40\xea\x90\x8f\x06\x07\x04\xff\x84\x20\x09\x60\x10\xfc\x84\x08\xfd\x84\x04\xfe\x84\x00\x03\x00\x00\x10\x08\x80\x37",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x90\x69\x84\x00\x10\x08\x80\x69\xa0\x20\x1a\x08\x40\x64\xa0\x10\x10\x08\x80\x4f\xa0\x20\x1d\x08\x40\x5f\xa0\x20\x1e\x08\x40\x5d",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x00\x08\x80\x68\x90\x40\xfe\xa0\x20\x0e\x07\x50\x00\x08\x40\x7f\xa0\x04\xfe\x84\x00\x04\x00\x10\xf0\x44\x50\x00\x08\x00\x7f",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x20\xf0\x60\x04\x5c\x84\x04\x61\x84\x04\x57\x84\x00\x00\x21\x04\x74\x84\x00\x40\x21\x00\x0b\x07\x04\x74\x84\x00\x00\x21\x10\x57",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x30\x20\x08\x00\x04\x11\x50\x00\x08\x03\xff\x11\x20\xfa\x60\x10\xf0\x44\x50\x20\x08\x00\x7f\x11\x60\x20\x08\x40\x00\x08\x00\x08",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x30\x1a\x07\x50\x00\x08\x07\x00\x11\x9f\x1d\x07\x8b\x1e\x07\x9c\x15\x07\x20\xf8\x60\x10\x28\x44\x60\x00\x08\x01\x00\x11\x20\x28",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8060,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x87\x1b\x07\x40\xf7\xa0\x81\x27\x07\x20\x48\x60\x10\x0a\x44\x10\xf4\x84\x3b\xd4\x00\xe0\x14\x43\x08\x00\x20\x00\x06\x07\x89\x02",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\xae\x90\x81\x04\x07\x40\x2e\x90\x40\x7f\xa0\x10\x2e\x44\x20\x2f\x08\x9f\x02\x07\x00\x00\x11\x10\x00\x20\x00\x00\x08\x40\x67",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x11\x84\x02\x07\x20\xf0\x60\x9f\x03\x07\x40\x7e\xa0\x40\x65\x90\x10\x62\x84\x10\x10\x08\x40\x61\xa0\x20\x1d\x08\x40\x5f",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x20\x1e\x08\x40\x5d\xa0\x10\x00\x08\x40\x5e\x90\x20\x30\x60\x10\xcf\x84\x20\x1c\x08\x7f\xff\x21\x3b\xf7\x00\xc8\x14\x43\x10\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8020,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x08\x40\xb5\x90\x40\xad\xa0\x00\x0a\x07\x10\x4f\x84\x20\x1c\x08\x7f\xff\x21\x3c\x01\x00\x48\x14\x43\x10\x00\x20\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x35\x90\x40\x2d\xa0\x00\x14\x07\x80\x0c\x07\x82\x03\x07\x04\x50\x84\x10\x00\x20\x20\x02\x07\x00\x08\x21\x40\x00\x08\x00\x01",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x50\xa0\x00\x04\x00\x20\x7f\x00\x80\x14\x43\x01\x00\x01\x04\x00\x11\x02\x00\x21\x10\x9f\x84\x10\x20\x08\x40\x97\x90\x10\xbf",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x20\x1d\x08\x40\xc6\xa0\x20\x8e\x08\x40\x72\x90\x40\x16\xa0\x10\x96\x84\x10\x00\x08\x40\x9e\x90\x20\x36\x60\x20\x91\x00\x00\x14",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8043,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x01\x01\x00\x04\x11\x00\x02\x21\x10\x1f\x84\x10\x20\x08\x40\x17\x90\x10\x3f\x84\x20\x1d\x08\x40\x46\xa0\x20\x8e\x08\x40\x70",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8090,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x96\xa0\x10\x16\x84\x10\x00\x08\x40\x1e\x90\x20\x34\x60\x10\x65\x84\x50\x10\x08\x00\x00\x21\x84\x03\x07\x20\xf0\x60\x9f\x03",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x7e\xa0\x00\x10\x08\x40\x65\xa0\x20\x0d\x08\x40\x64\x90\x40\x62\xa0\x00\x04\x00\x04\x1c\x44\x04\x1b\x44\xc3\xff\x21\x87\x04",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x7f\xa0\x10\x42\x84\x20\x1e\x08\x7f\xff\x21\x10\x20\x08\x40\x1c\x90\x10\x07\x84\x10\x00\x08\x40\x06\x90\x40\x55\xa0\x10\xc2",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x20\x1e\x08\x7f\xff\x21\x10\x20\x08\x40\x9c\x90\x10\x87\x84\x10\x00\x08\x40\x86\x90\x40\x56\xa0\x10\x56\x84\x20\x0f\x08\x40\x7b",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8090,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x22\x00\x60\x40\x00\x08\x40\x79\xa0\x00\x08\x11\x10\x55\x84\x20\x0f\x08\x40\x7a\x90\x22\x00\x60\x50\x00\x08\x40\x79\xa0\x00\xff",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x04\x6f\x84\x00\x05\x21\x21\x39\x00\x04\x00\x11\x10\xc1\x84\x20\x0f\x08\x70\x07\x07\x10\x10\x08\x40\xc0\xa0\x20\x0f\x08\x7f\xff",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x8f\x02\x07\x80\x00\x11\x08\x00\x20\x08\xc1\x84\x40\xbd\x90\x9e\x02\x07\x40\x7f\xa0\x40\xec\x90\x04\x6e\x84\x00\x05\x21\x21\x4c",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x04\x11\x10\x41\x84\x20\x0f\x08\x70\x07\x07\x10\x10\x08\x40\x40\xa0\x20\x0f\x08\x7f\xff\x11\x8f\x02\x07\x80\x00\x11\x08\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8020,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x08\x41\x84\x40\x3d\x90\x9e\x02\x07\x40\x7f\xa0\x40\x6d\x90\x10\x2d\x44\x20\xe7\x00\x08\x00\x11\x20\x32\x60\x80\x14\x43\x10\x2c",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8044,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x20\xec\x00\x00\x08\x11\x20\x31\x60\x00\x14\x43\x04\x34\x44\x40\x5b\xa0\x00\x04\x00\x21\x6a\x00\x80\x00\x11\x21\x70\x00\x3f\xff",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x08\xe8\x84\x20\x32\x50\x08\x68\x84\x20\x31\x50\x08\x5e\x84\x20\x30\x50\x0f\xc4\x07\x10\x34\x44\x10\x5b\x84\x6f\xf3\x07\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x02\x00\x21\x40\xfc\x90\x3d\xd4\x00\x04\xfc\x84\x00\x00\x21\x04\xfd\x84\x00\x08\x21\x00\x04\x00\x21\x81\x00\xc0\x00\x11\x04\x5b",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x02\x00\x21\x70\x06\x07\x10\xfe\x84\x30\x20\x08\x00\x03\x11\x10\x00\x08\x40\x59\x90\x04\xff\x84\x40\x58\xa0\x10\x59\x84\x20\x0f",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x59\xa0\x20\x00\x11\x70\x1e\x07\x10\x00\x08\x00\x0b\x11\x0f\xe4\x07\x10\x59\x84\x00\x00\x08\x20\x30\x50\x40\x59\xa0\x70\x06",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x00\x08\x00\x0a\x11\x0f\xec\x07\x08\x34\x44\x02\x10\x11\x10\x58\x84\x20\x0f\x08\x40\x58\xa0\x20\x00\x11\x30\x08\x07\x0f\xf4",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x58\x84\x00\x00\x08\x20\x30\x50\x40\x58\xa0\x70\x06\x07\x10\x00\x08\x00\x05\x11\x00\x04\x00\x10\x5a\x84\x00\x00\x08\x00\x01",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x5a\xa0\x08\x34\x44\x02\x00\x11\x30\x08\x07\x60\x00\x08\x00\x00\x11\x40\x5a\xa0\x8f\x4f\x07\x40\x7e\xa0\x00\x04\x00\x3c\xdd",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x00\x20\xa8\x14\x43\x04\xa7\x84\x20\x8e\x08\x40\xc4\x90\x40\xaf\xa0\x20\x0f\x08\x40\xc5\x90\x40\xa7\xa0\x3c\xe7\x00\x10\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8020,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x28\x14\x43\x04\x27\x84\x20\x8e\x08\x40\x44\x90\x40\x2f\xa0\x20\x0f\x08\x40\x45\x90\x40\x27\xa0\x00\x04\x00\x10\xf0\x44\x50\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\xff\xf7\x11\x93\x04\x07\x20\xf0\x60\x04\x61\x84\x04\x5c\x84\x40\x57\xa0\x00\x04\x00\x10\x61\x84\x00\x20\x08\x20\x1f\x08\x40\xff",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x00\x08\x40\x5c\x90\x40\x57\xa0\x04\x74\x84\x10\x00\x20\x00\x00\x08\x40\x74\xa0\x00\x01\x11\x20\x05\x07\x00\x40\x21\x70\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x7f\xff\x21\x08\xff\x84\x42\x00\x90\x85\x02\x07\x20\xf0\x60\x7f\xff\x11\x60\x20\x08\x00\xd0\x11\x40\x00\x08\x00\x02\x11\x86\x1b",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x74\xa0\x08\x6f\x84\x00\x00\x11\x04\xf1\x84\x00\x40\x21\x21\xf7\x00\x02\x00\x11\x60\x07\x07\x10\x20\x08\x40\xc3\x90\x20\x2f",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x80\x00\x11\x70\x02\x07\x7f\xff\x11\x10\x00\x08\x40\x8d\x90\x40\x85\xa0\x08\x6e\x84\x00\x00\x11\x04\xf0\x84\x00\x40\x21\x22\x07",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x02\x11\x60\x07\x07\x10\x20\x08\x40\x43\x90\x20\x2f\x08\x80\x00\x11\x70\x02\x07\x7f\xff\x11\x10\x00\x08\x40\x0d\x90\x40\x05",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\xec\x84\x00\x10\x08\x10\xe7\x84\x40\xe7\xa0\x20\x0f\x08\x40\xef\xa0\x10\x6d\x84\x00\x10\x08\x10\x6c\x84\x40\x6c\xa0\x20\x0f",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x7d\xa0\x7f\xff\x11\x00\x04\x00\x10\xef\x84\x00\x10\x08\x10\xee\x84\x40\xee\xa0\x20\x0f\x08\x40\xbd\xa0\x10\x7d\x84\x00\x10",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x7c\x84\x40\x7c\xa0\x20\x0f\x08\x40\x3d\xa0\x40\x00\x11\x3e\x9b\x00\xfb\xff\x21\x6f\x14\x43\x3e\x9e\x00\xff\xfb\x21\x6e\x14",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8043,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x3d\x57\x00\x40\xaf\xa0\xa0\x14\x43\x3d\x5a\x00\x40\x2f\xa0\x20\x14\x43\x00\x04\x00\x3d\x5e\x00\x40\xa5\xa0\xb0\x14\x43\x3d\x61",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x25\xa0\x30\x14\x43\x00\x00\x00\x22\x28\x00\x00\x02\x07\x22\x26\x00\x8a\x03\x07\x89\x04\x07\x40\x7e\xa0\x40\xbd\x90\x98\x14",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8043,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x9c\x84\x20\x8f\x08\x40\x9a\x90\x40\x99\xa0\x20\x2e\x08\x40\xbd\x90\x00\x10\x08\x40\x9b\xa0\x20\x0e\x08\x7f\xff\x21\x40\xf5",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8090,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x22\x3c\x00\x00\x02\x07\x22\x3a\x00\x82\x03\x07\x81\x04\x07\x40\x7e\xa0\x40\x3d\x90\x18\x14\x43\x10\x1c\x84\x20\x8f",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8008,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x1a\x90\x40\x19\xa0\x20\x2e\x08\x40\x3d\x90\x00\x10\x08\x40\x1b\xa0\x20\x0e\x08\x7f\xff\x21\x40\xf5\x90\x3d\x8c\x00\xb8\x14",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8043,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\xc2\xa0\x3d\x8f\x00\x38\x14\x43\x40\x42\xa0\x00\x04\x00\x10\x51\x84\x20\x0f\x08\x7f\xc0\x11\x40\x51\xa0\x08\xbb\x84\x08\x3b",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\xcb\x90\x11\x9e\x02\x07\xc7\x00\x11\x40\x7f\xa0\x08\xba\x84\x08\x3a\x84\x74\x70\x11\x9e\x02\x07\x59\x00\x11\x40\x7f\xa0\x3e\xed",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\xfd\xff\x21\xf1\x14\x43\x3e\xf0\x00\xff\xfd\x21\xf0\x14\x43\x00\x04\x00\x04\xb9\x84\x40\xb6\xa0\x04\x39\x84\x40\x36\xa0\x00\x04",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x3e\xc3\x00\x40\x87\xa0\x80\x14\x43\x3e\xc6\x00\x40\x07\xa0\x00\x14\x43\x00\x04\x00\x00\x00\x00\x04\xf5\x84\x00\x00\x21\x70\x03",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\xf5\x84\x10\x00\x08\x40\xf3\x90\x40\xf5\xa0\x3e\xd2\x00\x40\x85\xa0\x88\x14\x43\x3e\xd5\x00\x40\x05\xa0\x08\x14\x43\x00\x04",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8000,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x01\x07\x00\x02\x07\x00\x03\x07\x00\x04\x07\x00\x06\x07\x00\x16\x07\x00\x1e\x07\x00\x24\x07\x00\x3c\x07\x00\x72\x07\x00\x8d",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\xe0\x07\x00\xf6\x07\x01\x9f\x07\x01\xd1\x07\x00\x10\x07\x1f\xff\x07\x20\x03\x60\x0f\x5f\x07\x08\xfa\x84\x04\x11\x44\xff\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x2f\x63\x07\x70\x10\x08\x00\xf0\x21\x50\x00\x08\x00\xf0\x21\x40\xf9\x90\x00\x08\x07\x3e\x79\x00\x0f\x6b\x07\x90\x01\x07\x40\xf9",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x80a0,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x3f\x6e\x07\x70\x00\x08\x00\x7f\x11\x40\xf8\xa0\x9d\x07\x07\x20\x12\x60\x08\xfa\x84\x04\x11\x44\x3e\x54\x00\x00\x16\x07\x48\x80",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x08\x09\x44\x40\x75\x90\x7f\xef\x07\x10\x00\x20\x00\x10\x08\xff\xff\x21\x06\x00\x84\x40\xfb\xa0\x60\x00\x08\x00\x00\x21\x40\xff",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8090,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\xfb\x84\x60\x20\x08\x40\xfb\x90\x30\x00\x08\x00\x08\x11\x3d\xea\x00\x04\xfb\x84\x3d\xec\x00\x04\xff\x84\x40\xf8\xa0\x04\x11",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8044,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x8f\xff\x07\x20\x19\x60\x3e\xd2\x00\x00\x31\x07\x00\x01\x00\x00\x33\x07\x00\x02\x00\x27\x18\x43\x75\x00\x43\x8f\xff\x07\x20\x19",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8060,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x04\xf5\x84\x40\xf2\xa0\x04\x51\x84\x7f\xff\x21\x04\xf5\x84\x00\x00\x21\x04\xf3\x84\x00\x10\x21\x04\xf2\x84\x0c\x00\x21\x04\x72",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8044,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x77\x21\x3f\xfa\x07\x70\x20\x08\x00\x00\x11\x10\x2d\x44\x10\x2c\x44\x00\x20\x08\x10\x00\x11\x00\x00\x08\x00\x00\x11\x80\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8021,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x04\x73\x44\x07\x07\x21\x04\x72\x44\x00\x07\x21\x04\x73\x44\x07\x77\x21\x04\x73\x44\x00\x70\x21\x04\x2e\x44\x04\x2d\x44\x04\x2c",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8044,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x80\x00\x21\x04\x1c\x44\x04\x1b\x44\x0d\x00\x21\x04\x1c\x44\x04\x1b\x44\x0c\x80\x21\x04\x1c\x44\x04\x1b\x44\x1f\x08\x21\x0f\x18",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8043,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x08\x28\x44\x01\x11\x11\x08\x5d\x84\x10\x00\x10\x10\x00\x08\x40\x00\x21\x30\x04\x07\x40\xff\x90\x50\x20\x08\xc0\x00\x11\x10\xff",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x20\x08\x00\x40\x11\x40\x00\x08\x00\x01\x11\x20\x51\x60\x10\x00\x44\x60\x00\x08\x40\x54\x90\x20\x00\x60\x00\x79\x07\x81\x48",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x80\x03\x07\x40\xf8\xa0\x48\xc0\x11\x00\x77\x07\x00\x78\x07\x00\x79\x07\x00\x04\x07\x00\x7b\x07\x00\x7c\x07\x00\x7d\x07\x00\x55",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8007,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x7f\x07\x00\x80\x07\x00\x81\x07\x00\x74\x07\x00\x83\x07\x00\x82\x07\x00\x85\x07\x00\x86\x07\x0f\xef\x07\x1f\xff\x07\xff\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8011,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x10\x00\x20\x30\x10\x08\x00\x01\x21\x50\x00\x08\x00\x0f\x21\x40\xf9\x90\x30\x90\x07\x70\x10\x08\x00\x80\x21\x50\x00\x08\x00\xf0",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8021,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x08\xf9\x84\x04\xf8\x84\x2f\xff\x07\x3e\x59\x00\x04\xf1\x84\x04\xf0\x84\x04\x5a\x84\x00\x00\x21\x04\xf6\x84\x00\x02\x21\x04\xfa",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8084,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x40\x00\x21\x04\xf7\x84\x01\x04\x21\x04\x1c\x44\x04\x1b\x44\x0b\x23\x21\x0f\x18\x43\x04\x70\x44\x06\x66\x21\x04\x3b\x44\x04\x39",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8044,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x80\x21\x04\x3c\x44\x2a\x0a\x21\x04\x2f\x44\x00\x44\x21\x04\x29\x44\x00\x00\x21\x03\x19\x42\x4e\x19\x43\x0c\x59\x43\x27\x18",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8043,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x15\x00\x43\x04\x22\x44\x03\x07\x21\x04\x21\x44\x11\x11\x21\x04\x20\x44\x33\x33\x21\x00\x00\x42\xc0\x3d\x42\x80\x3d\x43\x04\x35",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8044,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x21\x04\x05\x43\x42\xcd\x00",
- .reg_data_seq_size = 9,
- },{
- .reg_addr = 0x88ef,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x20\x00\x18\x00\x00\x00\x00\xab\x0a\x00\x40\x00\x40\x00\x00\x00\x00\x00\x00\x00\x80\x00\x00\x27\x83\x64\x7e\xca\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x88b2,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x7f\x59\x7f\xbe\x7f\xfe\x7f\xfe\x7f\xee\x73\x54\x4c\x70\x5a\x00\x6b\x0e\x6b\x1c\x5b\xcb\x32\x8c\x0b\xd2\x13\x07\x28\x01\x49\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\xff\x7f\x0b\x97\x14\xae\xf7\x53\x26\x63\x07\x46\x00\x20\x00\x89\x00\x40\x80\x41\x00\x7f\xff\x7f\xd3\x09\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x88ff,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x7f\x00\x00\x00\x00\x90\xcb\x70\x74\xfe\x7f\x52\x09\x00\x00\xfe\x7f\x00\x00\x00\x00\x00\xf2\x00\x40\xf0\x7f\x00\x0c\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x4a\x00\x4a\x00\x24\xff\x7f\x00\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x00\x00\x50\x00\x30\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\xa0\x00\x00\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20\xff\x7f\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\xff\x7f\x00\x78\x00\x08\x00\x00\x10\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x40\x00\x40\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x80\xff\x7f\x1c\x1b\xc5\x07\xc0\x20\x12\xf0\x00\x10\x10\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\xcc\x59\x00\x00\x00\x40\x00\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x0a\x00\x00\x00\x00\x00\x00\x08\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x0b",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8897,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x14\xae\xf7\x53\x26\x63\x07\x46\x00\x20\x00\x89\x00\x40\x80\x41\x00\x7f\xff\x7f\xd3\x09\x00\x00\xff\x7f\x00\x00\x00\x00\x90\xcb",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8870,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x74\xfe\x7f\x52\x09\x00\x00\xfe\x7f\x00\x00\x00\x00\x00\xf2\x00\x40\xf0\x7f\x00\x0c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x4a\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x884a,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x24\xff\x7f\x00\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x00\x00\x50\x00\x30\x00\x00\x00\x00\x00\x00\x00\x00\xa0\x00\x00\x03",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8800,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20\xff\x7f\x00\x00\x00\x00\x00\x00\xff\x7f\x00",
- .reg_data_seq_size = 32,
- },{
- .reg_addr = 0x8878,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0000,
- .data_type = MSM_CAMERA_I2C_SEQ_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "\x00\x08\x00\x00\x10\x00\x00\x00\x00\x00\x00\x00\xff\x7f\x00\x40\x00\x40",
- .reg_data_seq_size = 18,
- },{
- .reg_addr = 0x8205,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0c00,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8205,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0d00,
- .data_type = MSM_CAMERA_I2C_WORD_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- },{
- .reg_addr = 0x8c01,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .reg_data = 0x0001,
- .data_type = MSM_CAMERA_I2C_BYTE_DATA,
- .i2c_operation = MSM_OIS_WRITE,
- .delay = 0,
- .reg_data_seq = "",
- .reg_data_seq_size = 0,
- }
-};
-
-
/*
// still mode settings:
{0x847f, 0x0c0c,}, //_M_EQCTL
@@ -1775,26 +616,6 @@ static struct reg_settings_ois_t ois_init_settings[] = {
{0x847f, 0x0d0d,}, //_M_EQCTL
*/
-static struct msm_camera_i2c_reg_array init_array_s5k3p8sp[] = {
- {0x6028,0x2000,0}, {0x6214,0x7971,0}, {0x6218,0x7150,0}, {0x30e,0x3d,0},
- {0x6028,0x2000,0}, {0x602a,0x2f38,0}, {0x6f12,0x88,0}, {0x6f12,0xd70,0},
- {0x344,0x18,0},
- {0x348,0x1217,0}, // last_pixel = 0x90C*2
- {0x346,0x18,0},
- {0x34a,0xd97,0}, // last_line = 0x6CC*2
- {0x34c,0x900,0}, // width?
- {0x34e,0x6c0,0}, // height?
- {0x342,0x1400,0}, // line_length_pclk
- {0x340,0xe3b,0}, // frame_length_lines
- {0x202,0x200,0}, // integ_time
- {0x200,0x618,0},
- {0x900,0x122,0}, {0x380,0x1,0}, {0x382,0x3,0}, {0x384,0x3,0}, {0x386,0x1,0}, {0x400,0x0,0}, {0x404,0x10,0},
- {0x3604,0x2,0}, {0x3606,0x103,0}, {0xf496,0x48,0}, {0xf470,0x20,0}, {0xf43a,0x15,0}, {0xf484,0x6,0}, {0xf440,0xaf,0}, {0xf442,0x44c6,0},
- {0xf408,0xfff7,0}, {0x3664,0x19,0}, {0xf494,0x1010,0}, {0x367a,0x100,0}, {0x362a,0x104,0}, {0x362e,0x404,0}, {0x32b2,0x8,0}, {0x3286,0x3,0}, {0x328a,0x5,0},
- {0xf47c,0x1f,0}, {0xf62e,0xc5,0}, {0xf630,0xcd,0}, {0xf632,0xdd,0}, {0xf634,0xe5,0}, {0xf636,0xf5,0}, {0xf638,0xfd,0}, {0xf63a,0x10d,0}, {0xf63c,0x115,0}, {0xf63e,0x125,0}, {0xf640,0x12d,0},
- {0x6028,0x2000,0}, {0x602a,0x1704,0}, {0x6f12,0x8011,0}, {0x3070,0x0,0}, {0xb0e,0x0,0}, {0x317a,0x7,0}, {0x31c0,0xc8,0}, {0x1006,0x4,0}, {0x31a4,0x102,0},
-};
-
static struct msm_camera_i2c_reg_array init_array_ov8865[] = {
// round 1
//{0x103,0x1,0}, // software reset
diff --git a/selfdrive/camerad/imgproc/utils.cc b/selfdrive/camerad/imgproc/utils.cc
index 4aeffac5..7dd0ab97 100644
--- a/selfdrive/camerad/imgproc/utils.cc
+++ b/selfdrive/camerad/imgproc/utils.cc
@@ -1,7 +1,14 @@
#include "utils.h"
+#include
#include
+#include
#include
#include
+
+const int16_t lapl_conv_krnl[9] = {0, 1, 0,
+ 1, -4, 1,
+ 0, 1, 0};
+
// calculate score based on laplacians in one area
uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) {
const int size = x_pitch * y_pitch;
@@ -9,7 +16,7 @@ uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) {
int16_t max = 0;
int sum = 0;
for (int i = 0; i < size; ++i) {
- const int16_t v = lap[i % x_pitch + (i / x_pitch) * x_pitch];
+ const int16_t v = lap[i];
sum += v;
if (v > max) max = v;
}
@@ -19,7 +26,7 @@ uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) {
// var of roi
int var = 0;
for (int i = 0; i < size; ++i) {
- var += std::pow(lap[i % x_pitch + (i / x_pitch) * x_pitch] - mean, 2);
+ var += std::pow(lap[i] - mean, 2);
}
const float fvar = (float)var / size;
@@ -34,4 +41,65 @@ bool is_blur(const uint16_t *lapmap, const size_t size) {
}
}
return (bad_sum > LM_PREC_THRESH);
-}
\ No newline at end of file
+}
+
+static cl_program build_conv_program(cl_device_id device_id, cl_context context, int image_w, int image_h, int filter_size) {
+ char args[4096];
+ snprintf(args, sizeof(args),
+ "-cl-fast-relaxed-math -cl-denorms-are-zero "
+ "-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d "
+ "-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d",
+ image_w, image_h, 1,
+ filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w);
+ return cl_program_from_file(context, device_id, "imgproc/conv.cl", args);
+}
+
+LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int filter_size)
+ : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y),
+ roi_buf(width * height * 3), result_buf(width * height) {
+
+ prg = build_conv_program(device_id, ctx, width, height, filter_size);
+ krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb2gray_conv2d", &err));
+ // TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter
+ roi_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, roi_buf.size() * sizeof(roi_buf[0]), NULL, &err));
+ result_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, result_buf.size() * sizeof(result_buf[0]), NULL, &err));
+ filter_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
+ 9 * sizeof(int16_t), (void *)&lapl_conv_krnl, &err));
+}
+
+LapConv::~LapConv() {
+ CL_CHECK(clReleaseMemObject(roi_cl));
+ CL_CHECK(clReleaseMemObject(result_cl));
+ CL_CHECK(clReleaseMemObject(filter_cl));
+ CL_CHECK(clReleaseKernel(krnl));
+ CL_CHECK(clReleaseProgram(prg));
+}
+
+uint16_t LapConv::Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id) {
+ // sharpness scores
+ const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1);
+ const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1);
+
+ const uint8_t *rgb_offset = rgb_buf + y_offset * height * FULL_STRIDE_X * 3 + x_offset * width * 3;
+ for (int i = 0; i < height; ++i) {
+ memcpy(&roi_buf[i * width * 3], &rgb_offset[i * FULL_STRIDE_X * 3], width * 3);
+ }
+
+ constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t));
+ const size_t global_work_size[] = {(size_t)width, (size_t)height};
+ const size_t local_work_size[] = {CONV_LOCAL_WORKSIZE, CONV_LOCAL_WORKSIZE};
+
+ CL_CHECK(clEnqueueWriteBuffer(q, roi_cl, CL_TRUE, 0, roi_buf.size() * sizeof(roi_buf[0]), roi_buf.data(), 0, 0, 0));
+ CL_CHECK(clSetKernelArg(krnl, 0, sizeof(cl_mem), (void *)&roi_cl));
+ CL_CHECK(clSetKernelArg(krnl, 1, sizeof(cl_mem), (void *)&result_cl));
+ CL_CHECK(clSetKernelArg(krnl, 2, sizeof(cl_mem), (void *)&filter_cl));
+ CL_CHECK(clSetKernelArg(krnl, 3, local_mem_size, 0));
+ cl_event conv_event;
+ CL_CHECK(clEnqueueNDRangeKernel(q, krnl, 2, NULL, global_work_size, local_work_size, 0, 0, &conv_event));
+ CL_CHECK(clWaitForEvents(1, &conv_event));
+ CL_CHECK(clReleaseEvent(conv_event));
+ CL_CHECK(clEnqueueReadBuffer(q, result_cl, CL_TRUE, 0,
+ result_buf.size() * sizeof(result_buf[0]), result_buf.data(), 0, 0, 0));
+
+ return get_lapmap_one(result_buf.data(), width, height);
+}
diff --git a/selfdrive/camerad/imgproc/utils.h b/selfdrive/camerad/imgproc/utils.h
index 4928f55a..e33cfe9c 100644
--- a/selfdrive/camerad/imgproc/utils.h
+++ b/selfdrive/camerad/imgproc/utils.h
@@ -2,6 +2,9 @@
#include
#include
+#include
+#include "clutil.h"
+
#define NUM_SEGMENTS_X 8
#define NUM_SEGMENTS_Y 6
@@ -19,9 +22,19 @@
#define CONV_LOCAL_WORKSIZE 16
-const int16_t lapl_conv_krnl[9] = {0, 1, 0,
- 1, -4, 1,
- 0, 1, 0};
+class LapConv {
+public:
+ LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int filter_size);
+ ~LapConv();
+ uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id);
+
+private:
+ cl_mem roi_cl, result_cl, filter_cl;
+ cl_program prg;
+ cl_kernel krnl;
+ const int width, height;
+ std::vector roi_buf;
+ std::vector result_buf;
+};
-uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch);
bool is_blur(const uint16_t *lapmap, const size_t size);
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index 77e24b4b..2af6df26 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -5,7 +5,6 @@ from selfdrive.version import comma_remote, tested_branch
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car
-from selfdrive.hardware import EON
from selfdrive.swaglog import cloudlog
import cereal.messaging as messaging
from selfdrive.car import gen_empty_fingerprint
@@ -24,8 +23,6 @@ def get_startup_event(car_recognized, controller_available):
event = EventName.startupNoCar
elif car_recognized and not controller_available:
event = EventName.startupNoControl
- elif EON and "letv" not in open("/proc/cmdline").read():
- event = EventName.startupOneplus
return event
diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py
index 3ab2ff84..46681b13 100644
--- a/selfdrive/car/chrysler/carcontroller.py
+++ b/selfdrive/car/chrysler/carcontroller.py
@@ -24,7 +24,7 @@ class CarController():
# *** compute control surfaces ***
# steer torque
- new_steer = actuators.steer * CarControllerParams.STEER_MAX
+ new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorqueEps, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
@@ -32,7 +32,7 @@ class CarController():
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
- elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
+ elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index e433665d..ccf3bc9f 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -11,10 +11,7 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@staticmethod
- def get_params(candidate, fingerprint=None, car_fw=None):
- if fingerprint is None:
- fingerprint = gen_empty_fingerprint()
-
+ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "chrysler"
ret.safetyModel = car.CarParams.SafetyModel.chrysler
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index 6cb3ff8b..17e636a0 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -64,7 +64,7 @@ FINGERPRINTS = {
},
# Based on "8190c7275a24557b|2020-02-24--09-57-23"
{
- 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8
}
],
CAR.JEEP_CHEROKEE: [
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index 41ce2fd8..0949ffe2 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -1,3 +1,4 @@
+import math
from cereal import car
from selfdrive.car import make_can_msg
from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button
@@ -33,7 +34,7 @@ class CarController():
if (frame % 3) == 0:
- curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*3.1415/180., CS.out.vEgo)
+ curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py
index 2584618e..468b17ba 100755
--- a/selfdrive/car/fw_versions.py
+++ b/selfdrive/car/fw_versions.py
@@ -56,50 +56,78 @@ HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x4
TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01'
TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01'
+VOLKSWAGEN_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
+ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
+ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER) + \
+ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
+VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
+
OBD_VERSION_REQUEST = b'\x09\x04'
OBD_VERSION_RESPONSE = b'\x49\x04'
+DEFAULT_RX_OFFSET = 0x8
+VOLKSWAGEN_RX_OFFSET = 0x6a
-# supports subaddressing, request, response
+# brand, request, response, response offset
REQUESTS = [
- # Hundai
+ # Hyundai
(
"hyundai",
[HYUNDAI_VERSION_REQUEST_SHORT],
[HYUNDAI_VERSION_RESPONSE],
+ DEFAULT_RX_OFFSET,
),
(
"hyundai",
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
+ DEFAULT_RX_OFFSET,
),
(
"hyundai",
[HYUNDAI_VERSION_REQUEST_MULTI],
[HYUNDAI_VERSION_RESPONSE],
+ DEFAULT_RX_OFFSET,
),
# Honda
(
"honda",
[UDS_VERSION_REQUEST],
[UDS_VERSION_RESPONSE],
+ DEFAULT_RX_OFFSET,
),
# Toyota
(
"toyota",
[SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST],
[SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE],
+ DEFAULT_RX_OFFSET,
),
(
"toyota",
[SHORT_TESTER_PRESENT_REQUEST, OBD_VERSION_REQUEST],
[SHORT_TESTER_PRESENT_RESPONSE, OBD_VERSION_RESPONSE],
+ DEFAULT_RX_OFFSET,
),
(
"toyota",
[TESTER_PRESENT_REQUEST, DEFAULT_DIAGNOSTIC_REQUEST, EXTENDED_DIAGNOSTIC_REQUEST, UDS_VERSION_REQUEST],
[TESTER_PRESENT_RESPONSE, DEFAULT_DIAGNOSTIC_RESPONSE, EXTENDED_DIAGNOSTIC_RESPONSE, UDS_VERSION_RESPONSE],
- )
+ DEFAULT_RX_OFFSET,
+ ),
+ # Volkswagen
+ (
+ "volkswagen",
+ [VOLKSWAGEN_VERSION_REQUEST_MULTI],
+ [VOLKSWAGEN_VERSION_RESPONSE],
+ VOLKSWAGEN_RX_OFFSET,
+ ),
+ (
+ "volkswagen",
+ [VOLKSWAGEN_VERSION_REQUEST_MULTI],
+ [VOLKSWAGEN_VERSION_RESPONSE],
+ DEFAULT_RX_OFFSET,
+ ),
]
@@ -173,12 +201,12 @@ def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False,
fw_versions = {}
for i, addr in enumerate(tqdm(addrs, disable=not progress)):
for addr_chunk in chunks(addr):
- for brand, request, response in REQUESTS:
+ for brand, request, response, response_offset in REQUESTS:
try:
addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any')]
if addrs:
- query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, request, response, debug=debug)
+ query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, request, response, response_offset, debug=debug)
t = 2 * timeout if i == 0 else timeout
fw_versions.update(query.get_data(t))
except Exception:
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index dcd300c7..116a562a 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -35,7 +35,7 @@ class CarController():
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
- new_steer = actuators.steer * P.STEER_MAX
+ new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
else:
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 7ec7679f..a59740c6 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -28,7 +28,6 @@ class CarState(CarStateBase):
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
- ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
@@ -38,9 +37,16 @@ class CarState(CarStateBase):
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
ret.gasPressed = ret.gas > 1e-5
+ ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
+ ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelRate']
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
+ ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]['LKATorqueDelivered']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
+ # 0 inactive, 1 active, 2 temporarily limited, 3 failed
+ self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
+ ret.steerWarning = self.lkas_status not in [0, 1]
+
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or
@@ -65,10 +71,6 @@ class CarState(CarStateBase):
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
- # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
- self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
- ret.steerWarning = self.lkas_status not in [0, 1]
-
return ret
@staticmethod
@@ -88,12 +90,14 @@ class CarState(CarStateBase):
("CruiseState", "AcceleratorPedal2", 0),
("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
("SteeringWheelAngle", "PSCMSteeringAngle", 0),
+ ("SteeringWheelRate", "PSCMSteeringAngle", 0),
("FLWheelSpd", "EBCMWheelSpdFront", 0),
("FRWheelSpd", "EBCMWheelSpdFront", 0),
("RLWheelSpd", "EBCMWheelSpdRear", 0),
("RRWheelSpd", "EBCMWheelSpdRear", 0),
("PRNDL", "ECMPRDNL", 0),
("LKADriverAppldTrq", "PSCMStatus", 0),
+ ("LKATorqueDelivered", "PSCMStatus", 0),
("LKATorqueDeliveredStatus", "PSCMStatus", 0),
("TractionControlOn", "ESPStatus", 0),
("EPBClosed", "EPBStatus", 0),
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 2fc52fc9..fe9a2908 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -623,6 +623,7 @@ FW_VERSIONS = {
b'37805-5PA-A870\x00\x00',
b'37805-5PA-A880\x00\x00',
b'37805-5PA-A890\x00\x00',
+ b'37805-5PA-AF20\x00\x00',
b'37805-5PD-Q630\x00\x00',
],
(Ecu.transmission, 0x18da1ef1, None): [
@@ -660,6 +661,7 @@ FW_VERSIONS = {
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TLA-A110\x00\x00',
+ b'78109-TLA-A120\x00\x00',
b'78109-TLA-A210\x00\x00',
b'78109-TLA-A220\x00\x00',
b'78109-TLA-C210\x00\x00',
@@ -692,6 +694,7 @@ FW_VERSIONS = {
b'77959-TLA-A250\x00\x00',
b'77959-TLA-A320\x00\x00',
b'77959-TLA-A410\x00\x00',
+ b'77959-TLA-A420\x00\x00',
b'77959-TLA-Q040\x00\x00',
],
},
@@ -933,12 +936,14 @@ FW_VERSIONS = {
b'37805-5YF-A230\x00\x00',
b'37805-5YF-A420\x00\x00',
b'37805-5YF-A430\x00\x00',
+ b'37805-5YF-C210\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TJB-A040\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36802-TJB-A040\x00\x00',
+ b'36802-TJB-A050\x00\x00',
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'36161-TJB-A040\x00\x00',
@@ -952,10 +957,13 @@ FW_VERSIONS = {
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TJB-AB10\x00\x00',
+ b'78109-TJB-AD10\x00\x00',
b'78109-TJB-AF10\x00\x00',
+ b'78109-TJB-AW10\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TJB-A040\x00\x00',
+ b'77959-TJB-A210\x00\x00',
],
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [
b'46114-TJB-A050\x00\x00',
@@ -963,21 +971,25 @@ FW_VERSIONS = {
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TJB-A110\x00\x00',
+ b'38897-TJB-A120\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TJB-A030\x00\x00',
+ b'39990-TJB-A040\x00\x00',
],
},
CAR.RIDGELINE: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-T6Z-A020\x00\x00',
b'39990-T6Z-A030\x00\x00',
+ b'39990-T6Z-A050\x00\x00',
],
(Ecu.fwdCamera, 0x18dab0f1, None): [
b'36161-T6Z-A020\x00\x00',
b'36161-T6Z-A310\x00\x00',
b'36161-T6Z-A420\x00\x00',
b'36161-T6Z-A520\x00\x00',
+ b'36161-T6Z-A620\x00\x00',
b'36161-TJZ-A120\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
@@ -988,6 +1000,7 @@ FW_VERSIONS = {
b'78109-T6Z-A420\x00\x00',
b'78109-T6Z-A510\x00\x00',
b'78109-T6Z-A710\x00\x00',
+ b'78109-T6Z-AA10\x00\x00',
b'78109-TJZ-A510\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
@@ -1051,6 +1064,7 @@ FW_VERSIONS = {
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-THX-A110\x00\x00',
b'78109-THX-A210\x00\x00',
+ b'78109-THX-A220\x00\x00',
b'78109-THX-C220\x00\x00',
],
},
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index e71ba6d6..cf4756e8 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -45,7 +45,7 @@ class CarController():
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart):
# Steering Torque
- new_steer = actuators.steer * self.p.STEER_MAX
+ new_steer = int(round(actuators.steer * self.p.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
self.steer_rate_limited = new_steer != apply_steer
@@ -75,13 +75,14 @@ class CarController():
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# send resume at a max freq of 10Hz
- if (frame - self.last_resume_frame)*DT_CTRL > 0.1:
+ if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
self.last_resume_frame = frame
# 20 Hz LFA MFA message
- if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.IONIQ_EV_2020]:
+ if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV,
+ CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
return can_sends
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index b6f8d412..228ccfec 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -17,7 +17,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
values["CF_Lkas_ActToi"] = steer_req
values["CF_Lkas_MsgCount"] = frame % 0x10
- if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.SANTA_FE, CAR.IONIQ_EV_2020]:
+ if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.SANTA_FE, CAR.IONIQ_EV_2020, CAR.KIA_SELTOS]:
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index e8965a2e..9af39af0 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -28,6 +28,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 1.
ret.maxSteeringAngleDeg = 90.
+ ret.startAccel = 1.0
eps_modified = False
for fw in car_fw:
@@ -142,6 +143,14 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+ elif candidate == CAR.KIA_SELTOS:
+ ret.lateralTuning.pid.kf = 0.00005
+ ret.mass = 1310. + STD_CARGO_KG
+ ret.wheelbase = 2.6
+ ret.steerRatio = 13.73 # Spec
+ tire_stiffness_factor = 0.5
+ ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
@@ -157,7 +166,6 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
-
elif candidate == CAR.KIA_FORTE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
@@ -166,6 +174,15 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+ elif candidate == CAR.KIA_CEED:
+ ret.lateralTuning.pid.kf = 0.00005
+ ret.mass = 1450. + STD_CARGO_KG
+ ret.wheelbase = 2.65
+ ret.steerRatio = 13.75
+ tire_stiffness_factor = 0.5
+ ret.lateralTuning.pid.kf = 0.00005
+ ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
# Genesis
elif candidate == CAR.GENESIS_G70:
@@ -197,8 +214,9 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
# these cars require a special panda safety mode due to missing counters and checksums in the messages
- if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF,
- CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80]:
+ if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO,
+ CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.KIA_SELTOS,
+ CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED]:
ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy
ret.centerToFront = ret.wheelbase * 0.4
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 1f827979..54fa98f4 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -39,8 +39,10 @@ class CAR:
KIA_NIRO_EV = "KIA NIRO EV 2020"
KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016"
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
+ KIA_SELTOS = "KIA SELTOS 2021"
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
KIA_STINGER = "KIA STINGER GT2 2018"
+ KIA_CEED = "KIA CEED INTRO ED 2019"
# Genesis
GENESIS_G70 = "GENESIS G70 2018"
@@ -140,6 +142,9 @@ FINGERPRINTS = {
CAR.KIA_NIRO_EV: [{
127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 516: 8, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1193: 8, 1225: 8, 1260: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
}],
+ CAR.KIA_CEED: [{
+ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1157: 4, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1427: 6, 1456: 4, 2015: 8
+ }],
CAR.KIA_FORTE: [{
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
}],
@@ -149,6 +154,9 @@ FINGERPRINTS = {
{
68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1265: 4, 1268: 8, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1420: 8, 1425: 2, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8
}],
+ CAR.KIA_SELTOS: [{
+ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 524: 8, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 905: 8, 909: 8, 910: 5, 911: 5, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1102: 8, 1107: 5, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1470: 8, 1485: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
+ }],
CAR.PALISADE: [{
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 547: 8, 548: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8
}],
@@ -158,7 +166,7 @@ FINGERPRINTS = {
}
# Don't use these fingerprints for fingerprinting, they are still used for ECU detection
-IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA]
+IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA, CAR.KIA_CEED, CAR.KIA_SELTOS]
FW_VERSIONS = {
CAR.IONIQ_EV_2020: {
@@ -189,6 +197,9 @@ FW_VERSIONS = {
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ',
b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ',
b'\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ',
+ b'\xf1\x00DN89110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa \xf1\xa01.00\xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00',
+ b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa \xf1\xa01.00\xaa\xaa\xaa',
+ b'\xf1\xa01.00',
],
(Ecu.esp, 0x7d1, None): [
b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02',
@@ -196,17 +207,23 @@ FW_VERSIONS = {
b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04',
b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100\xf1\xa01.04',
b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100\xf1\xa01.06',
+ b'\xf1\x00DN ESC \a 106 \a\x01 58910-L0100',
+ b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100',
],
(Ecu.engine, 0x7e0, None): [
b'HM6M2_0a0_BD0',
b'\xf1\x87391162M003\xf1\xa0000F',
b'\xf1\x87391162M003\xf1\xa00240',
b'HM6M1_0a0_F00',
+ b'\xf1\x81HM6M1_0a0_F00',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103\xf1\xa01.03',
b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101\xf1\xa01.01',
b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101\xf1\xa01.01',
+ b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101\xf1\xa01.01',
+ b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101',
+ b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422',
@@ -285,12 +302,14 @@ FW_VERSIONS = {
CAR.PALISADE: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04',
+ b'\xf1\000LX2_ SCC F-CUP 1.00 1.05 99110-S8100 \xf1\xa01.05',
b'\xf1\x00LX2 SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04',
],
(Ecu.esp, 0x7d1, None): [
b'\xf1\x00LX ESC \v 102\x19\x05\a 58910-S8330\xf1\xa01.02',
b'\xf1\x00LX ESC \v 103\x19\t\x10 58910-S8360\xf1\xa01.03',
b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360\xf1\xa01.03',
+ b'\xf1\x00LX ESC \x01 103\x31\t\020 58910-S8360\xf1\xa01.03',
b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330',
],
(Ecu.engine, 0x7e0, None): [
@@ -304,10 +323,12 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125',
b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.05 99211-S8100 190909',
+ b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.08 99211-S8100 200903',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',
b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',
+ b'\xf1\x87LBLUFN731381KF36\xb9\x99\x89\x98\x98\x88\x88\x88\x89\x99\xa8\x99\x88\x99\xa8\x89\x88\x88\x98\x88V\177\xf6\xff\x99w\x8f\xff\xad\xd8\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\000bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',
b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
b'\xf1\x87LDLVBN560098KF26\x86fff\x87vgfg\x88\x96xfw\x86gfw\x86g\x95\xf6\xffeU_\xff\x92c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
],
@@ -338,6 +359,14 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ],
},
+ CAR.KIA_CEED: {
+ (Ecu.fwdRadar, 0x7D0, None): [b'\xf1\000CD__ SCC F-CUP 1.00 1.02 99110-J7000 ', ],
+ (Ecu.esp, 0x7D4, None): [b'\xf1\000CD MDPS C 1.00 1.06 56310-XX000 4CDEC106', ],
+ (Ecu.fwdCamera, 0x7C4, None): [b'\xf1\000CD LKAS AT EUR LHD 1.00 1.01 99211-J7000 B40', ],
+ (Ecu.engine, 0x7E0, None): [b'\001TCD-JECU4F202H0K', ],
+ (Ecu.transmission, 0x7E1, None): [b'\xf1\x816U2V7051\000\000\xf1\0006U2V0_C2\000\0006U2V7051\000\000DCD0T14US1\000\000\000\000', ],
+ (Ecu.esp, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ],
+ },
CAR.KONA_EV: {
(Ecu.esp, 0x7D1, None): [
b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05',
@@ -351,6 +380,7 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x7D0, None): [
b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 \xf1\xa01.03',
b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 \xf1\xa01.00',
+ b'\xf1\x8799110Q4500\xf1\000DEev SCC F-CUP 1.00 1.00 99110-Q4500 \xf1\xa01.00',
],
(Ecu.esp, 0x7D1, None): [
b'\xf1\xa01.06',
@@ -363,8 +393,26 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821',
b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211',
+ b'\xf1\000DEE MFC AT EUR LHD 1.00 1.00 99211-Q4100 200706',
],
},
+ CAR.KIA_SELTOS: {
+ (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 \xf1\xa01.05',],
+ (Ecu.esp, 0x7d1, None): [
+ b'\xf1\x8758910-Q5450\xf1\000SP ESC \a 101\031\t\005 58910-Q5450\xf1\xa01.01',
+ b'\xf1\x8758910-Q5450\xf1\000SP ESC \t 101\031\t\005 58910-Q5450\xf1\xa01.01',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x81616D2051\000\000\000\000\000\000\000\000',
+ b'\001TSP2KNL06F100J0K',
+ ],
+ (Ecu.eps, 0x7d4, None): [b'\xf1\000SP2 MDPS C 1.00 1.04 56300Q5200 ',],
+ (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\000SP2 MFC AT USA LHD 1.00 1.04 99210-Q5000 191114',],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x87CZLUB49370612JF7h\xa8y\x87\x99\xa7hv\x99\x97fv\x88\x87x\x89x\x96O\xff\x88\xff\xff\xff.@\xf1\x816V2C2051\000\000\xf1\0006V2B0_C2\000\0006V2C2051\000\000CSP4N20NS3\000\000\000\000',
+ b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS6\xd30\xa5\xb9',
+ ],
+ },
CAR.KIA_OPTIMA: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 '],
(Ecu.esp, 0x7d1, None): [b'\xf1\x00JF ESC \v 11 \x18\x030 58920-D5180',],
@@ -376,7 +424,7 @@ FW_VERSIONS = {
}
CHECKSUM = {
- "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE],
+ "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS],
"6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
}
@@ -387,11 +435,11 @@ FEATURES = {
"use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020]),
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
- "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE]),
+ "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS]),
"use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70,
CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA, CAR.IONIQ_EV_2020,
- CAR.SANTA_FE, CAR.KIA_NIRO_EV]),
+ CAR.SANTA_FE, CAR.KIA_NIRO_EV, CAR.KIA_SELTOS]),
}
EV_HYBRID = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
@@ -410,6 +458,7 @@ DBC = {
CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
+ CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
CAR.KONA: dbc_dict('hyundai_kia_generic', None),
@@ -419,6 +468,7 @@ DBC = {
CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None),
CAR.PALISADE: dbc_dict('hyundai_kia_generic', None),
CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
+ CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None),
}
STEER_THRESHOLD = 150
diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py
index c5185dec..c95ef2c7 100644
--- a/selfdrive/car/isotp_parallel_query.py
+++ b/selfdrive/car/isotp_parallel_query.py
@@ -9,7 +9,7 @@ from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_a
class IsoTpParallelQuery():
- def __init__(self, sendcan, logcan, bus, addrs, request, response, functional_addr=False, debug=False):
+ def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addr=False, debug=False):
self.sendcan = sendcan
self.logcan = logcan
self.bus = bus
@@ -25,7 +25,7 @@ class IsoTpParallelQuery():
else:
self.real_addrs.append((a, None))
- self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0]) for tx_addr in self.real_addrs}
+ self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in self.real_addrs}
self.msg_buffer = defaultdict(list)
def rx(self):
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index 8fe7c74d..604851cb 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -1,4 +1,5 @@
#!/usr/bin/env python3
+import math
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.swaglog import cloudlog
@@ -10,7 +11,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
TS = 0.01 # 100Hz
YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter
# low pass gain
-LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
+LPG = 2 * math.pi * YAW_FR * TS / (1 + 2 * math.pi * YAW_FR * TS)
class CarInterface(CarInterfaceBase):
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index 1ad4a93c..30f4e94f 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -28,11 +28,6 @@ class CarInterface(CarInterfaceBase):
ret.steerRateCost = 0.5
ret.steerActuatorDelay = 0.1
- ret.lateralTuning.pid.kf = 0.00006
- ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.0], [0.0]]
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]]
- ret.steerMaxBP = [0.] # m/s
- ret.steerMaxV = [1.]
if candidate in [CAR.ROGUE, CAR.XTRAIL]:
ret.mass = 1610 + STD_CARGO_KG
diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py
index 9eb6f275..60d5f5ed 100755
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@@ -33,13 +33,14 @@ class TestCarInterfaces(unittest.TestCase):
self.assertGreater(car_params.mass, 1)
self.assertGreater(car_params.steerRateCost, 1e-3)
- tuning = car_params.lateralTuning.which()
- if tuning == 'pid':
- self.assertTrue(len(car_params.lateralTuning.pid.kpV))
- elif tuning == 'lqr':
- self.assertTrue(len(car_params.lateralTuning.lqr.a))
- elif tuning == 'indi':
- self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))
+ if car_params.steerControlType != car.CarParams.SteerControlType.angle:
+ tuning = car_params.lateralTuning.which()
+ if tuning == 'pid':
+ self.assertTrue(len(car_params.lateralTuning.pid.kpV))
+ elif tuning == 'lqr':
+ self.assertTrue(len(car_params.lateralTuning.lqr.a))
+ elif tuning == 'indi':
+ self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))
# Run car interface
CC = car.CarControl.new_message()
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 9d782cbd..9c857f96 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -300,14 +300,16 @@ FW_VERSIONS = {
CAR.AVALON: {
(Ecu.esp, 0x7b0, None): [
b'F152607110\x00\x00\x00\x00\x00\x00',
+ b'F152607171\x00\x00\x00\x00\x00\x00',
b'F152607180\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
- b'881510705200\x00\x00\x00\x00',
b'881510701300\x00\x00\x00\x00',
b'881510703200\x00\x00\x00\x00',
+ b'881510705200\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
+ b'8965B41080\x00\x00\x00\x00\x00\x00',
b'8965B41090\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
@@ -315,6 +317,7 @@ FW_VERSIONS = {
b'\x0230721200\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
+ b'\x01896630735100\x00\x00\x00\x00',
b'\x01896630738000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
@@ -324,8 +327,8 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F0701100\x00\x00\x00\x00',
- b'8646F0703000\x00\x00\x00\x00',
b'8646F0702100\x00\x00\x00\x00',
+ b'8646F0703000\x00\x00\x00\x00',
],
},
CAR.CAMRY: {
@@ -338,6 +341,7 @@ FW_VERSIONS = {
b'\x018966306Q4000\x00\x00\x00\x00',
b'\x018966306Q4100\x00\x00\x00\x00',
b'\x018966306Q4200\x00\x00\x00\x00',
+ b'\x018966333Q9200\x00\x00\x00\x00',
b'\x018966333P3100\x00\x00\x00\x00',
b'\x018966333P3200\x00\x00\x00\x00',
b'\x018966333P4200\x00\x00\x00\x00',
@@ -355,6 +359,7 @@ FW_VERSIONS = {
b'8821F0601300 ',
b'8821F0602000 ',
b'8821F0603300 ',
+ b'8821F0604100 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
@@ -366,6 +371,7 @@ FW_VERSIONS = {
b'F152606270\x00\x00\x00\x00\x00\x00',
b'F152606290\x00\x00\x00\x00\x00\x00',
b'F152633540\x00\x00\x00\x00\x00\x00',
+ b'F152633A10\x00\x00\x00\x00\x00\x00',
b'F152633A20\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
@@ -380,6 +386,7 @@ FW_VERSIONS = {
b'8821F0601300 ',
b'8821F0602000 ',
b'8821F0603300 ',
+ b'8821F0604100 ',
b'8821F0607200 ',
b'8821F0608000 ',
b'8821F0608200 ',
@@ -389,6 +396,7 @@ FW_VERSIONS = {
b'8646F0601200 ',
b'8646F0601300 ',
b'8646F0603400 ',
+ b'8821F0604100 ',
b'8646F0605000 ',
b'8646F0606000 ',
b'8646F0606100 ',
@@ -472,6 +480,7 @@ FW_VERSIONS = {
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152606370\x00\x00\x00\x00\x00\x00',
+ b'\x01F152606390\x00\x00\x00\x00\x00\x00',
b'\x01F152606400\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
@@ -483,6 +492,7 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x750, 109): [
b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
+ b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
],
},
CAR.CAMRYH_TSS2: {
@@ -511,13 +521,13 @@ FW_VERSIONS = {
],
(Ecu.dsu, 0x791, None): [
b'8821F0W01000 ',
+ b'8821F0W01100 ',
b'8821FF401600 ',
b'8821FF404000 ',
b'8821FF404100 ',
b'8821FF405100 ',
b'8821FF406000 ',
b'8821FF407100 ',
- b'8821F0W01100 ',
],
(Ecu.esp, 0x7b0, None): [
b'F152610020\x00\x00\x00\x00\x00\x00',
@@ -535,11 +545,11 @@ FW_VERSIONS = {
b'8965B10070\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
- b'\x033F401100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203102\x00\x00\x00\x00',
- b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
b'\x0331036000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
+ b'\x033F401100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203102\x00\x00\x00\x00',
+ b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F0W01000 ',
@@ -560,34 +570,41 @@ FW_VERSIONS = {
},
CAR.CHRH: {
(Ecu.engine, 0x700, None): [
- b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+ b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+ b'\x0289663F405000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0189663F438000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152610040\x00\x00\x00\x00\x00\x00',
b'F152610190\x00\x00\x00\x00\x00\x00',
+ b'F152610013\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'8821FF404000 ',
+ b'8821FF406000 ',
b'8821FF407100 ',
],
(Ecu.eps, 0x7a1, None): [
+ b'8965B10011\x00\x00\x00\x00\x00\x00',
b'8965B10040\x00\x00\x00\x00\x00\x00',
b'8965B10050\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821FF404000 ',
+ b'8821FF406000 ',
b'8821FF407100 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646FF404000 ',
+ b'8821FF406000 ',
b'8646FF407000 ',
],
},
CAR.COROLLA: {
(Ecu.engine, 0x7e0, None): [
b'\x01896630E88000\x00\x00\x00\x00',
+ b'\x0330ZC1200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0230ZC2100\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0230ZC2200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -595,7 +612,6 @@ FW_VERSIONS = {
b'\x0230ZC3000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0230ZC3200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0230ZC3300\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
- b'\x0330ZC1200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'881510201100\x00\x00\x00\x00',
@@ -681,6 +697,7 @@ FW_VERSIONS = {
b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+ b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
@@ -832,6 +849,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x700, None): [
b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
+ b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
@@ -852,6 +870,7 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x7e0, None): [
b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\x02353P9000\x00\x00\x00\x00\x00\x00\x00\x00553C1000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152653330\x00\x00\x00\x00\x00\x00',
@@ -863,6 +882,7 @@ FW_VERSIONS = {
b'881515306200\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
+ b'8965B53270\x00\x00\x00\x00\x00\x00',
b'8965B53271\x00\x00\x00\x00\x00\x00',
b'8965B53280\x00\x00\x00\x00\x00\x00',
b'8965B53281\x00\x00\x00\x00\x00\x00',
@@ -1079,6 +1099,7 @@ FW_VERSIONS = {
b'8965B42181\x00\x00\x00\x00\x00\x00',
b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00',
b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00',
+ b'\x028965B0R01400\x00\x00\x00\x008965B0R02400\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
@@ -1100,15 +1121,17 @@ FW_VERSIONS = {
b'\x01896634A15000\x00\x00\x00\x00',
b'\x018966342M5000\x00\x00\x00\x00',
b'\x018966342W8000\x00\x00\x00\x00',
- b'\0018966342X5000\x00\x00\x00\x00',
+ b'\x018966342X5000\x00\x00\x00\x00',
b'\x018966342X6000\x00\x00\x00\x00',
b'\x01896634A25000\x00\x00\x00\x00',
b'\x018966342W5000\x00\x00\x00\x00',
b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
b'\x02896634A13001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
+ b'\x02896634A13101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
+ b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152642291\x00\x00\x00\x00\x00\x00',
@@ -1130,6 +1153,7 @@ FW_VERSIONS = {
b'8965B42181\x00\x00\x00\x00\x00\x00',
b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00',
b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00',
+ b'\x028965B0R01400\x00\x00\x00\x008965B0R02400\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
@@ -1237,6 +1261,7 @@ FW_VERSIONS = {
CAR.LEXUS_NX: {
(Ecu.engine, 0x700, None): [
b'\x01896637851000\x00\x00\x00\x00',
+ b'\x01896637852000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152678140\x00\x00\x00\x00\x00\x00',
@@ -1246,6 +1271,7 @@ FW_VERSIONS = {
],
(Ecu.eps, 0x7a1, None): [
b'8965B78060\x00\x00\x00\x00\x00\x00',
+ b'8965B78080\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F4702300\x00\x00\x00\x00',
@@ -1294,6 +1320,7 @@ FW_VERSIONS = {
b'\x018966348W1300\x00\x00\x00\x00',
b'\x01896630EA4300\x00\x00\x00\x00',
b'\x01896630EA6300\x00\x00\x00\x00',
+ b'\x01896630EA4100\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152648472\x00\x00\x00\x00\x00\x00',
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index 73a83bb6..0010c11e 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -4,16 +4,19 @@ from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
-from selfdrive.car.volkswagen.values import DBC, CANBUS, BUTTON_STATES, CarControllerParams
+from selfdrive.car.volkswagen.values import DBC, CANBUS, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
- self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe']
+ if CP.transmissionType == TransmissionType.automatic:
+ self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe']
+ elif CP.transmissionType == TransmissionType.direct:
+ self.shifter_values = can_define.dv["EV_Gearshift"]['GearPosition']
self.buttonStates = BUTTON_STATES.copy()
- def update(self, pt_cp):
+ def update(self, pt_cp, trans_type):
ret = car.CarState.new_message()
# Update vehicle speed and acceleration from ABS wheel speeds.
ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
@@ -28,9 +31,9 @@ class CarState(CarStateBase):
# Update steering angle, rate, yaw rate, and driver input torque. VW send
# the sign/direction in a separate signal so they must be recombined.
- ret.steeringAngleDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
- ret.steeringRateDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
- ret.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1, -1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])]
+ ret.steeringAngleDeg = pt_cp.vl["LH_EPS_03"]['EPS_Berechneter_LW'] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]['EPS_VZ_BLW'])]
+ ret.steeringRateDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradw_Geschw'])]
+ ret.steeringTorque = pt_cp.vl["LH_EPS_03"]['EPS_Lenkmoment'] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]['EPS_VZ_Lenkmoment'])]
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1, -1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD
@@ -42,8 +45,16 @@ class CarState(CarStateBase):
ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
# Update gear and/or clutch position data.
- can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe'])
- ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
+ if trans_type == TransmissionType.automatic:
+ ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe'], None))
+ elif trans_type == TransmissionType.direct:
+ ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["EV_Gearshift"]['GearPosition'], None))
+ elif trans_type == TransmissionType.manual:
+ ret.clutchPressed = not pt_cp.vl["Motor_14"]['MO_Kuppl_schalter']
+ if bool(pt_cp.vl["Gateway_72"]['BCM1_Rueckfahrlicht_Schalter']):
+ ret.gearShifter = GearShifter.reverse
+ else:
+ ret.gearShifter = GearShifter.drive
# Update door and trunk/hatch lid open status.
ret.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],
@@ -60,6 +71,26 @@ class CarState(CarStateBase):
# We use the speed preference for OP.
self.displayMetricUnits = not pt_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"]
+ # Consume blind-spot monitoring info/warning LED states, if available. The
+ # info signal (LED on) is enabled whenever a vehicle is detected in the
+ # driver's blind spot. The warning signal (LED flashing) is enabled if the
+ # driver shows possibly hazardous intent toward a BSM detected vehicle, by
+ # setting the turn signal in that direction, or (for cars with factory Lane
+ # Assist) approaches the lane boundary in that direction. Size of the BSM
+ # detection box is dynamic based on speed and road curvature.
+ # Refer to VW Self Study Program 890253: Volkswagen Driver Assist Systems,
+ # pages 32-35.
+ ret.leftBlindspot = bool(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"])
+ ret.rightBlindspot = bool(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_re"])
+
+ # Stock FCW is considered active if the release bit for brake-jerk warning
+ # is set. Stock AEB considered active if the partial braking or target
+ # braking release bits are set.
+ # Refer to VW Self Study Program 890253: Volkswagen Driver Assistance
+ # Systems, chapter on Front Assist with Braking: Golf Family for all MQB
+ ret.stockFcw = bool(pt_cp.vl["ACC_10"]["AWV2_Freigabe"])
+ ret.stockAeb = bool(pt_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(pt_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"])
+
# Update ACC radar status.
accStatus = pt_cp.vl["TSK_06"]['TSK_Status']
if accStatus == 2:
@@ -77,7 +108,7 @@ class CarState(CarStateBase):
# Update ACC setpoint. When the setpoint is zero or there's an error, the
# radar sends a set-speed of ~90.69 m/s / 203mph.
- ret.cruiseState.speed = pt_cp.vl["ACC_02"]['SetSpeed']
+ ret.cruiseState.speed = pt_cp.vl["ACC_02"]['ACC_Wunschgeschw'] * CV.KPH_TO_MS
if ret.cruiseState.speed > 90:
ret.cruiseState.speed = 0
@@ -104,7 +135,7 @@ class CarState(CarStateBase):
# Check to make sure the electric power steering rack is configured to
# accept and respond to HCA_01 messages and has not encountered a fault.
- self.steeringFault = not pt_cp.vl["EPS_01"]["HCA_Ready"]
+ self.steeringFault = not pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]
# Additional safety checks performed in CarInterface.
self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well
@@ -117,8 +148,8 @@ class CarState(CarStateBase):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
- ("LWI_Lenkradwinkel", "LWI_01", 0), # Absolute steering angle
- ("LWI_VZ_Lenkradwinkel", "LWI_01", 0), # Steering angle sign
+ ("EPS_Berechneter_LW", "LH_EPS_03", 0), # Absolute steering angle
+ ("EPS_VZ_BLW", "LH_EPS_03", 0), # Steering angle sign
("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate
("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign
("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left
@@ -134,7 +165,6 @@ class CarState(CarStateBase):
("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open
("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on
("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on
- ("GE_Fahrstufe", "Getriebe_11", 0), # Auto trans gear selector position
("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver
("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger
("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed
@@ -142,9 +172,9 @@ class CarState(CarStateBase):
("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied
("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value
("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch
- ("Driver_Strain", "EPS_01", 0), # Absolute driver torque input
- ("Driver_Strain_VZ", "EPS_01", 0), # Driver torque input sign
- ("HCA_Ready", "EPS_01", 0), # Steering rack HCA support configured
+ ("EPS_Lenkmoment", "LH_EPS_03", 0), # Absolute driver torque input
+ ("EPS_VZ_Lenkmoment", "LH_EPS_03", 0), # Driver torque input sign
+ ("EPS_HCA_Status", "LH_EPS_03", 0), # Steering rack HCA support configured
("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled
("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display
("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied
@@ -152,7 +182,10 @@ class CarState(CarStateBase):
("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator
("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status
("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go)
- ("SetSpeed", "ACC_02", 0), # ACC set speed
+ ("ACC_Wunschgeschw", "ACC_02", 0), # ACC set speed
+ ("AWV2_Freigabe", "ACC_10", 0), # FCW brake jerk release
+ ("ANB_Teilbremsung_Freigabe", "ACC_10", 0), # AEB partial braking release
+ ("ANB_Zielbremsung_Freigabe", "ACC_10", 0), # AEB target braking release
("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off
("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel
("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set
@@ -164,21 +197,25 @@ class CarState(CarStateBase):
("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type
("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type
("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter
+ ("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blind spot object info, left
+ ("SWA_Warnung_SWA_li", "SWA_01", 0), # Blind spot object warning, left
+ ("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blind spot object info, right
+ ("SWA_Warnung_SWA_re", "SWA_01", 0), # Blind spot object warning, right
]
checks = [
# sig_address, frequency
("LWI_01", 100), # From J500 Steering Assist with integrated sensors
- ("EPS_01", 100), # From J500 Steering Assist with integrated sensors
+ ("LH_EPS_03", 100), # From J500 Steering Assist with integrated sensors
("ESP_19", 100), # From J104 ABS/ESP controller
("ESP_05", 50), # From J104 ABS/ESP controller
("ESP_21", 50), # From J104 ABS/ESP controller
("ACC_06", 50), # From J428 ACC radar control module
+ ("ACC_10", 50), # From J428 ACC radar control module
("Motor_20", 50), # From J623 Engine control module
("TSK_06", 50), # From J623 Engine control module
("GRA_ACC_01", 33), # From J??? steering wheel control buttons
("ACC_02", 17), # From J428 ACC radar control module
- ("Getriebe_11", 20), # From J743 Auto transmission control module
("Gateway_72", 10), # From J533 CAN gateway (aggregated data)
("Motor_14", 10), # From J623 Engine control module
("Airbag_02", 5), # From J234 Airbag control module
@@ -187,6 +224,17 @@ class CarState(CarStateBase):
("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM
]
+ if CP.transmissionType == TransmissionType.automatic:
+ signals += [("GE_Fahrstufe", "Getriebe_11", 0)] # Auto trans gear selector position
+ checks += [("Getriebe_11", 20)] # From J743 Auto transmission control module
+ elif CP.transmissionType == TransmissionType.direct:
+ signals += [("GearPosition", "EV_Gearshift", 0)] # EV gear selector position
+ checks += [("EV_Gearshift", 10)] # From J??? unknown EV control module
+ elif CP.transmissionType == TransmissionType.manual:
+ signals += [("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch
+ ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72", 0)] # Reverse light from BCM
+ checks += [("Motor_14", 10)] # From J623 Engine control module
+
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.pt)
# A single signal is monitored from the camera CAN bus, and then ignored,
@@ -197,7 +245,7 @@ class CarState(CarStateBase):
signals = [
# sig_name, sig_address, default
- ("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED
+ ("LDW_Status_LED_gruen", "LDW_02", 0), # Lane Assist status LED
]
checks = [
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index 24c078d7..21721a0c 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -1,9 +1,9 @@
from cereal import car
-from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES
+from selfdrive.swaglog import cloudlog
+from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, TransmissionType, GearShifter
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
-GEAR = car.CarState.GearShifter
EventName = car.CarEvent.EventName
@@ -25,31 +25,86 @@ class CarInterface(CarInterfaceBase):
# VW port is a community feature, since we don't own one to test
ret.communityFeature = True
- if candidate in [CAR.GOLF, CAR.AUDI_A3]:
+ if True: # pylint: disable=using-constant-test
# Set common MQB parameters that will apply globally
ret.carName = "volkswagen"
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.volkswagen
+ ret.steerActuatorDelay = 0.05
- # Additional common MQB parameters that may be overridden per-vehicle
- ret.steerRateCost = 1.0
- ret.steerActuatorDelay = 0.05 # Hopefully all MQB racks are similar here
- ret.steerLimitTimer = 0.4
+ if 0xAD in fingerprint[0]:
+ # Getriebe_11 detected: traditional automatic or DSG gearbox
+ ret.transmissionType = TransmissionType.automatic
+ elif 0x187 in fingerprint[0]:
+ # EV_Gearshift detected: e-Golf or similar direct-drive electric
+ ret.transmissionType = TransmissionType.direct
+ else:
+ # No trans message at all, must be a true stick-shift manual
+ ret.transmissionType = TransmissionType.manual
+ cloudlog.info("Detected transmission type: %s", ret.transmissionType)
- ret.lateralTuning.pid.kpBP = [0.]
- ret.lateralTuning.pid.kiBP = [0.]
+ # Global tuning defaults, can be overridden per-vehicle
+ ret.steerRateCost = 1.0
+ ret.steerLimitTimer = 0.4
+ ret.steerRatio = 15.6 # Let the params learner figure this out
+ tire_stiffness_factor = 1.0 # Let the params learner figure this out
+ ret.lateralTuning.pid.kpBP = [0.]
+ ret.lateralTuning.pid.kiBP = [0.]
+ ret.lateralTuning.pid.kf = 0.00006
+ ret.lateralTuning.pid.kpV = [0.6]
+ ret.lateralTuning.pid.kiV = [0.2]
+
+ # Per-chassis tuning values, override tuning defaults here if desired
+
+ if candidate == CAR.GOLF_MK7:
+ # Averages of all AU Golf variants
+ ret.mass = 1397 + STD_CARGO_KG
+ ret.wheelbase = 2.62
+
+ elif candidate == CAR.JETTA_MK7:
+ # Averages of all BU Jetta variants
+ ret.mass = 1328 + STD_CARGO_KG
+ ret.wheelbase = 2.71
+
+ elif candidate == CAR.PASSAT_MK8:
+ # Averages of all 3C Passat variants
+ ret.mass = 1551 + STD_CARGO_KG
+ ret.wheelbase = 2.79
+
+ elif candidate == CAR.TIGUAN_MK2:
+ # Average of SWB and LWB variants
+ ret.mass = 1715 + STD_CARGO_KG
+ ret.wheelbase = 2.74
+
+ elif candidate == CAR.AUDI_A3:
+ # Temporarily carry forward old tuning values while we test vehicle identification
ret.mass = 1500 + STD_CARGO_KG
ret.wheelbase = 2.64
- ret.centerToFront = ret.wheelbase * 0.45
- ret.steerRatio = 15.6
- ret.lateralTuning.pid.kf = 0.00006
- ret.lateralTuning.pid.kpV = [0.6]
- ret.lateralTuning.pid.kiV = [0.2]
- tire_stiffness_factor = 1.0
+
+ elif candidate == CAR.SEAT_ATECA_MK1:
+ # Averages of all 5F Ateca variants
+ ret.mass = 1900 + STD_CARGO_KG
+ ret.wheelbase = 2.64
+
+ elif candidate == CAR.SKODA_KODIAQ_MK1:
+ # Averages of all 5N Kodiaq variants
+ ret.mass = 1569 + STD_CARGO_KG
+ ret.wheelbase = 2.79
+
+ elif candidate == CAR.SKODA_SCALA_MK1:
+ # Averages of all NW Scala variants
+ ret.mass = 1192 + STD_CARGO_KG
+ ret.wheelbase = 2.65
+
+ elif candidate == CAR.SKODA_SUPERB_MK3:
+ # Averages of all 3V/NP Scala variants
+ ret.mass = 1505 + STD_CARGO_KG
+ ret.wheelbase = 2.84
+
+ ret.centerToFront = ret.wheelbase * 0.45
ret.enableCamera = True # Stock camera detection doesn't apply to VW
- ret.transmissionType = car.CarParams.TransmissionType.automatic
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
@@ -72,7 +127,7 @@ class CarInterface(CarInterfaceBase):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
- ret = self.CS.update(self.cp)
+ ret = self.CS.update(self.cp, self.CP.transmissionType)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
@@ -91,7 +146,7 @@ class CarInterface(CarInterfaceBase):
be.pressed = self.CS.buttonStates[button]
buttonEvents.append(be)
- events = self.create_common_events(ret, extra_gears=[GEAR.eco, GEAR.sport])
+ events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport])
# Vehicle health and operation safety checks
if self.CS.parkingBrakeSet:
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index f20c781c..8d85cb47 100644
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -1,6 +1,8 @@
# flake8: noqa
from selfdrive.car import dbc_dict
+from cereal import car
+Ecu = car.CarParams.Ecu
class CarControllerParams:
HCA_STEP = 2 # HCA_01 message frequency 50Hz
@@ -24,6 +26,9 @@ class CANBUS:
pt = 0
cam = 2
+TransmissionType = car.CarParams.TransmissionType
+GearShifter = car.CarState.GearShifter
+
BUTTON_STATES = {
"accelCruise": False,
"decelCruise": False,
@@ -45,20 +50,289 @@ MQB_LDW_MESSAGES = {
"laneAssistDeactivated": 10, # "Lane Assist deactivated." silent with persistent icon afterward
}
+# Check the 7th and 8th characters of the VIN before adding a new CAR. If the
+# chassis code is already listed below, don't add a new CAR, just add to the
+# FW_VERSIONS for that existing CAR.
+
class CAR:
- GOLF = "VOLKSWAGEN GOLF"
- AUDI_A3 = "AUDI A3"
+ GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants
+ JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 Jetta
+ PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 Passat and variants
+ TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants
+ SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca
+ SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN" # Chassis NS, Mk1 Skoda Kodiaq
+ SKODA_SCALA_MK1 = "SKODA SCALA 1ST GEN" # Chassis NW, Mk1 Skoda Scala and Skoda Kamiq
+ SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN" # Chassis 3V/NP, Mk3 Skoda Superb and variants
+ AUDI_A3 = "AUDI A3" # Chassis 8V/FF, Mk3 Audi A3 and variants
FINGERPRINTS = {
- CAR.GOLF: [{
- 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8
+ CAR.GOLF_MK7: [{
+ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 927: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1529: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8
+ }],
+ CAR.JETTA_MK7: [{
+ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 376: 8, 418: 8, 427: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 828: 8, 870: 8, 879: 8, 884: 8, 888: 8, 891: 8, 901: 8, 913: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1156: 8, 1157: 8, 1158: 8, 1162: 8, 1312: 8, 1343: 8, 1385: 8, 1413: 8, 1440: 5, 1471: 4, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1635: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8
+ }],
+ CAR.PASSAT_MK8: [{
+ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 295: 8, 299: 8, 302: 8, 346: 8, 385: 8, 391: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 791: 8, 792: 8, 799: 8, 802: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 828: 8, 838: 8, 839: 8, 840: 8, 841: 8, 842: 8, 843: 8, 844: 8, 845: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 927: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1438: 8, 1440: 5, 1461: 8, 1514: 8, 1515: 8, 1520: 8, 1529: 8, 1600: 8, 1601: 8, 1603: 8, 1624: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8
+ }],
+ CAR.TIGUAN_MK2: [{
+ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 376: 8, 418: 8, 427: 8, 573: 8, 679: 8, 681: 8, 684: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 828: 8, 870: 8, 879: 8, 884: 8, 888: 8, 891: 8, 896: 8, 897: 8, 898: 8, 901: 8, 913: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1156: 8, 1157: 8, 1158: 8, 1162: 8, 1175: 8, 1312: 8, 1343: 8, 1385: 8, 1413: 8, 1440: 5, 1471: 4, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1635: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8
}],
CAR.AUDI_A3: [{
64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 285: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 295: 8, 299: 8, 302: 8, 346: 8, 418: 8, 427: 8, 506: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 792: 8, 802: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 846: 8, 847: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1624: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8, 1792: 8, 1872: 8, 1976: 8, 1977: 8, 1982: 8, 1985: 8
}],
+ CAR.SEAT_ATECA_MK1: [{
+ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 684: 8, 779: 8, 780: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 901: 8, 917: 8, 919: 8, 927: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8
+ }],
+ CAR.SKODA_KODIAQ_MK1: [{
+ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 573: 8, 668: 8, 679: 8, 681: 8, 684: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 792: 8, 795: 8, 802: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 828: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1529: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8, 1792: 8, 1871: 8, 1872: 8, 1879: 8, 1909: 8, 1976: 8, 1977: 8, 1985: 8
+ }],
+ CAR.SKODA_SCALA_MK1: [{
+ 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 262: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 418: 8, 427: 8, 506: 8, 568: 8, 569: 8, 572: 8, 573: 8, 679: 8, 681: 8, 684: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 826: 8, 827: 8, 828: 8, 870: 8, 879: 8, 884: 8, 888: 8, 891: 8, 901: 8, 913: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1156: 8, 1157: 8, 1158: 8, 1162: 8, 1175: 8, 1312: 8, 1343: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1635: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8, 1792: 8, 1872: 8, 1879: 8, 1976: 8, 1977: 8, 1982: 8, 1985: 8
+ }],
+ CAR.SKODA_SUPERB_MK3: [{
+ 64: 8, 134: 8, 159: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 295: 8, 299: 8, 302: 8, 346: 8, 418: 8, 427: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 791: 8, 792: 8, 795: 8, 799: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 838: 8, 839: 8, 840: 8, 841: 8, 842: 8, 843: 8, 844: 8, 845: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1792: 8, 1872: 8, 1879: 8, 1976: 8, 1977: 8, 1985: 8, 2017: 8
+ }],
+}
+
+IGNORED_FINGERPRINTS = [CAR.JETTA_MK7, CAR.PASSAT_MK8, CAR.TIGUAN_MK2, CAR.SEAT_ATECA_MK1, CAR.SKODA_KODIAQ_MK1, CAR.SKODA_SCALA_MK1, CAR.SKODA_SUPERB_MK3]
+
+FW_VERSIONS = {
+ CAR.AUDI_A3: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x875G0906259L \xf1\x890002',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870D9300013B \xf1\x894931',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x875Q0907572G \xf1\x890571',
+ ],
+ },
+ CAR.GOLF_MK7: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704E906016A \xf1\x897697',
+ b'\xf1\x8704E906016AD\xf1\x895758',
+ b'\xf1\x8704E906023AG\xf1\x891726',
+ b'\xf1\x8704E906023BN\xf1\x894518',
+ b'\xf1\x8704E906027GR\xf1\x892394',
+ b'\xf1\x8704L906026NF\xf1\x899528',
+ b'\xf1\x8704L906056HE\xf1\x893758',
+ b'\xf1\x870EA906016A \xf1\x898343',
+ b'\xf1\x870EA906016S \xf1\x897207',
+ b'\xf1\x875G0906259 \xf1\x890007',
+ b'\xf1\x875G0906259J \xf1\x890002',
+ b'\xf1\x875G0906259L \xf1\x890002',
+ b'\xf1\x875G0906259Q \xf1\x890002',
+ b'\xf1\x878V0906259P \xf1\x890001',
+ b'\xf1\x878V0906259Q \xf1\x890002',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870CW300045 \xf1\x894531',
+ b'\xf1\x870D9300040S \xf1\x894311',
+ b'\xf1\x870CW300047D \xf1\x895261',
+ b'\xf1\x870D9300012 \xf1\x894913',
+ b'\xf1\x870CW300042F \xf1\x891604',
+ b'\xf1\x870DD300046F \xf1\x891601',
+ b'\xf1\x870D9300020S \xf1\x895201',
+ b'\xf1\x870GC300012A \xf1\x891403',
+ b'\xf1\x870GC300043T \xf1\x899999',
+ b'\xf1\x870GC300020G \xf1\x892404',
+ b'\xf1\x870GC300014B \xf1\x892405',
+ b'\xf1\x870DD300045K \xf1\x891120',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023271212111312--071104171838103891131211',
+ b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13272512111312--07110417182C102C91131211',
+ b'\xf1\x875Q0959655M \xf1\x890361\xf1\x82\0211413001112120041114115121611169112',
+ b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02324230011211200621143171724112491132111',
+ b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011211200621143171717111791132111',
+ b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011211200061104171717101791132111',
+ b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02324230011211200061104171724102491132111',
+ b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\0211413001113120053114317121C111C9113',
+ b'\xf1\x875Q0959655AA\xf1\x890386\xf1\x82\0211413001113120043114317121C111C9113',
+ b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120053114317121C111C9113',
+ b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120043114417121411149113',
+ b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02314160011123300314211012230229333463100',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\00561A01612A0',
+ b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566A0J612A1',
+ b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1',
+ b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\00571A0JA16A1',
+ b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820519A9040203',
+ b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00522A00402A0',
+ b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511A00403A0',
+ b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516A07A02A1',
+ b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\00521A00441A1',
+ b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00641A1',
+ b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A00642A1',
+ b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A07B05A1',
+ b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A17A1',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\00101',
+ b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101',
+ b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\00101',
+ b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\00101',
+ b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101',
+ b'\xf1\x875Q0907572H \xf1\x890620',
+ b'\xf1\x875Q0907572J \xf1\x890654',
+ b'\xf1\x875Q0907572P \xf1\x890682',
+ ],
+ },
+ CAR.JETTA_MK7: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704E906024B \xf1\x895594',
+ b'\xf1\x8704E906024L \xf1\x895595',
+ b'\xf1\x8704E906024AK\xf1\x899937',
+ b'\xf1\x875G0906259T \xf1\x890003',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x8709S927158R \xf1\x893552',
+ b'\xf1\x8709S927158R \xf1\x893587',
+ b'\xf1\x870GC300020N \xf1\x892803',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655AG\xf1\x890336\xf1\x82\02314171231313500314611011630169333463100',
+ b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02311170031313300314240011150119333433100',
+ b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314643011650169333463100',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\00521A10A01A1',
+ b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521B00404A1',
+ b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A10A01A1',
+ b'\xf1\x875QN909144B \xf1\x895082\xf1\x82\00571A10A11A1',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x875Q0907572N \xf1\x890681',
+ b'\xf1\x875Q0907572R \xf1\x890771',
+ ],
+ },
+ CAR.PASSAT_MK8: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704E906023AH\xf1\x893379',
+ b'\xf1\x8704L906026GA\xf1\x892013',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870DD300045T \xf1\x891601',
+ b'\xf1\x870D9300014L \xf1\x895002',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111',
+ b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803',
+ b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x875Q0907572R \xf1\x890771',
+ b'\xf1\x873Q0907572C \xf1\x890195',
+ ],
+ },
+ CAR.TIGUAN_MK2: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8783A907115B \xf1\x890005',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x8709G927158DT\xf1\x893698',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572R \xf1\x890372',
+ ],
+ },
+ CAR.SEAT_ATECA_MK1: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704E906027KA\xf1\x893749',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870D9300014S \xf1\x895202',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0161212001211001305121211052900',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\00571N60511A1',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572M \xf1\x890233',
+ ],
+ },
+ CAR.SKODA_KODIAQ_MK1: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704E906027DD\xf1\x893123',
+ b'\xf1\x875NA907115E \xf1\x890003',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870D9300043 \xf1\x895202',
+ b'\xf1\x870DL300012M \xf1\x892107',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\0161213001211001205212111052100',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405',
+ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6060405',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572R \xf1\x890372',
+ ],
+ },
+ CAR.SKODA_SCALA_MK1: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704C906025AK\xf1\x897053',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870CW300050 \xf1\x891709',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\022111104111104112104040404111111112H14',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x872Q1909144M \xf1\x896041',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572R \xf1\x890372',
+ ],
+ },
+ CAR.SKODA_SUPERB_MK3: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704L906026KB\xf1\x894071',
+ ],
+ # Only onboarded Superb so far is a manual (Ecu.transmission, 0x7e1, None): [],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x873Q0907572C \xf1\x890195',
+ ],
+ },
}
DBC = {
- CAR.GOLF: dbc_dict('vw_mqb_2010', None),
+ CAR.GOLF_MK7: dbc_dict('vw_mqb_2010', None),
+ CAR.JETTA_MK7: dbc_dict('vw_mqb_2010', None),
+ CAR.PASSAT_MK8: dbc_dict('vw_mqb_2010', None),
+ CAR.TIGUAN_MK2: dbc_dict('vw_mqb_2010', None),
CAR.AUDI_A3: dbc_dict('vw_mqb_2010', None),
+ CAR.SEAT_ATECA_MK1: dbc_dict('vw_mqb_2010', None),
+ CAR.SKODA_KODIAQ_MK1: dbc_dict('vw_mqb_2010', None),
+ CAR.SKODA_SCALA_MK1: dbc_dict('vw_mqb_2010', None),
+ CAR.SKODA_SUPERB_MK3: dbc_dict('vw_mqb_2010', None),
}
diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py
index 3cd47a09..4d0eaec6 100644
--- a/selfdrive/car/volkswagen/volkswagencan.py
+++ b/selfdrive/car/volkswagen/volkswagencan.py
@@ -25,12 +25,13 @@ def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert
rightlanehud = 2 if rightLaneVisible else 1
values = {
- "LDW_Unknown": 2, # FIXME: possible speed or attention relationship
- "Kombi_Lamp_Orange": 1 if hca_enabled and steering_pressed else 0,
- "Kombi_Lamp_Green": 1 if hca_enabled and not steering_pressed else 0,
- "Left_Lane_Status": leftlanehud,
- "Right_Lane_Status": rightlanehud,
- "Alert_Message": hud_alert,
+ "LDW_SW_Warnung_links": 0, # FIXME: to be store-and-forwarded from the stock camera
+ "LDW_SW_Warnung_rechts": 1, # FIXME: to be store-and-forwarded from the stock camera
+ "LDW_Status_LED_gelb": 1 if hca_enabled and steering_pressed else 0,
+ "LDW_Status_LED_gruen": 1 if hca_enabled and not steering_pressed else 0,
+ "LDW_Lernmodus_links": leftlanehud,
+ "LDW_Lernmodus_rechts": rightlanehud,
+ "LDW_Texte": hud_alert,
}
return packer.make_can_msg("LDW_02", bus, values)
diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript
index ec3603d3..fb36e2b9 100644
--- a/selfdrive/common/SConscript
+++ b/selfdrive/common/SConscript
@@ -5,7 +5,14 @@ if SHARED:
else:
fxn = env.Library
-common_libs = ['params.cc', 'swaglog.cc', 'util.cc', 'gpio.cc', 'i2c.cc']
+common_libs = [
+ 'params.cc',
+ 'swaglog.cc',
+ 'util.cc',
+ 'gpio.cc',
+ 'i2c.cc',
+ 'watchdog.cc',
+]
_common = fxn('common', common_libs, LIBS="json11")
diff --git a/selfdrive/common/framebuffer.cc b/selfdrive/common/framebuffer.cc
index b006479a..62ac7053 100644
--- a/selfdrive/common/framebuffer.cc
+++ b/selfdrive/common/framebuffer.cc
@@ -48,9 +48,6 @@ void FrameBuffer::set_power(int mode) {
}
FrameBuffer::FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w, int *out_h) {
- status_t status;
- int success;
-
s = new FramebufferState;
s->session = new SurfaceComposerClient();
@@ -60,7 +57,7 @@ FrameBuffer::FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w
ISurfaceComposer::eDisplayIdMain);
assert(s->dtoken != NULL);
- status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
+ status_t status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
assert(status == 0);
//int orientation = 3; // rotate framebuffer 270 degrees
@@ -106,7 +103,7 @@ FrameBuffer::FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w
s->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
assert(s->display != EGL_NO_DISPLAY);
- success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
+ int success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
assert(success);
printf("egl version %d.%d\n", s->egl_major, s->egl_minor);
diff --git a/selfdrive/common/framebuffer.h b/selfdrive/common/framebuffer.h
index 1285b67c..d60424d6 100644
--- a/selfdrive/common/framebuffer.h
+++ b/selfdrive/common/framebuffer.h
@@ -1,35 +1,9 @@
#pragma once
-
#include
+#include "hardware/hwcomposer_defs.h"
bool set_brightness(int brightness);
-/* Display power modes */
-enum {
- /* The display is turned off (blanked). */
- HWC_POWER_MODE_OFF = 0,
- /* The display is turned on and configured in a low power state
- * that is suitable for presenting ambient information to the user,
- * possibly with lower fidelity than normal but greater efficiency. */
- HWC_POWER_MODE_DOZE = 1,
- /* The display is turned on normally. */
- HWC_POWER_MODE_NORMAL = 2,
- /* The display is configured as in HWC_POWER_MODE_DOZE but may
- * stop applying frame buffer updates from the graphics subsystem.
- * This power mode is effectively a hint from the doze dream to
- * tell the hardware that it is done drawing to the display for the
- * time being and that the display should remain on in a low power
- * state and continue showing its current contents indefinitely
- * until the mode changes.
- *
- * This mode may also be used as a signal to enable hardware-based doze
- * functionality. In this case, the doze dream is effectively
- * indicating that the hardware is free to take over the display
- * and manage it autonomously to implement low power always-on display
- * functionality. */
- HWC_POWER_MODE_DOZE_SUSPEND = 3,
-};
-
struct FramebufferState;
class FrameBuffer {
public:
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index 8d41b2b3..fbbd9ce4 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -16,3 +16,37 @@ const double X_IDXS[TRAJECTORY_SIZE] = { 0. , 0.1875, 0.75 , 1.6875,
60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875,
108. , 117.1875, 126.75 , 136.6875, 147. , 157.6875,
168.75 , 180.1875, 192.};
+
+#ifdef __cplusplus
+
+#include "common/mat.h"
+#ifdef QCOM2
+const mat3 fcam_intrinsic_matrix = (mat3){{
+ 2648.0, 0.0, 1928.0/2,
+ 0.0, 2648.0, 1208.0/2,
+ 0.0, 0.0, 1.0
+}};
+#else
+const mat3 fcam_intrinsic_matrix = (mat3){{
+ 910., 0., 1164.0/2,
+ 0., 910., 874.0/2,
+ 0., 0., 1.
+}};
+#endif
+
+static inline mat3 get_model_yuv_transform(bool bayer = true) {
+#ifndef QCOM2
+ float db_s = 0.5; // debayering does a 2x downscale
+#else
+ float db_s = 1.0;
+#endif
+ const mat3 transform = (mat3){{
+ 1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, 1.0
+ }};
+ return bayer ? transform_scale_buffer(transform, db_s) : transform;
+}
+
+#endif
+
diff --git a/selfdrive/common/util.cc b/selfdrive/common/util.cc
index 49f53db3..a9db1a15 100644
--- a/selfdrive/common/util.cc
+++ b/selfdrive/common/util.cc
@@ -1,11 +1,7 @@
-#include
-#include
-#include
-#include
-#include
-#include
#include
+#include "common/util.h"
+
#ifdef __linux__
#include
#include
@@ -45,8 +41,8 @@ void* read_file(const char* path, size_t* out_len) {
return buf;
}
-int write_file(const char* path, const void* data, size_t size) {
- int fd = open(path, O_WRONLY);
+int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) {
+ int fd = open(path, flags, mode);
if (fd == -1) {
return -1;
}
diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h
index b3dfae7e..b75c2f43 100644
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@@ -1,8 +1,12 @@
#pragma once
-#include
-#include
+#include
#include
+#include
+#include
+#include
+#include
+
#include
#include
#include
@@ -10,6 +14,7 @@
#include
#include
#include
+#include
#ifndef sighandler_t
typedef void (*sighandler_t)(int sig);
@@ -25,7 +30,7 @@ typedef void (*sighandler_t)(int sig);
// Returns NULL on failure, otherwise the NULL-terminated file contents.
// The result must be freed by the caller.
void* read_file(const char* path, size_t* out_len);
-int write_file(const char* path, const void* data, size_t size);
+int write_file(const char* path, const void* data, size_t size, int flags=O_WRONLY, mode_t mode=0777);
void set_thread_name(const char* name);
@@ -34,6 +39,19 @@ int set_core_affinity(int core);
namespace util {
+// ***** math helpers *****
+
+// map x from [a1, a2] to [b1, b2]
+template
+T map_val(T x, T a1, T a2, T b1, T b2) {
+ x = std::clamp(x, a1, a2);
+ T ra = a2 - a1;
+ T rb = b2 - b1;
+ return (x - a1)*rb / ra + b1;
+}
+
+// ***** string helpers *****
+
inline bool starts_with(const std::string &s, const std::string &prefix) {
return s.compare(0, prefix.size(), prefix) == 0;
}
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 978ae635..6646026b 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.8.2-280192ed-2021-02-23T16:46:20"
+#define COMMA_VERSION "0.8.3-9896438d-2021-03-29T23:54:19"
diff --git a/selfdrive/common/watchdog.cc b/selfdrive/common/watchdog.cc
new file mode 100644
index 00000000..2e8afb39
--- /dev/null
+++ b/selfdrive/common/watchdog.cc
@@ -0,0 +1,17 @@
+#include
+#include
+#include
+
+#include "common/timing.h"
+#include "common/util.h"
+#include "common/watchdog.h"
+
+const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // +
+
+bool watchdog_kick(){
+ std::string fn = watchdog_fn_prefix + std::to_string(getpid());
+ std::string cur_t = std::to_string(nanos_since_boot());
+
+ int r = write_file(fn.c_str(), cur_t.data(), cur_t.length(), O_WRONLY | O_CREAT);
+ return r == 0;
+}
diff --git a/selfdrive/common/watchdog.h b/selfdrive/common/watchdog.h
new file mode 100644
index 00000000..7ed23aa0
--- /dev/null
+++ b/selfdrive/common/watchdog.h
@@ -0,0 +1,3 @@
+#pragma once
+
+bool watchdog_kick();
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 58a9ac63..9b37834b 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import os
+import math
from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL
@@ -16,6 +17,7 @@ from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEE
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
+from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -30,12 +32,11 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
SIMULATION = "SIMULATION" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
-IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned"])
+IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"])
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
PandaType = log.PandaState.PandaType
-LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
Desire = log.LateralPlan.Desire
LaneChangeState = log.LateralPlan.LaneChangeState
LaneChangeDirection = log.LateralPlan.LaneChangeDirection
@@ -102,7 +103,9 @@ class Controls:
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
- if self.CP.lateralTuning.which() == 'pid':
+ if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+ self.LaC = LatControlAngle(self.CP)
+ elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
@@ -131,6 +134,7 @@ class Controls:
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
+ self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized, controller_available)
@@ -140,6 +144,8 @@ class Controls:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
+ elif self.read_only:
+ self.events.add(EventName.dashcamMode, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
@@ -202,14 +208,17 @@ class Controls:
if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL):
self.events.add(EventName.canError)
- if (self.sm['pandaState'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \
- self.mismatch_counter >= 200:
+
+ safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel
+ safety_mismatch = safety_mismatch or self.sm['pandaState'].safetyParam != self.CP.safetyParam
+ if (safety_mismatch and self.sm.frame > 2 / DT_CTRL) or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
+ if not self.sm['liveParameters'].valid:
+ self.events.add(EventName.vehicleModelInvalid)
+
if len(self.sm['radarState'].radarErrors):
self.events.add(EventName.radarFault)
- elif not self.sm.valid['liveParameters']:
- self.events.add(EventName.vehicleModelInvalid)
elif not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue)
if not self.logged_comm_issue:
@@ -235,9 +244,7 @@ class Controls:
# TODO: fix simulator
if not SIMULATION:
if not NOSENSOR:
- if not self.sm.alive['ubloxRaw'] and (self.sm.frame > 10. / DT_CTRL):
- self.events.add(EventName.gpsMalfunction)
- elif not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and not TICI:
+ if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and not TICI:
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
self.events.add(EventName.noGps)
if not self.sm.all_alive(['roadCameraState', 'driverCameraState']) and (self.sm.frame > 5 / DT_CTRL):
@@ -365,6 +372,12 @@ class Controls:
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
+ # Update VehicleModel
+ params = self.sm['liveParameters']
+ x = max(params.stiffnessFactor, 0.1)
+ sr = max(params.steerRatio, 0.1)
+ self.VM.update_params(x, sr)
+
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
@@ -388,8 +401,9 @@ class Controls:
# Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol, self.CP)
+
# Steering PID loop and lateral MPC
- actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, lat_plan)
+ actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, self.VM, params, lat_plan)
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
@@ -403,12 +417,14 @@ class Controls:
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
- # Check if we deviated from the path
- left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1
- right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1
- if left_deviation or right_deviation:
- self.events.add(EventName.steerSaturated)
+ if len(lat_plan.dPathPoints):
+ # Check if we deviated from the path
+ left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1
+ right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1
+
+ if left_deviation or right_deviation:
+ self.events.add(EventName.steerSaturated)
return actuators, v_acc_sol, a_acc_sol, lac_log
@@ -470,7 +486,14 @@ class Controls:
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
- steer_angle_rad = (CS.steeringAngleDeg - self.sm['lateralPlan'].angleOffsetDeg) * CV.DEG_TO_RAD
+ # Curvature & Steering angle
+ params = self.sm['liveParameters']
+ lat_plan = self.sm['lateralPlan']
+
+ steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
+ curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo)
+ angle_steers_des = math.degrees(self.VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))
+ angle_steers_des += params.angleOffsetDeg
# controlsState
dat = messaging.new_message('controlsState')
@@ -488,7 +511,8 @@ class Controls:
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
- controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo)
+ controlsState.curvature = curvature
+ controlsState.steeringAngleDesiredDeg = angle_steers_des
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
@@ -497,7 +521,6 @@ class Controls:
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
- controlsState.steeringAngleDesiredDeg = float(self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc)
controlsState.cumLagMs = -self.rk.remaining * 1000.
@@ -505,7 +528,9 @@ class Controls:
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
- if self.CP.lateralTuning.which() == 'pid':
+ if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+ controlsState.lateralControlState.angleState = lac_log
+ elif self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 49336c45..12fd6519 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -253,12 +253,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
},
- EventName.startupOneplus: {
+ EventName.dashcamMode: {
ET.PERMANENT: Alert(
- "WARNING: Original EON deprecated",
- "Device will no longer update",
- AlertStatus.userPrompt, AlertSize.mid,
- Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
+ "Dashcam Mode",
+ "",
+ AlertStatus.normal, AlertSize.small,
+ Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
},
EventName.invalidLkasSetting: {
@@ -329,6 +329,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
},
EventName.vehicleModelInvalid: {
+ ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"),
ET.WARNING: Alert(
"Vehicle Parameter Identification Failed",
"",
@@ -731,7 +732,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.speedTooHigh: {
ET.WARNING: Alert(
"Speed Too High",
- "Slow down to resume operation",
+ "Model uncertain at this speed",
AlertStatus.normal, AlertSize.mid,
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.none, 2.2, 3., 4.),
ET.NO_ENTRY: Alert(
diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py
index bb3ba5f5..200ab5d3 100644
--- a/selfdrive/controls/lib/lane_planner.py
+++ b/selfdrive/controls/lib/lane_planner.py
@@ -8,10 +8,13 @@ TRAJECTORY_SIZE = 33
# camera offset is meters from center car to camera
if EON:
CAMERA_OFFSET = 0.06
+ PATH_OFFSET = 0.0
elif TICI:
CAMERA_OFFSET = -0.04
+ PATH_OFFSET = -0.04
else:
CAMERA_OFFSET = 0.0
+ PATH_OFFSET = 0.0
@@ -56,6 +59,7 @@ class LanePlanner:
def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or
# will be in a few seconds
+ path_xyz[:,1] -= PATH_OFFSET
l_prob, r_prob = self.lll_prob, self.rll_prob
width_pts = self.rll_y - self.lll_y
prob_mods = []
diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py
new file mode 100644
index 00000000..eab023b9
--- /dev/null
+++ b/selfdrive/controls/lib/latcontrol_angle.py
@@ -0,0 +1,25 @@
+import math
+from cereal import log
+
+
+class LatControlAngle():
+ def __init__(self, CP):
+ pass
+
+ def reset(self):
+ pass
+
+ def update(self, active, CS, CP, VM, params, lat_plan):
+ angle_log = log.ControlsState.LateralAngleState.new_message()
+
+ if CS.vEgo < 0.3 or not active:
+ angle_log.active = False
+ angle_steers_des = float(CS.steeringAngleDeg)
+ else:
+ angle_log.active = True
+ angle_steers_des = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))
+ angle_steers_des += params.angleOffsetDeg
+
+ angle_log.saturated = False
+ angle_log.steeringAngleDeg = angle_steers_des
+ return 0, float(angle_steers_des), angle_log
diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py
index ca0532e2..ba398789 100644
--- a/selfdrive/controls/lib/latcontrol_indi.py
+++ b/selfdrive/controls/lib/latcontrol_indi.py
@@ -80,7 +80,7 @@ class LatControlINDI():
return self.sat_count > self.sat_limit
- def update(self, active, CS, CP, lat_plan):
+ def update(self, active, CS, CP, VM, params, lat_plan):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
@@ -96,11 +96,10 @@ class LatControlINDI():
self.output_steer = 0.0
self.delayed_output = 0.0
else:
- self.angle_steers_des = lat_plan.steeringAngleDeg
- self.rate_steers_des = lat_plan.steeringRateDeg
+ steers_des = VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)
+ steers_des += math.radians(params.angleOffsetDeg)
- steers_des = math.radians(self.angle_steers_des)
- rate_des = math.radians(self.rate_steers_des)
+ rate_des = VM.get_steer_from_curvature(-lat_plan.curvatureRate, CS.vEgo)
# Expected actuator value
alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
@@ -143,4 +142,4 @@ class LatControlINDI():
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
- return float(self.output_steer), float(self.angle_steers_des), indi_log
+ return float(self.output_steer), 0, indi_log
diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py
index 1d9356be..03464ea5 100644
--- a/selfdrive/controls/lib/latcontrol_lqr.py
+++ b/selfdrive/controls/lib/latcontrol_lqr.py
@@ -1,4 +1,6 @@
+import math
import numpy as np
+
from selfdrive.controls.lib.drive_helpers import get_steer_max
from common.numpy_fast import clip
from common.realtime import DT_CTRL
@@ -28,7 +30,6 @@ class LatControlLQR():
def reset(self):
self.i_lqr = 0.0
- self.output_steer = 0.0
self.sat_count = 0.0
def _check_saturation(self, control, check_saturation, limit):
@@ -43,39 +44,42 @@ class LatControlLQR():
return self.sat_count > self.sat_limit
- def update(self, active, CS, CP, lat_plan):
+ def update(self, active, CS, CP, VM, params, lat_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
- steering_angle = CS.steeringAngleDeg
-
# Subtract offset. Zero angle should correspond to zero torque
- self.angle_steers_des = lat_plan.steeringAngleDeg - lat_plan.angleOffsetDeg
- steering_angle -= lat_plan.angleOffsetDeg
+ steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
+
+ desired_angle = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))
+
+ instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
+ desired_angle += instant_offset # Only add offset that originates from vehicle model errors
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
- e = steering_angle - angle_steers_k
+ e = steering_angle_no_offset - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
if CS.vEgo < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
+ output_steer = 0.
self.reset()
else:
lqr_log.active = True
# LQR
- u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
+ u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
- error = self.angle_steers_des - angle_steers_k
+ error = desired_angle - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
@@ -83,15 +87,15 @@ class LatControlLQR():
(error <= 0 and (control >= -steers_max or i > 0.0)):
self.i_lqr = i
- self.output_steer = lqr_output + self.i_lqr
- self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+ output_steer = lqr_output + self.i_lqr
+ output_steer = clip(output_steer, -steers_max, steers_max)
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
- saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
+ saturated = self._check_saturation(output_steer, check_saturation, steers_max)
- lqr_log.steeringAngleDeg = angle_steers_k + lat_plan.angleOffsetDeg
+ lqr_log.steeringAngleDeg = angle_steers_k + params.angleOffsetAverageDeg
lqr_log.i = self.i_lqr
- lqr_log.output = self.output_steer
+ lqr_log.output = output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = saturated
- return self.output_steer, float(self.angle_steers_des), lqr_log
+ return output_steer, 0, lqr_log
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index be521b0c..65f2c6fb 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -1,6 +1,7 @@
+import math
+
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.drive_helpers import get_steer_max
-from cereal import car
from cereal import log
@@ -10,35 +11,35 @@ class LatControlPID():
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
- self.angle_steers_des = 0.
def reset(self):
self.pid.reset()
- def update(self, active, CS, CP, lat_plan):
+ def update(self, active, CS, CP, VM, params, lat_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
+ angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))
+ angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
- self.angle_steers_des = lat_plan.steeringAngleDeg # get from MPC/LateralPlanner
-
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
- steer_feedforward = self.angle_steers_des # feedforward desired angle
- if CP.steerControlType == car.CarParams.SteerControlType.torque:
- # TODO: feedforward something based on lat_plan.rateSteers
- steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque
- steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
+
+ # TODO: feedforward something based on lat_plan.rateSteers
+ steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque
+ steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
+
deadzone = 0.0
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
- output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
+ output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
@@ -47,4 +48,4 @@ class LatControlPID():
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
- return output_steer, float(self.angle_steers_des), pid_log
+ return output_steer, 0, pid_log
diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp
index 96488f4c..ca4be4c2 100644
--- a/selfdrive/controls/lib/lateral_mpc/generator.cpp
+++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp
@@ -1,8 +1,8 @@
+#include
#include
#include "common/modeldata.h"
-#define PI 3.1415926536
-#define deg2rad(d) (d/180.0*PI)
+#define deg2rad(d) (d/180.0*M_PI)
const int N_steps = 16;
using namespace std;
@@ -23,7 +23,7 @@ int main( )
OnlineData rotation_radius;
Control curvature_rate;
-
+
// Equations of motion
f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * curvature);
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
index ff4fd420..8da44f4f 100644
--- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
@@ -24,8 +24,8 @@ typedef struct {
double x[N+1];
double y[N+1];
double psi[N+1];
- double tire_angle[N+1];
- double tire_angle_rate[N];
+ double curvature[N+1];
+ double curvature_rate[N];
double cost;
} log_t;
@@ -95,9 +95,9 @@ int run_mpc(state_t * x0, log_t * solution, double v_ego,
solution->x[i] = acadoVariables.x[i*NX];
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
- solution->tire_angle[i] = acadoVariables.x[i*NX+3];
+ solution->curvature[i] = acadoVariables.x[i*NX+3];
if (i < N){
- solution->tire_angle_rate[i] = acadoVariables.u[i];
+ solution->curvature_rate[i] = acadoVariables.u[i];
}
}
solution->cost = acado_getObjective();
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 724f93ff..3b7772df 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -1,14 +1,14 @@
import os
import math
import numpy as np
+from common.params import Params
from common.realtime import sec_since_boot, DT_MDL
-from common.numpy_fast import interp
+from common.numpy_fast import interp, clip
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
from selfdrive.config import Conversions as CV
-from common.params import Params
import cereal.messaging as messaging
from cereal import log
@@ -19,6 +19,9 @@ LOG_MPC = os.environ.get('LOG_MPC', False)
LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
LANE_CHANGE_TIME_MAX = 10.
+# this corresponds to 80deg/s and 20deg/s steering angle in a toyota corolla
+MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992]
+MAX_CURVATURE_RATE_SPEEDS = [0, 35]
DESIRES = {
LaneChangeDirection.none: {
@@ -51,7 +54,7 @@ class LateralPlanner():
self.setup_mpc()
self.solution_invalid_cnt = 0
- self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1'
+ self.use_lanelines = Params().get('EndToEndToggle') != b'1'
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
@@ -75,23 +78,15 @@ class LateralPlanner():
self.cur_state[0].psi = 0.0
self.cur_state[0].curvature = 0.0
- self.angle_steers_des = 0.0
- self.angle_steers_des_mpc = 0.0
- self.angle_steers_des_time = 0.0
+ self.desired_curvature = 0.0
+ self.safe_desired_curvature = 0.0
+ self.desired_curvature_rate = 0.0
+ self.safe_desired_curvature_rate = 0.0
- def update(self, sm, CP, VM):
+ def update(self, sm, CP):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
- steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffsetDeg
- steering_wheel_angle_deg = sm['carState'].steeringAngleDeg
-
- # Update vehicle model
- x = max(sm['liveParameters'].stiffnessFactor, 0.1)
- sr = max(sm['liveParameters'].steerRatio, 0.1)
- VM.update_params(x, sr)
- curvature_factor = VM.curvature_factor(v_ego)
- measured_curvature = -curvature_factor * math.radians(steering_wheel_angle_deg - steering_wheel_angle_offset_deg) / VM.sR
-
+ measured_curvature = sm['controlsState'].curvature
md = sm['modelV2']
self.LP.parse_model(sm['modelV2'])
@@ -109,7 +104,7 @@ class LateralPlanner():
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
- if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not self.lane_change_enabled):
+ if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
@@ -165,9 +160,12 @@ class LateralPlanner():
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob
- d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
- y_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
- heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
+ if self.use_lanelines:
+ d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
+ else:
+ d_path_xyz = self.path_xyz
+ y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
+ heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
self.y_pts = y_pts
assert len(y_pts) == MPC_N + 1
@@ -181,31 +179,29 @@ class LateralPlanner():
self.cur_state.x = 0.0
self.cur_state.y = 0.0
self.cur_state.psi = 0.0
- self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature)
+ self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N + 1], self.mpc_solution.curvature)
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
current_curvature = self.mpc_solution.curvature[0]
- psi = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.psi)
+ psi = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.psi)
next_curvature_rate = self.mpc_solution.curvature_rate[0]
# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
- curvature_diff_from_psi = psi/(max(v_ego, 1e-1) * delay) - current_curvature
- next_curvature = current_curvature + 2*curvature_diff_from_psi
+ curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature
+ next_curvature = current_curvature + 2 * curvature_diff_from_psi
- # reset to current steer angle if not active or overriding
- if active:
- curvature_desired = next_curvature
- desired_curvature_rate = next_curvature_rate
- else:
- curvature_desired = measured_curvature
- desired_curvature_rate = 0.0
-
- # negative sign, controls uses different convention
- self.desired_steering_wheel_angle_deg = -float(math.degrees(curvature_desired * VM.sR)/curvature_factor) + steering_wheel_angle_offset_deg
- self.desired_steering_wheel_angle_rate_deg = -float(math.degrees(desired_curvature_rate * VM.sR)/curvature_factor)
+ self.desired_curvature = next_curvature
+ self.desired_curvature_rate = next_curvature_rate
+ max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES)
+ self.safe_desired_curvature_rate = clip(self.desired_curvature_rate,
+ -max_curvature_rate,
+ max_curvature_rate)
+ self.safe_desired_curvature = clip(self.desired_curvature,
+ self.safe_desired_curvature - max_curvature_rate/DT_MDL,
+ self.safe_desired_curvature + max_curvature_rate/DT_MDL)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature)
@@ -226,16 +222,18 @@ class LateralPlanner():
def publish(self, sm, pm):
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('lateralPlan')
- plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2'])
+ plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
plan_send.lateralPlan.lProb = float(self.LP.lll_prob)
plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
plan_send.lateralPlan.dProb = float(self.LP.d_prob)
- plan_send.lateralPlan.steeringAngleDeg = float(self.desired_steering_wheel_angle_deg)
- plan_send.lateralPlan.steeringRateDeg = float(self.desired_steering_wheel_angle_rate_deg)
- plan_send.lateralPlan.angleOffsetDeg = float(sm['liveParameters'].angleOffsetAverageDeg)
+ plan_send.lateralPlan.rawCurvature = float(self.desired_curvature)
+ plan_send.lateralPlan.rawCurvatureRate = float(self.desired_curvature_rate)
+ plan_send.lateralPlan.curvature = float(self.safe_desired_curvature)
+ plan_send.lateralPlan.curvatureRate = float(self.safe_desired_curvature_rate)
+
plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.lateralPlan.desire = self.desire
@@ -246,9 +244,9 @@ class LateralPlanner():
if LOG_MPC:
dat = messaging.new_message('liveMpc')
- dat.liveMpc.x = list(self.mpc_solution[0].x)
- dat.liveMpc.y = list(self.mpc_solution[0].y)
- dat.liveMpc.psi = list(self.mpc_solution[0].psi)
- dat.liveMpc.tire_angle = list(self.mpc_solution[0].tire_angle)
- dat.liveMpc.cost = self.mpc_solution[0].cost
+ dat.liveMpc.x = list(self.mpc_solution.x)
+ dat.liveMpc.y = list(self.mpc_solution.y)
+ dat.liveMpc.psi = list(self.mpc_solution.psi)
+ dat.liveMpc.curvature = list(self.mpc_solution.curvature)
+ dat.liveMpc.cost = self.mpc_solution.cost
pm.send('liveMpc', dat)
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index d7c7aff3..2f18a753 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -107,7 +107,7 @@ class Planner():
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
- def update(self, sm, CP, VM, PP):
+ def update(self, sm, CP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py
index 238550dc..a0b1dddf 100755
--- a/selfdrive/controls/lib/vehicle_model.py
+++ b/selfdrive/controls/lib/vehicle_model.py
@@ -1,6 +1,6 @@
#!/usr/bin/env python3
"""
-Dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"
+Dynamic bicycle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"
The state is x = [v, r]^T
with v lateral speed [m/s], and r rotational speed [rad/s]
@@ -46,7 +46,7 @@ class VehicleModel:
def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
"""Returns the steady state solution.
- If the speed is too small we can't use the dynamic model (tire slip is undefined),
+ If the speed is too low we can't use the dynamic model (tire slip is undefined),
we then have to use the kinematic model
Args:
@@ -155,8 +155,8 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
A tuple with the 2x2 A matrix, and 2x1 B matrix
Parameters in the vehicle model:
- cF: Tire stiffnes Front [N/rad]
- cR: Tire stiffnes Front [N/rad]
+ cF: Tire stiffness Front [N/rad]
+ cR: Tire stiffness Front [N/rad]
aF: Distance from CG to front wheels [m]
aR: Distance from CG to rear wheels [m]
m: Mass [kg]
@@ -177,7 +177,7 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
def dyn_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
"""Calculate the steady state solution when x_dot = 0,
- Ax + Bu = 0 => x = A^{-1} B u
+ Ax + Bu = 0 => x = -A^{-1} B u
Args:
sa: Steering angle [rad]
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 2d61a1b5..ceed00c7 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -4,7 +4,6 @@ from common.params import Params
from common.realtime import Priority, config_realtime_process
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.longitudinal_planner import Planner
-from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.lateral_planner import LateralPlanner
import cereal.messaging as messaging
@@ -17,32 +16,25 @@ def plannerd_thread(sm=None, pm=None):
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
- PL = Planner(CP)
- PP = LateralPlanner(CP)
-
- VM = VehicleModel(CP)
+ longitudinal_planner = Planner(CP)
+ lateral_planner = LateralPlanner(CP)
if sm is None:
- sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'liveParameters'],
+ sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'],
poll=['radarState', 'modelV2'])
if pm is None:
pm = messaging.PubMaster(['longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc'])
- sm['liveParameters'].valid = True
- sm['liveParameters'].sensorValid = True
- sm['liveParameters'].steerRatio = CP.steerRatio
- sm['liveParameters'].stiffnessFactor = 1.0
-
while True:
sm.update()
if sm.updated['modelV2']:
- PP.update(sm, CP, VM)
- PP.publish(sm, pm)
+ lateral_planner.update(sm, CP)
+ lateral_planner.publish(sm, pm)
if sm.updated['radarState']:
- PL.update(sm, CP, VM, PP)
- PL.publish(sm, pm)
+ longitudinal_planner.update(sm, CP)
+ longitudinal_planner.publish(sm, pm)
def main(sm=None, pm=None):
diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py
index 0bc3efde..fa7c266c 100755
--- a/selfdrive/debug/cpu_usage_stat.py
+++ b/selfdrive/debug/cpu_usage_stat.py
@@ -24,7 +24,7 @@ import argparse
import re
from collections import defaultdict
-import selfdrive.manager as manager
+from selfdrive.manager.process_config import managed_processes
# Do statistics every 5 seconds
PRINT_INTERVAL = 5
@@ -36,7 +36,7 @@ monitored_proc_names = [
# android procs
'SurfaceFlinger', 'sensors.qcom'
-] + manager.car_started_processes + manager.persistent_processes
+] + list(managed_processes.keys())
cpu_time_names = ['user', 'system', 'children_user', 'children_system']
diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py
index 112ca96a..908b129c 100755
--- a/selfdrive/debug/dump.py
+++ b/selfdrive/debug/dump.py
@@ -4,6 +4,8 @@ import sys
import argparse
import json
from hexdump import hexdump
+import codecs
+codecs.register_error("strict", codecs.backslashreplace_errors)
from cereal import log
import cereal.messaging as messaging
diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py
index 7b619805..e194957e 100755
--- a/selfdrive/debug/uiview.py
+++ b/selfdrive/debug/uiview.py
@@ -1,22 +1,27 @@
#!/usr/bin/env python3
import time
import cereal.messaging as messaging
-from selfdrive.manager import start_managed_process, kill_managed_process
+from selfdrive.manager.process_config import managed_processes
-services = ['controlsState', 'deviceState', 'radarState'] # the services needed to be spoofed to start ui offroad
-procs = ['camerad', 'ui', 'modeld', 'calibrationd']
-[start_managed_process(p) for p in procs] # start needed processes
-pm = messaging.PubMaster(services)
-dat_cs, dat_deviceState, dat_radar = [messaging.new_message(s) for s in services]
-dat_cs.controlsState.rearViewCam = False # ui checks for these two messages
-dat_deviceState.deviceState.started = True
+if __name__ == "__main__":
+ services = ['controlsState', 'deviceState', 'radarState'] # the services needed to be spoofed to start ui offroad
+ procs = ['camerad', 'ui', 'modeld', 'calibrationd']
-try:
- while True:
- pm.send('controlsState', dat_cs)
- pm.send('deviceState', dat_deviceState)
- pm.send('radarState', dat_radar)
- time.sleep(1 / 100) # continually send, rate doesn't matter for deviceState
-except KeyboardInterrupt:
- [kill_managed_process(p) for p in procs]
+ for p in procs:
+ managed_processes[p].start()
+
+ pm = messaging.PubMaster(services)
+
+ dat_controlsState, dat_deviceState, dat_radar = [messaging.new_message(s) for s in services]
+ dat_deviceState.deviceState.started = True
+
+ try:
+ while True:
+ pm.send('controlsState', dat_controlsState)
+ pm.send('deviceState', dat_deviceState)
+ pm.send('radarState', dat_radar)
+ time.sleep(1 / 100) # continually send, rate doesn't matter for deviceState
+ except KeyboardInterrupt:
+ for p in procs:
+ managed_processes[p].stop()
diff --git a/selfdrive/hardware/base.h b/selfdrive/hardware/base.h
new file mode 100644
index 00000000..c82610d9
--- /dev/null
+++ b/selfdrive/hardware/base.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include
+#include
+
+
+// no-op base hw class
+class HardwareNone {
+public:
+ static constexpr float MAX_VOLUME = 0;
+ static constexpr float MIN_VOLUME = 0;
+
+ static std::string get_os_version() { return "openpilot for PC"; };
+
+ static void reboot() {};
+ static void poweroff() {};
+ static void set_brightness(int percent) {};
+ static void set_display_power(bool on) {};
+
+ static bool get_ssh_enabled() { return false; };
+ static void set_ssh_enabled(bool enabled) {};
+};
diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py
index f5274deb..14a1a6f8 100644
--- a/selfdrive/hardware/base.py
+++ b/selfdrive/hardware/base.py
@@ -101,3 +101,7 @@ class HardwareBase:
@abstractmethod
def get_thermal_config(self):
pass
+
+ @abstractmethod
+ def set_screen_brightness(self, percentage):
+ pass
diff --git a/selfdrive/hardware/eon/apk.py b/selfdrive/hardware/eon/apk.py
deleted file mode 100644
index 275cdefe..00000000
--- a/selfdrive/hardware/eon/apk.py
+++ /dev/null
@@ -1,105 +0,0 @@
-import os
-import subprocess
-import glob
-import hashlib
-import shutil
-from common.basedir import BASEDIR
-from selfdrive.swaglog import cloudlog
-
-android_packages = ("ai.comma.plus.offroad",)
-
-def get_installed_apks():
- dat = subprocess.check_output(["pm", "list", "packages", "-f"], encoding='utf8').strip().split("\n")
- ret = {}
- for x in dat:
- if x.startswith("package:"):
- v, k = x.split("package:")[1].split("=")
- ret[k] = v
- return ret
-
-def install_apk(path):
- # can only install from world readable path
- install_path = "/sdcard/%s" % os.path.basename(path)
- shutil.copyfile(path, install_path)
-
- ret = subprocess.call(["pm", "install", "-r", install_path])
- os.remove(install_path)
- return ret == 0
-
-def start_offroad():
- set_package_permissions()
- system("am start -n ai.comma.plus.offroad/.MainActivity")
-
-def set_package_permissions():
- try:
- output = subprocess.check_output(['dumpsys', 'package', 'ai.comma.plus.offroad'], encoding="utf-8")
- given_permissions = output.split("runtime permissions")[1]
- except Exception:
- given_permissions = ""
-
- wanted_permissions = ["ACCESS_FINE_LOCATION", "READ_PHONE_STATE", "READ_EXTERNAL_STORAGE"]
- for permission in wanted_permissions:
- if permission not in given_permissions:
- pm_grant("ai.comma.plus.offroad", "android.permission."+permission)
-
- appops_set("ai.comma.plus.offroad", "SU", "allow")
- appops_set("ai.comma.plus.offroad", "WIFI_SCAN", "allow")
-
-def appops_set(package, op, mode):
- system(f"LD_LIBRARY_PATH= appops set {package} {op} {mode}")
-
-def pm_grant(package, permission):
- system(f"pm grant {package} {permission}")
-
-def system(cmd):
- try:
- cloudlog.info("running %s" % cmd)
- subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True)
- except subprocess.CalledProcessError as e:
- cloudlog.event("running failed",
- cmd=e.cmd,
- output=e.output[-1024:],
- returncode=e.returncode)
-
-# *** external functions ***
-
-def update_apks():
- # install apks
- installed = get_installed_apks()
-
- install_apks = glob.glob(os.path.join(BASEDIR, "apk/*.apk"))
- for apk in install_apks:
- app = os.path.basename(apk)[:-4]
- if app not in installed:
- installed[app] = None
-
- cloudlog.info("installed apks %s" % (str(installed), ))
-
- for app in installed.keys():
- apk_path = os.path.join(BASEDIR, "apk/"+app+".apk")
- if not os.path.exists(apk_path):
- continue
-
- h1 = hashlib.sha1(open(apk_path, 'rb').read()).hexdigest()
- h2 = None
- if installed[app] is not None:
- h2 = hashlib.sha1(open(installed[app], 'rb').read()).hexdigest()
- cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2))
-
- if h2 is None or h1 != h2:
- cloudlog.info("installing %s" % app)
-
- success = install_apk(apk_path)
- if not success:
- cloudlog.info("needing to uninstall %s" % app)
- system("pm uninstall %s" % app)
- success = install_apk(apk_path)
-
- assert success
-
-def pm_apply_packages(cmd):
- for p in android_packages:
- system("pm %s %s" % (cmd, p))
-
-if __name__ == "__main__":
- update_apks()
diff --git a/selfdrive/hardware/eon/hardware.h b/selfdrive/hardware/eon/hardware.h
new file mode 100644
index 00000000..bcf99a62
--- /dev/null
+++ b/selfdrive/hardware/eon/hardware.h
@@ -0,0 +1,65 @@
+#pragma once
+
+#include
+#include
+
+#include
+#include
+#include
+
+#include "selfdrive/common/util.h"
+#include "selfdrive/hardware/base.h"
+
+class HardwareEon : public HardwareNone {
+public:
+ static constexpr float MAX_VOLUME = 1.0;
+ static constexpr float MIN_VOLUME = 0.5;
+
+ static std::string get_os_version() {
+ return "NEOS " + util::read_file("/VERSION");
+ };
+
+ static void reboot() { std::system("reboot"); };
+ static void poweroff() { std::system("LD_LIBRARY_PATH= svc power shutdown"); };
+ static void set_brightness(int percent) {
+ std::ofstream brightness_control("/sys/class/leds/lcd-backlight/brightness");
+ if (brightness_control.is_open()) {
+ brightness_control << (int)(percent * (255/100.)) << "\n";
+ brightness_control.close();
+ }
+ };
+ static void set_display_power(bool on) {
+ auto dtoken = android::SurfaceComposerClient::getBuiltInDisplay(android::ISurfaceComposer::eDisplayIdMain);
+ android::SurfaceComposerClient::setDisplayPowerMode(dtoken, on ? HWC_POWER_MODE_NORMAL : HWC_POWER_MODE_OFF);
+ };
+
+ static bool get_ssh_enabled() {
+ return std::system("getprop persist.neos.ssh | grep -qF '1'") == 0;
+ };
+ static void set_ssh_enabled(bool enabled) {
+ std::string cmd = util::string_format("setprop persist.neos.ssh %d", enabled ? 1 : 0);
+ std::system(cmd.c_str());
+ };
+
+ // android only
+ inline static bool launched_activity = false;
+ static void check_activity() {
+ int ret = std::system("dumpsys SurfaceFlinger --list | grep -Fq 'com.android.settings'");
+ launched_activity = ret == 0;
+ }
+ static void launch_activity(std::string activity, std::string opts = "") {
+ if (!launched_activity) {
+ std::string cmd = "am start -n " + activity + " " + opts +
+ " --ez extra_prefs_show_button_bar true \
+ --es extra_prefs_set_next_text ''";
+ std::system(cmd.c_str());
+ }
+ launched_activity = true;
+ }
+ static void launch_wifi() {
+ launch_activity("com.android.settings/.wifi.WifiPickerActivity", "-a android.net.wifi.PICK_WIFI_NETWORK");
+ }
+ static void launch_tethering() {
+ launch_activity("com.android.settings/.TetherSettings");
+ }
+};
diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py
index 4666c2a6..2ef2a8f4 100644
--- a/selfdrive/hardware/eon/hardware.py
+++ b/selfdrive/hardware/eon/hardware.py
@@ -348,3 +348,7 @@ class Android(HardwareBase):
def get_thermal_config(self):
return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), bat=(29, 1000), ambient=(25, 1))
+
+ def set_screen_brightness(self, percentage):
+ with open("/sys/class/leds/lcd-backlight/brightness", "w") as f:
+ f.write(str(int(percentage * 2.55)))
diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h
new file mode 100644
index 00000000..0e0980f1
--- /dev/null
+++ b/selfdrive/hardware/hw.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "selfdrive/hardware/base.h"
+
+#ifdef QCOM
+#include "selfdrive/hardware/eon/hardware.h"
+#define Hardware HardwareEon
+#elif QCOM2
+#include "selfdrive/hardware/tici/hardware.h"
+#define Hardware HardwareTici
+#else
+#define Hardware HardwareNone
+#endif
diff --git a/selfdrive/hardware/pc/hardware.py b/selfdrive/hardware/pc/hardware.py
index 1744b695..7ee40bd6 100644
--- a/selfdrive/hardware/pc/hardware.py
+++ b/selfdrive/hardware/pc/hardware.py
@@ -76,3 +76,6 @@ class Pc(HardwareBase):
def get_thermal_config(self):
return ThermalConfig(cpu=((None,), 1), gpu=((None,), 1), mem=(None, 1), bat=(None, 1), ambient=(None, 1))
+
+ def set_screen_brightness(self, percentage):
+ pass
diff --git a/selfdrive/hardware/tici/agnos.json b/selfdrive/hardware/tici/agnos.json
index 467bad92..1fa725b9 100644
--- a/selfdrive/hardware/tici/agnos.json
+++ b/selfdrive/hardware/tici/agnos.json
@@ -1,17 +1,17 @@
[
{
"name": "system",
- "url": "https://commadist.azureedge.net/agnosupdate-staging/system-09c324e1fc98905efffc356592235a5ea2955922b04d16f4406d0a96122b69b2.img.xz",
- "hash": "8f7ed837a155abbdb3bc9490b65c8a3c67553a0aa32f429aa57d54134c1fe886",
- "hash_raw": "09c324e1fc98905efffc356592235a5ea2955922b04d16f4406d0a96122b69b2",
+ "url": "https://commadist.azureedge.net/agnosupdate-staging/system-990a4362f863f860d70440016005b434bcd17130547deafed96c720d0fc8495e.img.xz",
+ "hash": "f30370eda7253029ec1e4322fbb060ab69f541e4d5176c41541df608183cee4b",
+ "hash_raw": "990a4362f863f860d70440016005b434bcd17130547deafed96c720d0fc8495e",
"size": 10737418240,
"sparse": true
},
{
"name": "boot",
- "url": "https://commadist.azureedge.net/agnosupdate-staging/boot-d292aacf2f760e85c60c1a25adcecae9df2c80ee3f3240ea8592bbac369db038.img.xz",
- "hash": "d292aacf2f760e85c60c1a25adcecae9df2c80ee3f3240ea8592bbac369db038",
- "hash_raw": "d292aacf2f760e85c60c1a25adcecae9df2c80ee3f3240ea8592bbac369db038",
+ "url": "https://commadist.azureedge.net/agnosupdate-staging/boot-b8aebb538f030bbd8e69c227bee33f78ac3be0affe55dc76ab7e87e3f030eceb.img.xz",
+ "hash": "b8aebb538f030bbd8e69c227bee33f78ac3be0affe55dc76ab7e87e3f030eceb",
+ "hash_raw": "b8aebb538f030bbd8e69c227bee33f78ac3be0affe55dc76ab7e87e3f030eceb",
"size": 14768128,
"sparse": false
}
diff --git a/selfdrive/hardware/tici/hardware.h b/selfdrive/hardware/tici/hardware.h
new file mode 100644
index 00000000..034e01a8
--- /dev/null
+++ b/selfdrive/hardware/tici/hardware.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include
+#include
+
+#include "selfdrive/common/util.h"
+#include "selfdrive/common/params.h"
+#include "selfdrive/hardware/base.h"
+
+class HardwareTici : public HardwareNone {
+public:
+ static constexpr float MAX_VOLUME = 0.5;
+ static constexpr float MIN_VOLUME = 0.4;
+
+ static std::string get_os_version() {
+ return "AGNOS " + util::read_file("/VERSION");
+ };
+
+ static void reboot() { std::system("sudo reboot"); };
+ static void poweroff() { std::system("sudo poweroff"); };
+ static void set_brightness(int percent) {
+ std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness");
+ if (brightness_control.is_open()) {
+ brightness_control << (percent * (int)(1023/100.)) << "\n";
+ brightness_control.close();
+ }
+ };
+ static void set_display_power(bool on) {};
+
+ static bool get_ssh_enabled() { return Params().read_db_bool("SshEnabled"); };
+ static void set_ssh_enabled(bool enabled) { Params().write_db_value("SshEnabled", (enabled ? "1" : "0")); };
+};
diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py
index 7fca07a8..bdf2170c 100644
--- a/selfdrive/hardware/tici/hardware.py
+++ b/selfdrive/hardware/tici/hardware.py
@@ -44,7 +44,8 @@ class Tici(HardwareBase):
return "tici"
def get_sound_card_online(self):
- return os.system("pulseaudio --check") == 0
+ return (os.path.isfile('/proc/asound/card0/state') and
+ open('/proc/asound/card0/state').read().strip() == 'ONLINE')
def reboot(self, reason=None):
subprocess.check_output(["sudo", "reboot"])
@@ -182,4 +183,11 @@ class Tici(HardwareBase):
os.system("sudo poweroff")
def get_thermal_config(self):
- return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(70, 1000))
+ return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(65, 1000))
+
+ def set_screen_brightness(self, percentage):
+ try:
+ with open("/sys/class/backlight/panel0-backlight/brightness", "w") as f:
+ f.write(str(int(percentage * 10.23)))
+ except Exception:
+ pass
diff --git a/selfdrive/launcher.py b/selfdrive/launcher.py
deleted file mode 100644
index dcedf071..00000000
--- a/selfdrive/launcher.py
+++ /dev/null
@@ -1,27 +0,0 @@
-import importlib
-from setproctitle import setproctitle # pylint: disable=no-name-in-module
-
-import cereal.messaging as messaging
-import selfdrive.crash as crash
-from selfdrive.swaglog import cloudlog
-
-def launcher(proc):
- try:
- # import the process
- mod = importlib.import_module(proc)
-
- # rename the process
- setproctitle(proc)
-
- # create new context since we forked
- messaging.context = messaging.Context()
-
- # exec the process
- mod.main()
- except KeyboardInterrupt:
- cloudlog.warning("child %s got SIGINT" % proc)
- except Exception:
- # can't install the crash handler becuase sys.excepthook doesn't play nice
- # with threads, so catch it here.
- crash.capture_exception()
- raise
diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py
index d061452f..f7267f0c 100755
--- a/selfdrive/locationd/locationd.py
+++ b/selfdrive/locationd/locationd.py
@@ -62,7 +62,7 @@ class Localizer():
self.calib = np.zeros(3)
self.device_from_calib = np.eye(3)
self.calib_from_device = np.eye(3)
- self.calibrated = 0
+ self.calibrated = False
self.H = get_H()
self.posenet_invalid_count = 0
@@ -77,7 +77,7 @@ class Localizer():
self.device_fell = False
@staticmethod
- def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov):
+ def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov, calibrated):
predicted_std = np.sqrt(np.diagonal(predicted_cov))
fix_ecef = predicted_state[States.ECEF_POS]
@@ -130,12 +130,12 @@ class Localizer():
(fix.velocityDevice, vel_device, vel_device_std, True),
(fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True),
(fix.orientationECEF, orientation_ecef, orientation_ecef_std, True),
- (fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), True),
+ (fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), calibrated),
(fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True),
(fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True),
- (fix.velocityCalibrated, vel_calib, vel_calib_std, True),
- (fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, True),
- (fix.accelerationCalibrated, acc_calib, acc_calib_std, True),
+ (fix.velocityCalibrated, vel_calib, vel_calib_std, calibrated),
+ (fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, calibrated),
+ (fix.accelerationCalibrated, acc_calib, acc_calib_std, calibrated),
]
for field, value, std, valid in measurements:
@@ -147,7 +147,7 @@ class Localizer():
return fix
def liveLocationMsg(self):
- fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
+ fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P, self.calibrated)
# experimentally found these values, no false positives in 20k minutes of driving
old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:])
std_spike = new_mean/old_mean > 4 and new_mean > 7
diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py
index ee409ece..6c5357f2 100755
--- a/selfdrive/locationd/models/car_kf.py
+++ b/selfdrive/locationd/models/car_kf.py
@@ -9,6 +9,7 @@ import sympy as sp
from rednose import KalmanFilter
from rednose.helpers.ekf_sym import EKF_sym, gen_code
from selfdrive.locationd.models.constants import ObservationKind
+from selfdrive.swaglog import cloudlog
i = 0
@@ -148,7 +149,7 @@ class CarKalman(KalmanFilter):
x_init[States.ANGLE_OFFSET] = angle_offset
# init filter
- self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars)
+ self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
if __name__ == "__main__":
diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py
index 8c9cb7a3..3d8a1f65 100755
--- a/selfdrive/locationd/models/live_kf.py
+++ b/selfdrive/locationd/models/live_kf.py
@@ -5,6 +5,7 @@ import sys
import numpy as np
import sympy as sp
+from selfdrive.swaglog import cloudlog
from selfdrive.locationd.models.constants import ObservationKind
from rednose.helpers.ekf_sym import EKF_sym, gen_code
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
@@ -200,13 +201,13 @@ class LiveKalman():
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
- ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
+ ObservationKind.NO_ROT: np.diag([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.diag([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])}
# init filter
- self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2)
+ self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2, logger=cloudlog)
@property
def x(self):
diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index 263f5cc1..96b3c26d 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -5,14 +5,12 @@ import json
import numpy as np
import cereal.messaging as messaging
-from cereal import car, log
+from cereal import car
from common.params import Params, put_nonblocking
from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
from selfdrive.locationd.models.constants import GENERATED_DIR
from selfdrive.swaglog import cloudlog
-KalmanStatus = log.LiveLocationKalman.Status
-
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
@@ -38,9 +36,10 @@ class ParamsLearner:
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
+ yaw_rate_valid = msg.angularVelocityCalibrated.valid
if self.active:
- if msg.inputsOK and msg.posenetOK and msg.status == KalmanStatus.valid:
+ if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[[-yaw_rate]]]),
@@ -89,9 +88,10 @@ def main(sm=None, pm=None):
params = None
try:
- if params is not None and not all((
- abs(params.get('angleOffsetAverageDeg')) < 10.0,
- min_sr <= params['steerRatio'] <= max_sr)):
+ angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
+ steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
+ params_sane = angle_offset_sane and steer_ratio_sane
+ if params is not None and not params_sane:
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e:
diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc
index 51924f64..7fd39543 100644
--- a/selfdrive/locationd/ubloxd_main.cc
+++ b/selfdrive/locationd/ubloxd_main.cc
@@ -23,7 +23,7 @@ ExitHandler do_exit;
using namespace ublox;
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) {
LOGW("starting ubloxd");
-
+ AlignedBuffer aligned_buf;
UbloxMsgParser parser;
Context * context = Context::create();
@@ -42,10 +42,7 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
continue;
}
- auto amsg = kj::heapArray((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();
auto ubloxRaw = event.getUbloxRaw();
diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc
index e369a8ed..ca621810 100644
--- a/selfdrive/loggerd/logger.cc
+++ b/selfdrive/loggerd/logger.cc
@@ -111,7 +111,7 @@ kj::Array logger_build_init_data() {
for (auto& kv : params_map) {
auto lentry = lparams[i];
lentry.setKey(kv.first);
- lentry.setValue(kv.second);
+ lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size()));
i++;
}
}
diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc
index cad256e9..c28657c6 100644
--- a/selfdrive/loggerd/loggerd.cc
+++ b/selfdrive/loggerd/loggerd.cc
@@ -383,7 +383,7 @@ int main(int argc, char** argv) {
uint64_t msg_count = 0;
uint64_t bytes_count = 0;
- kj::Array buf = kj::heapArray(1024);
+ AlignedBuffer aligned_buf;
double start_ts = seconds_since_boot();
double last_rotate_tms = millis_since_boot();
@@ -427,15 +427,7 @@ int main(int argc, char** argv) {
if (fpkt_id >= 0) {
// track camera frames to sync to encoder
// only process last frame
- const uint8_t* data = (uint8_t*)last_msg->getData();
- const size_t len = last_msg->getSize();
- const size_t size = len / sizeof(capnp::word) + 1;
- if (buf.size() < size) {
- buf = kj::heapArray(size);
- }
- memcpy(buf.begin(), data, len);
-
- capnp::FlatArrayMessageReader cmsg(buf);
+ capnp::FlatArrayMessageReader cmsg(aligned_buf.align(last_msg));
cereal::Event::Reader event = cmsg.getRoot();
if (fpkt_id == LOG_CAMERA_ID_FCAMERA) {
diff --git a/selfdrive/loggerd/omx_encoder.cc b/selfdrive/loggerd/omx_encoder.cc
index 1496e82d..b629490f 100644
--- a/selfdrive/loggerd/omx_encoder.cc
+++ b/selfdrive/loggerd/omx_encoder.cc
@@ -349,12 +349,10 @@ void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) {
if (e->remuxing) {
if (!e->wrote_codec_config && e->codec_config_len > 0) {
- if (e->codec_ctx->extradata_size < e->codec_config_len) {
- e->codec_ctx->extradata = (uint8_t *)realloc(e->codec_ctx->extradata, e->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE);
- }
+ // extradata will be freed by av_free() in avcodec_free_context()
+ e->codec_ctx->extradata = (uint8_t*)av_mallocz(e->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE);
e->codec_ctx->extradata_size = e->codec_config_len;
memcpy(e->codec_ctx->extradata, e->codec_config, e->codec_config_len);
- memset(e->codec_ctx->extradata + e->codec_ctx->extradata_size, 0, AV_INPUT_BUFFER_PADDING_SIZE);
err = avcodec_parameters_from_context(e->out_stream->codecpar, e->codec_ctx);
assert(err >= 0);
diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc
index 71703c1f..2e8fd20a 100644
--- a/selfdrive/loggerd/raw_logger.cc
+++ b/selfdrive/loggerd/raw_logger.cc
@@ -25,8 +25,6 @@ RawLogger::RawLogger(const char* filename, int width, int height, int fps,
: filename(filename),
fps(fps) {
- int err = 0;
-
av_register_all();
codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
// codec = avcodec_find_encoder(AV_CODEC_ID_FFV1);
@@ -45,7 +43,7 @@ RawLogger::RawLogger(const char* filename, int width, int height, int fps,
codec_ctx->time_base = (AVRational){ 1, fps };
- err = avcodec_open2(codec_ctx, codec, NULL);
+ int err = avcodec_open2(codec_ctx, codec, NULL);
assert(err >= 0);
frame = av_frame_alloc();
@@ -65,10 +63,6 @@ RawLogger::~RawLogger() {
}
void RawLogger::encoder_open(const char* path) {
- int err = 0;
-
- std::lock_guard guard(lock);
-
vid_path = util::string_format("%s/%s.mkv", path, filename);
// create camera lock file
@@ -91,7 +85,7 @@ void RawLogger::encoder_open(const char* path) {
stream->time_base = (AVRational){ 1, fps };
// codec_ctx->time_base = stream->time_base;
- err = avcodec_parameters_from_context(stream->codecpar, codec_ctx);
+ int err = avcodec_parameters_from_context(stream->codecpar, codec_ctx);
assert(err >= 0);
err = avio_open(&format_ctx->pb, vid_path.c_str(), AVIO_FLAG_WRITE);
@@ -105,13 +99,9 @@ void RawLogger::encoder_open(const char* path) {
}
void RawLogger::encoder_close() {
- int err = 0;
-
- std::lock_guard guard(lock);
-
if (!is_open) return;
- err = av_write_trailer(format_ctx);
+ int err = av_write_trailer(format_ctx);
assert(err == 0);
avcodec_close(stream->codec);
@@ -128,8 +118,6 @@ void RawLogger::encoder_close() {
int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, uint64_t ts) {
- int err = 0;
-
AVPacket pkt;
av_init_packet(&pkt);
pkt.data = NULL;
@@ -143,7 +131,7 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui
int ret = counter;
int got_output = 0;
- err = avcodec_encode_video2(codec_ctx, &pkt, frame, &got_output);
+ int err = avcodec_encode_video2(codec_ctx, &pkt, frame, &got_output);
if (err) {
LOGE("encoding error\n");
ret = -1;
diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h
index 28bfdf90..5df6eabe 100644
--- a/selfdrive/loggerd/raw_logger.h
+++ b/selfdrive/loggerd/raw_logger.h
@@ -5,8 +5,6 @@
#include
#include
-#include
-#include
extern "C" {
#include
@@ -34,8 +32,6 @@ private:
std::string vid_path, lock_path;
- std::recursive_mutex lock;
-
AVCodec *codec = NULL;
AVCodecContext *codec_ctx = NULL;
diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py
index 8fdb2a77..4a8539dd 100644
--- a/selfdrive/loggerd/uploader.py
+++ b/selfdrive/loggerd/uploader.py
@@ -6,11 +6,13 @@ import requests
import threading
import time
import traceback
+from pathlib import Path
from cereal import log
import cereal.messaging as messaging
from common.api import Api
from common.params import Params
+from selfdrive.hardware import TICI
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.loggerd.config import ROOT
from selfdrive.swaglog import cloudlog
@@ -125,10 +127,10 @@ class Uploader():
url_resp_json = json.loads(url_resp.text)
url = url_resp_json['url']
headers = url_resp_json['headers']
- cloudlog.info("upload_url v1.3 %s %s", url, str(headers))
+ cloudlog.debug("upload_url v1.3 %s %s", url, str(headers))
if fake_upload:
- cloudlog.info("*** WARNING, THIS IS A FAKE UPLOAD TO %s ***" % url)
+ cloudlog.debug("*** WARNING, THIS IS A FAKE UPLOAD TO %s ***" % url)
class FakeResponse():
def __init__(self):
@@ -162,7 +164,7 @@ class Uploader():
cloudlog.event("upload", key=key, fn=fn, sz=sz)
- cloudlog.info("checking %r with size %r", key, sz)
+ cloudlog.debug("checking %r with size %r", key, sz)
if sz == 0:
try:
@@ -172,10 +174,10 @@ class Uploader():
cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz)
success = True
else:
- cloudlog.info("uploading %r", fn)
+ cloudlog.debug("uploading %r", fn)
stat = self.normal_upload(key, fn)
if stat is not None and stat.status_code in (200, 201, 412):
- cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz)
+ cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, debug=True)
try:
# tag file as uploaded
setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE)
@@ -183,14 +185,12 @@ class Uploader():
cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz)
success = True
else:
- cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
+ cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, debug=True)
success = False
return success
def uploader_fn(exit_event):
- cloudlog.info("uploader_fn")
-
params = Params()
dongle_id = params.get("DongleId").decode('utf8')
@@ -198,14 +198,23 @@ def uploader_fn(exit_event):
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
+ if TICI and not Path("/data/media").is_mount():
+ cloudlog.debug("NVME not mounted")
+
sm = messaging.SubMaster(['deviceState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
- on_wifi = force_wifi or sm['deviceState'].networkType == NetworkType.wifi
offroad = params.get("IsOffroad") == b'1'
+ network_type = sm['deviceState'].networkType if not force_wifi else NetworkType.wifi
+ if network_type == NetworkType.none:
+ if allow_sleep:
+ time.sleep(60 if offroad else 5)
+ continue
+
+ on_wifi = network_type == NetworkType.wifi
allow_raw_upload = params.get("IsUploadRawEnabled") != b"0"
d = uploader.next_file_to_upload(with_raw=allow_raw_upload and on_wifi and offroad)
@@ -216,13 +225,12 @@ def uploader_fn(exit_event):
key, fn = d
- cloudlog.event("uploader_netcheck", is_on_wifi=on_wifi)
- cloudlog.info("to upload %r", d)
+ cloudlog.debug("upload %r over %s", d, network_type)
success = uploader.upload(key, fn)
if success:
backoff = 0.1
elif allow_sleep:
- cloudlog.info("backoff %r", backoff)
+ cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff*2, 120)
cloudlog.info("upload done, success=%r", success)
diff --git a/selfdrive/logmessaged.py b/selfdrive/logmessaged.py
index 953f3d47..17ef42cd 100755
--- a/selfdrive/logmessaged.py
+++ b/selfdrive/logmessaged.py
@@ -1,12 +1,14 @@
#!/usr/bin/env python3
import zmq
import cereal.messaging as messaging
-from selfdrive.swaglog import get_le_handler
+from common.logging_extra import SwagLogFileFormatter
+from selfdrive.swaglog import get_file_handler
def main():
- le_handler = get_le_handler()
- le_level = 20 # logging.INFO
+ log_handler = get_file_handler()
+ log_handler.setFormatter(SwagLogFileFormatter(None))
+ log_level = 20 # logging.INFO
ctx = zmq.Context().instance()
sock = ctx.socket(zmq.PULL)
@@ -17,19 +19,14 @@ def main():
while True:
dat = b''.join(sock.recv_multipart())
- dat = dat.decode('utf8')
-
- levelnum = ord(dat[0])
- dat = dat[1:]
-
- if levelnum >= le_level:
- # push to logentries
- # TODO: push to athena instead
- le_handler.emit_raw(dat)
+ level = dat[0]
+ record = dat[1:].decode("utf-8")
+ if level >= log_level:
+ log_handler.emit(record)
# then we publish them
msg = messaging.new_message()
- msg.logMessage = dat
+ msg.logMessage = record
pub_sock.send(msg.to_bytes())
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
deleted file mode 100755
index 1e3d9d68..00000000
--- a/selfdrive/manager.py
+++ /dev/null
@@ -1,607 +0,0 @@
-#!/usr/bin/env python3
-import datetime
-import importlib
-import os
-import sys
-import fcntl
-import errno
-import signal
-import shutil
-import subprocess
-import textwrap
-import time
-import traceback
-
-from multiprocessing import Process
-from typing import Dict
-
-from common.basedir import BASEDIR
-from common.spinner import Spinner
-from common.text_window import TextWindow
-import selfdrive.crash as crash
-from selfdrive.hardware import HARDWARE, EON, PC, TICI
-from selfdrive.hardware.eon.apk import update_apks, pm_apply_packages, start_offroad
-from selfdrive.swaglog import cloudlog, add_logentries_handler
-from selfdrive.version import version, dirty
-
-os.environ['BASEDIR'] = BASEDIR
-sys.path.append(os.path.join(BASEDIR, "pyextra"))
-
-TOTAL_SCONS_NODES = 1225
-MAX_BUILD_PROGRESS = 70
-WEBCAM = os.getenv("WEBCAM") is not None
-PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt'))
-
-
-def unblock_stdout():
- # get a non-blocking stdout
- child_pid, child_pty = os.forkpty()
- if child_pid != 0: # parent
-
- # child is in its own process group, manually pass kill signals
- signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT))
- signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM))
-
- fcntl.fcntl(sys.stdout, fcntl.F_SETFL, fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK)
-
- while True:
- try:
- dat = os.read(child_pty, 4096)
- except OSError as e:
- if e.errno == errno.EIO:
- break
- continue
-
- if not dat:
- break
-
- try:
- sys.stdout.write(dat.decode('utf8'))
- except (OSError, IOError, UnicodeDecodeError):
- pass
-
- # os.wait() returns a tuple with the pid and a 16 bit value
- # whose low byte is the signal number and whose high byte is the exit satus
- exit_status = os.wait()[1] >> 8
- os._exit(exit_status)
-
-if __name__ == "__main__":
- unblock_stdout()
-
-
-# Start spinner
-spinner = Spinner()
-spinner.update_progress(0, 100)
-if __name__ != "__main__":
- spinner.close()
-
-def build():
- env = os.environ.copy()
- env['SCONS_PROGRESS'] = "1"
- env['SCONS_CACHE'] = "1"
- nproc = os.cpu_count()
- j_flag = "" if nproc is None else f"-j{nproc - 1}"
-
- for retry in [True, False]:
- scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
-
- compile_output = []
-
- # Read progress from stderr and update spinner
- while scons.poll() is None:
- try:
- line = scons.stderr.readline()
- if line is None:
- continue
- line = line.rstrip()
-
- prefix = b'progress: '
- if line.startswith(prefix):
- i = int(line[len(prefix):])
- spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.)
- elif len(line):
- compile_output.append(line)
- print(line.decode('utf8', 'replace'))
- except Exception:
- pass
-
- if scons.returncode != 0:
- # Read remaining output
- r = scons.stderr.read().split(b'\n')
- compile_output += r
-
- if retry and (not dirty):
- if not os.getenv("CI"):
- print("scons build failed, cleaning in")
- for i in range(3, -1, -1):
- print("....%d" % i)
- time.sleep(1)
- subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
- shutil.rmtree("/tmp/scons_cache", ignore_errors=True)
- shutil.rmtree("/data/scons_cache", ignore_errors=True)
- else:
- print("scons build failed after retry")
- sys.exit(1)
- else:
- # Build failed log errors
- errors = [line.decode('utf8', 'replace') for line in compile_output
- if any([err in line for err in [b'error: ', b'not found, needed by target']])]
- error_s = "\n".join(errors)
- add_logentries_handler(cloudlog)
- cloudlog.error("scons build failed\n" + error_s)
-
- # Show TextWindow
- spinner.close()
- error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors])
- with TextWindow("openpilot failed to build\n \n" + error_s) as t:
- t.wait_for_exit()
- exit(1)
- else:
- break
-
-if __name__ == "__main__" and not PREBUILT:
- build()
-
-import cereal.messaging as messaging
-from cereal import log
-
-from common.params import Params
-from selfdrive.registration import register
-from selfdrive.launcher import launcher
-
-
-# comment out anything you don't want to run
-managed_processes = {
- "thermald": "selfdrive.thermald.thermald",
- "uploader": "selfdrive.loggerd.uploader",
- "deleter": "selfdrive.loggerd.deleter",
- "controlsd": "selfdrive.controls.controlsd",
- "plannerd": "selfdrive.controls.plannerd",
- "radard": "selfdrive.controls.radard",
- "dmonitoringd": "selfdrive.monitoring.dmonitoringd",
- "ubloxd": ("selfdrive/locationd", ["./ubloxd"]),
- "loggerd": ("selfdrive/loggerd", ["./loggerd"]),
- "logmessaged": "selfdrive.logmessaged",
- "locationd": "selfdrive.locationd.locationd",
- "tombstoned": "selfdrive.tombstoned",
- "logcatd": ("selfdrive/logcatd", ["./logcatd"]),
- "proclogd": ("selfdrive/proclogd", ["./proclogd"]),
- "pandad": "selfdrive.pandad",
- "ui": ("selfdrive/ui", ["./ui"]),
- "calibrationd": "selfdrive.locationd.calibrationd",
- "paramsd": "selfdrive.locationd.paramsd",
- "camerad": ("selfdrive/camerad", ["./camerad"]),
- "sensord": ("selfdrive/sensord", ["./sensord"]),
- "clocksd": ("selfdrive/clocksd", ["./clocksd"]),
- "updated": "selfdrive.updated",
- "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]),
- "modeld": ("selfdrive/modeld", ["./modeld"]),
- "rtshield": "selfdrive.rtshield",
-}
-
-daemon_processes = {
- "manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"),
-}
-
-running: Dict[str, Process] = {}
-def get_running():
- return running
-
-# due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption
-unkillable_processes = ['camerad']
-
-# processes to end with SIGKILL instead of SIGTERM
-kill_processes = []
-if EON:
- kill_processes += [
- 'sensord',
- ]
-
-persistent_processes = [
- 'pandad',
- 'thermald',
- 'logmessaged',
- 'ui',
- 'uploader',
- 'deleter',
-]
-
-if not PC:
- persistent_processes += [
- 'updated',
- 'tombstoned',
- ]
-
-if EON:
- persistent_processes += [
- 'sensord',
- ]
-
-if TICI:
- managed_processes["timezoned"] = "selfdrive.timezoned"
- persistent_processes += ['timezoned']
-
-car_started_processes = [
- 'controlsd',
- 'plannerd',
- 'loggerd',
- 'radard',
- 'calibrationd',
- 'paramsd',
- 'camerad',
- 'modeld',
- 'proclogd',
- 'locationd',
- 'clocksd',
- 'logcatd',
-]
-
-driver_view_processes = [
- 'camerad',
- 'dmonitoringd',
- 'dmonitoringmodeld'
-]
-
-if not PC or WEBCAM:
- car_started_processes += [
- 'ubloxd',
- 'dmonitoringd',
- 'dmonitoringmodeld',
- ]
-
-if EON:
- car_started_processes += [
- 'rtshield',
- ]
-else:
- car_started_processes += [
- 'sensord',
- ]
-
-def register_managed_process(name, desc, car_started=False):
- global managed_processes, car_started_processes, persistent_processes
- managed_processes[name] = desc
- if car_started:
- car_started_processes.append(name)
- else:
- persistent_processes.append(name)
-
-# ****************** process management functions ******************
-def nativelauncher(pargs, cwd):
- # exec the process
- os.chdir(cwd)
- os.execvp(pargs[0], pargs)
-
-def start_managed_process(name):
- if name in running or name not in managed_processes:
- return
- proc = managed_processes[name]
- if isinstance(proc, str):
- cloudlog.info("starting python %s" % proc)
- running[name] = Process(name=name, target=launcher, args=(proc,))
- else:
- pdir, pargs = proc
- cwd = os.path.join(BASEDIR, pdir)
- cloudlog.info("starting process %s" % name)
- running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd))
- running[name].start()
-
-def start_daemon_process(name):
- params = Params()
- proc, pid_param = daemon_processes[name]
- pid = params.get(pid_param, encoding='utf-8')
-
- if pid is not None:
- try:
- os.kill(int(pid), 0)
- with open(f'/proc/{pid}/cmdline') as f:
- if proc in f.read():
- # daemon is running
- return
- except (OSError, FileNotFoundError):
- # process is dead
- pass
-
- cloudlog.info("starting daemon %s" % name)
- proc = subprocess.Popen(['python', '-m', proc], # pylint: disable=subprocess-popen-preexec-fn
- stdin=open('/dev/null', 'r'),
- stdout=open('/dev/null', 'w'),
- stderr=open('/dev/null', 'w'),
- preexec_fn=os.setpgrp)
-
- params.put(pid_param, str(proc.pid))
-
-def prepare_managed_process(p, build=False):
- proc = managed_processes[p]
- if isinstance(proc, str):
- # import this python
- cloudlog.info("preimporting %s" % proc)
- importlib.import_module(proc)
- elif os.path.isfile(os.path.join(BASEDIR, proc[0], "SConscript")) and build:
- # build this process
- cloudlog.info("building %s" % (proc,))
- try:
- subprocess.check_call(["scons", "u", "-j4", "."], cwd=os.path.join(BASEDIR, proc[0]))
- except subprocess.CalledProcessError:
- # clean and retry if the build failed
- cloudlog.warning("building %s failed, cleaning and retrying" % (proc, ))
- subprocess.check_call(["scons", "-u", "-c", "."], cwd=os.path.join(BASEDIR, proc[0]))
- subprocess.check_call(["scons", "-u", "-j4", "."], cwd=os.path.join(BASEDIR, proc[0]))
-
-
-def join_process(process, timeout):
- # Process().join(timeout) will hang due to a python 3 bug: https://bugs.python.org/issue28382
- # We have to poll the exitcode instead
- t = time.time()
- while time.time() - t < timeout and process.exitcode is None:
- time.sleep(0.001)
-
-
-def kill_managed_process(name, retry=True):
- if name not in running or name not in managed_processes:
- return
- cloudlog.info(f"killing {name}")
-
- if running[name].exitcode is None:
- sig = signal.SIGKILL if name in kill_processes else signal.SIGINT
- os.kill(running[name].pid, sig)
-
- join_process(running[name], 5)
-
- if running[name].exitcode is None:
- if not retry:
- raise Exception(f"{name} failed to die")
-
- if name in unkillable_processes:
- cloudlog.critical("unkillable process %s failed to exit! rebooting in 15 if it doesn't die" % name)
- join_process(running[name], 15)
- if running[name].exitcode is None:
- cloudlog.critical("unkillable process %s failed to die!" % name)
- os.system("date >> /data/unkillable_reboot")
- os.sync()
- HARDWARE.reboot()
- raise RuntimeError
- else:
- cloudlog.info("killing %s with SIGKILL" % name)
- os.kill(running[name].pid, signal.SIGKILL)
- running[name].join()
-
- ret = running[name].exitcode
- cloudlog.info(f"{name} is dead with {ret}")
- del running[name]
- return ret
-
-
-def cleanup_all_processes(signal, frame):
- cloudlog.info("caught ctrl-c %s %s" % (signal, frame))
-
- if EON:
- pm_apply_packages('disable')
-
- for name in list(running.keys()):
- kill_managed_process(name)
- cloudlog.info("everything is dead")
-
-
-def send_managed_process_signal(name, sig):
- if name not in running or name not in managed_processes or \
- running[name].exitcode is not None:
- return
-
- cloudlog.info(f"sending signal {sig} to {name}")
- os.kill(running[name].pid, sig)
-
-
-# ****************** run loop ******************
-
-def manager_init():
- os.umask(0) # Make sure we can create files with 777 permissions
-
- # Create folders needed for msgq
- try:
- os.mkdir("/dev/shm")
- except FileExistsError:
- pass
- except PermissionError:
- print("WARNING: failed to make /dev/shm")
-
- # set dongle id
- reg_res = register(spinner)
- if reg_res:
- dongle_id = reg_res
- else:
- raise Exception("server registration failed")
- os.environ['DONGLE_ID'] = dongle_id
-
- if not dirty:
- os.environ['CLEAN'] = '1'
-
- cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty,
- device=HARDWARE.get_device_type())
- crash.bind_user(id=dongle_id)
- crash.bind_extra(version=version, dirty=dirty, device=HARDWARE.get_device_type())
-
- # ensure shared libraries are readable by apks
- if EON:
- os.chmod(BASEDIR, 0o755)
- os.chmod("/dev/shm", 0o777)
- os.chmod(os.path.join(BASEDIR, "cereal"), 0o755)
- os.chmod(os.path.join(BASEDIR, "cereal", "libmessaging_shared.so"), 0o755)
-
-def manager_thread():
-
- cloudlog.info("manager start")
- cloudlog.info({"environ": os.environ})
-
- # save boot log
- subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
-
- # start daemon processes
- for p in daemon_processes:
- start_daemon_process(p)
-
- # start persistent processes
- for p in persistent_processes:
- start_managed_process(p)
-
- # start offroad
- if EON:
- pm_apply_packages('enable')
- start_offroad()
-
- if os.getenv("NOBOARD") is not None:
- del managed_processes["pandad"]
-
- if os.getenv("BLOCK") is not None:
- for k in os.getenv("BLOCK").split(","):
- del managed_processes[k]
-
- started_prev = False
- logger_dead = False
- params = Params()
- device_state_sock = messaging.sub_sock('deviceState')
- pm = messaging.PubMaster(['managerState'])
-
- while 1:
- msg = messaging.recv_sock(device_state_sock, wait=True)
-
- if msg.deviceState.freeSpacePercent < 5:
- logger_dead = True
-
- if msg.deviceState.started:
- for p in car_started_processes:
- if p == "loggerd" and logger_dead:
- kill_managed_process(p)
- else:
- start_managed_process(p)
- else:
- logger_dead = False
- driver_view = params.get("IsDriverViewEnabled") == b"1"
-
- # TODO: refactor how manager manages processes
- for p in reversed(car_started_processes):
- if p not in driver_view_processes or not driver_view:
- kill_managed_process(p)
-
- for p in driver_view_processes:
- if driver_view:
- start_managed_process(p)
- else:
- kill_managed_process(p)
-
- # trigger an update after going offroad
- if started_prev:
- os.sync()
- send_managed_process_signal("updated", signal.SIGHUP)
-
- started_prev = msg.deviceState.started
-
- # check the status of all processes, did any of them die?
- running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running]
- cloudlog.debug(' '.join(running_list))
-
- # send managerState
- states = []
- for p in managed_processes:
- state = log.ManagerState.ProcessState.new_message()
- state.name = p
- if p in running:
- state.running = running[p].is_alive()
- state.pid = running[p].pid
- state.exitCode = running[p].exitcode or 0
- states.append(state)
- msg = messaging.new_message('managerState')
- msg.managerState.processes = states
- pm.send('managerState', msg)
-
- # Exit main loop when uninstall is needed
- if params.get("DoUninstall", encoding='utf8') == "1":
- break
-
-def manager_prepare():
- # build all processes
- os.chdir(os.path.dirname(os.path.abspath(__file__)))
-
- total = 100.0 - (0 if PREBUILT else MAX_BUILD_PROGRESS)
-
- for i, p in enumerate(managed_processes):
- perc = (100.0 - total) + total * (i + 1) / len(managed_processes)
- spinner.update_progress(perc, 100.)
- prepare_managed_process(p)
-
-def main():
- params = Params()
- params.manager_start()
-
- default_params = [
- ("CommunityFeaturesToggle", "0"),
- ("CompletedTrainingVersion", "0"),
- ("IsRHD", "0"),
- ("IsMetric", "0"),
- ("RecordFront", "0"),
- ("HasAcceptedTerms", "0"),
- ("HasCompletedSetup", "0"),
- ("IsUploadRawEnabled", "1"),
- ("IsLdwEnabled", "1"),
- ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')),
- ("OpenpilotEnabledToggle", "1"),
- ("VisionRadarToggle", "0"),
- ("LaneChangeEnabled", "1"),
- ("IsDriverViewEnabled", "0"),
- ]
-
- # set unset params
- for k, v in default_params:
- if params.get(k) is None:
- params.put(k, v)
-
- # is this dashcam?
- if os.getenv("PASSIVE") is not None:
- params.put("Passive", str(int(os.getenv("PASSIVE"))))
-
- if params.get("Passive") is None:
- raise Exception("Passive must be set to continue")
-
- if EON:
- update_apks()
- manager_init()
- manager_prepare()
- spinner.close()
-
- if os.getenv("PREPAREONLY") is not None:
- return
-
- # SystemExit on sigterm
- signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
-
- try:
- manager_thread()
- except Exception:
- traceback.print_exc()
- crash.capture_exception()
- finally:
- cleanup_all_processes(None, None)
-
- if params.get("DoUninstall", encoding='utf8') == "1":
- cloudlog.warning("uninstalling")
- HARDWARE.uninstall()
-
-
-if __name__ == "__main__":
- try:
- main()
- except Exception:
- add_logentries_handler(cloudlog)
- cloudlog.exception("Manager failed to start")
-
- # Show last 3 lines of traceback
- error = traceback.format_exc(-3)
- error = "Manager failed to start\n\n" + error
- spinner.close()
- with TextWindow(error) as t:
- t.wait_for_exit()
-
- raise
-
- # manual exit because we are forked
- sys.exit(0)
diff --git a/panda/board/pedal/obj/.gitkeep b/selfdrive/manager/__init__.py
similarity index 100%
rename from panda/board/pedal/obj/.gitkeep
rename to selfdrive/manager/__init__.py
diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py
new file mode 100755
index 00000000..84f2e48a
--- /dev/null
+++ b/selfdrive/manager/build.py
@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+import os
+import shutil
+import subprocess
+import sys
+import time
+import textwrap
+
+# NOTE: Do NOT import anything here that needs be built (e.g. params)
+from common.basedir import BASEDIR
+from common.spinner import Spinner
+from common.text_window import TextWindow
+from selfdrive.swaglog import cloudlog, add_file_handler
+from selfdrive.version import dirty
+
+TOTAL_SCONS_NODES = 2405
+MAX_BUILD_PROGRESS = 100
+PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt'))
+
+
+def build(spinner, dirty=False):
+ env = os.environ.copy()
+ env['SCONS_PROGRESS'] = "1"
+ env['SCONS_CACHE'] = "1"
+ nproc = os.cpu_count()
+ j_flag = "" if nproc is None else f"-j{nproc - 1}"
+
+ for retry in [True, False]:
+ scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
+
+ compile_output = []
+
+ # Read progress from stderr and update spinner
+ while scons.poll() is None:
+ try:
+ line = scons.stderr.readline()
+ if line is None:
+ continue
+ line = line.rstrip()
+
+ prefix = b'progress: '
+ if line.startswith(prefix):
+ i = int(line[len(prefix):])
+ spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.)
+ elif len(line):
+ compile_output.append(line)
+ print(line.decode('utf8', 'replace'))
+ except Exception:
+ pass
+
+ if scons.returncode != 0:
+ # Read remaining output
+ r = scons.stderr.read().split(b'\n')
+ compile_output += r
+
+ if retry and (not dirty):
+ if not os.getenv("CI"):
+ print("scons build failed, cleaning in")
+ for i in range(3, -1, -1):
+ print("....%d" % i)
+ time.sleep(1)
+ subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
+ shutil.rmtree("/tmp/scons_cache", ignore_errors=True)
+ shutil.rmtree("/data/scons_cache", ignore_errors=True)
+ else:
+ print("scons build failed after retry")
+ sys.exit(1)
+ else:
+ # Build failed log errors
+ errors = [line.decode('utf8', 'replace') for line in compile_output
+ if any([err in line for err in [b'error: ', b'not found, needed by target']])]
+ error_s = "\n".join(errors)
+ add_file_handler(cloudlog)
+ cloudlog.error("scons build failed\n" + error_s)
+
+ # Show TextWindow
+ spinner.close()
+ error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors])
+ with TextWindow("openpilot failed to build\n \n" + error_s) as t:
+ t.wait_for_exit()
+ exit(1)
+ else:
+ break
+
+
+if __name__ == "__main__" and not PREBUILT:
+ spinner = Spinner()
+ spinner.update_progress(0, 100)
+ build(spinner, dirty)
diff --git a/selfdrive/manager/helpers.py b/selfdrive/manager/helpers.py
new file mode 100644
index 00000000..fdda0deb
--- /dev/null
+++ b/selfdrive/manager/helpers.py
@@ -0,0 +1,38 @@
+import os
+import sys
+import fcntl
+import errno
+import signal
+
+
+def unblock_stdout():
+ # get a non-blocking stdout
+ child_pid, child_pty = os.forkpty()
+ if child_pid != 0: # parent
+
+ # child is in its own process group, manually pass kill signals
+ signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT))
+ signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM))
+
+ fcntl.fcntl(sys.stdout, fcntl.F_SETFL, fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK)
+
+ while True:
+ try:
+ dat = os.read(child_pty, 4096)
+ except OSError as e:
+ if e.errno == errno.EIO:
+ break
+ continue
+
+ if not dat:
+ break
+
+ try:
+ sys.stdout.write(dat.decode('utf8'))
+ except (OSError, IOError, UnicodeDecodeError):
+ pass
+
+ # os.wait() returns a tuple with the pid and a 16 bit value
+ # whose low byte is the signal number and whose high byte is the exit satus
+ exit_status = os.wait()[1] >> 8
+ os._exit(exit_status)
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
new file mode 100755
index 00000000..393b5139
--- /dev/null
+++ b/selfdrive/manager/manager.py
@@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+import datetime
+import os
+import signal
+import subprocess
+import sys
+import traceback
+
+import cereal.messaging as messaging
+import selfdrive.crash as crash
+from common.basedir import BASEDIR
+from common.params import Params
+from common.text_window import TextWindow
+from selfdrive.hardware import HARDWARE
+from selfdrive.manager.helpers import unblock_stdout
+from selfdrive.manager.process import ensure_running
+from selfdrive.manager.process_config import managed_processes
+from selfdrive.registration import register
+from selfdrive.swaglog import cloudlog, add_file_handler
+from selfdrive.version import dirty, version
+
+
+def manager_init():
+ params = Params()
+ params.manager_start()
+
+ default_params = [
+ ("CommunityFeaturesToggle", "0"),
+ ("EndToEndToggle", "0"),
+ ("CompletedTrainingVersion", "0"),
+ ("IsRHD", "0"),
+ ("IsMetric", "0"),
+ ("RecordFront", "0"),
+ ("HasAcceptedTerms", "0"),
+ ("HasCompletedSetup", "0"),
+ ("IsUploadRawEnabled", "1"),
+ ("IsLdwEnabled", "0"),
+ ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')),
+ ("OpenpilotEnabledToggle", "1"),
+ ("VisionRadarToggle", "0"),
+ ("IsDriverViewEnabled", "0"),
+ ]
+
+ if params.get("RecordFrontLock", encoding='utf-8') == "1":
+ params.put("RecordFront", "1")
+
+ # set unset params
+ for k, v in default_params:
+ if params.get(k) is None:
+ params.put(k, v)
+
+ # is this dashcam?
+ if os.getenv("PASSIVE") is not None:
+ params.put("Passive", str(int(os.getenv("PASSIVE"))))
+
+ if params.get("Passive") is None:
+ raise Exception("Passive must be set to continue")
+
+ os.umask(0) # Make sure we can create files with 777 permissions
+
+ # Create folders needed for msgq
+ try:
+ os.mkdir("/dev/shm")
+ except FileExistsError:
+ pass
+ except PermissionError:
+ print("WARNING: failed to make /dev/shm")
+
+ # set dongle id
+ reg_res = register(show_spinner=True)
+ if reg_res:
+ dongle_id = reg_res
+ else:
+ raise Exception("server registration failed")
+ os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog and loggerd
+
+ if not dirty:
+ os.environ['CLEAN'] = '1'
+
+ cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty,
+ device=HARDWARE.get_device_type())
+ crash.bind_user(id=dongle_id)
+ crash.bind_extra(version=version, dirty=dirty, device=HARDWARE.get_device_type())
+
+
+def manager_prepare():
+ for p in managed_processes.values():
+ p.prepare()
+
+
+def manager_cleanup():
+ for p in managed_processes.values():
+ p.stop()
+
+ cloudlog.info("everything is dead")
+
+
+def manager_thread():
+ cloudlog.info("manager start")
+ cloudlog.info({"environ": os.environ})
+
+ # save boot log
+ subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
+
+ ignore = []
+ if os.getenv("NOBOARD") is not None:
+ ignore.append("pandad")
+ if os.getenv("BLOCK") is not None:
+ ignore += os.getenv("BLOCK").split(",")
+
+ ensure_running(managed_processes.values(), started=False, not_run=ignore)
+
+ started_prev = False
+ params = Params()
+ sm = messaging.SubMaster(['deviceState'])
+ pm = messaging.PubMaster(['managerState'])
+
+ while True:
+ sm.update()
+ not_run = ignore[:]
+
+ if sm['deviceState'].freeSpacePercent < 5:
+ not_run.append("loggerd")
+
+ started = sm['deviceState'].started
+ driverview = params.get("IsDriverViewEnabled") == b"1"
+ ensure_running(managed_processes.values(), started, driverview, not_run)
+
+ # trigger an update after going offroad
+ if started_prev and not started and 'updated' in managed_processes:
+ os.sync()
+ managed_processes['updated'].signal(signal.SIGHUP)
+
+ started_prev = started
+
+ running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
+ for p in managed_processes.values() if p.proc]
+ cloudlog.debug(' '.join(running_list))
+
+ # send managerState
+ msg = messaging.new_message('managerState')
+ msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()]
+ pm.send('managerState', msg)
+
+ # Exit main loop when uninstall is needed
+ if params.get("DoUninstall", encoding='utf8') == "1":
+ break
+
+
+def main():
+ prepare_only = os.getenv("PREPAREONLY") is not None
+
+ manager_init()
+
+ # Start ui early so prepare can happen in the background
+ if not prepare_only:
+ managed_processes['ui'].start()
+
+ manager_prepare()
+
+ if prepare_only:
+ return
+
+ # SystemExit on sigterm
+ signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
+
+ try:
+ manager_thread()
+ except Exception:
+ traceback.print_exc()
+ crash.capture_exception()
+ finally:
+ manager_cleanup()
+
+ if Params().get("DoUninstall", encoding='utf8') == "1":
+ cloudlog.warning("uninstalling")
+ HARDWARE.uninstall()
+
+
+if __name__ == "__main__":
+ unblock_stdout()
+
+ try:
+ main()
+ except Exception:
+ add_file_handler(cloudlog)
+ cloudlog.exception("Manager failed to start")
+
+ # Show last 3 lines of traceback
+ error = traceback.format_exc(-3)
+ error = "Manager failed to start\n\n" + error
+ with TextWindow(error) as t:
+ t.wait_for_exit()
+
+ raise
+
+ # manual exit because we are forked
+ sys.exit(0)
diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py
new file mode 100644
index 00000000..ccf780b6
--- /dev/null
+++ b/selfdrive/manager/process.py
@@ -0,0 +1,290 @@
+import importlib
+import os
+import signal
+import time
+import subprocess
+from abc import ABC, abstractmethod
+from multiprocessing import Process
+
+from setproctitle import setproctitle # pylint: disable=no-name-in-module
+
+import cereal.messaging as messaging
+import selfdrive.crash as crash
+from common.basedir import BASEDIR
+from common.params import Params
+from common.realtime import sec_since_boot
+from selfdrive.swaglog import cloudlog
+from selfdrive.hardware import HARDWARE
+from cereal import log
+
+WATCHDOG_FN = "/dev/shm/wd_"
+ENABLE_WATCHDOG = os.getenv("NO_WATCHDOG") is None
+
+
+def launcher(proc):
+ try:
+ # import the process
+ mod = importlib.import_module(proc)
+
+ # rename the process
+ setproctitle(proc)
+
+ # create new context since we forked
+ messaging.context = messaging.Context()
+
+ # exec the process
+ mod.main()
+ except KeyboardInterrupt:
+ cloudlog.warning("child %s got SIGINT" % proc)
+ except Exception:
+ # can't install the crash handler becuase sys.excepthook doesn't play nice
+ # with threads, so catch it here.
+ crash.capture_exception()
+ raise
+
+
+def nativelauncher(pargs, cwd):
+ # exec the process
+ os.chdir(cwd)
+ os.execvp(pargs[0], pargs)
+
+
+def join_process(process, timeout):
+ # Process().join(timeout) will hang due to a python 3 bug: https://bugs.python.org/issue28382
+ # We have to poll the exitcode instead
+ t = time.monotonic()
+ while time.monotonic() - t < timeout and process.exitcode is None:
+ time.sleep(0.001)
+
+
+class ManagerProcess(ABC):
+ unkillable = False
+ daemon = False
+ sigkill = False
+ proc = None
+ enabled = True
+ name = ""
+
+ last_watchdog_time = 0
+ watchdog_max_dt = None
+ watchdog_seen = False
+ shutting_down = False
+
+ @abstractmethod
+ def prepare(self):
+ pass
+
+ @abstractmethod
+ def start(self):
+ pass
+
+ def restart(self):
+ self.stop()
+ self.start()
+
+ def check_watchdog(self, started):
+ if self.watchdog_max_dt is None or self.proc is None:
+ return
+
+ try:
+ fn = WATCHDOG_FN + str(self.proc.pid)
+ self.last_watchdog_time = int(open(fn).read())
+ except Exception:
+ pass
+
+ dt = sec_since_boot() - self.last_watchdog_time / 1e9
+
+ if dt > self.watchdog_max_dt:
+ # Only restart while offroad for now
+ if self.watchdog_seen and ENABLE_WATCHDOG and (not started):
+ cloudlog.error(f"Watchdog timeout for {self.name} (exitcode {self.proc.exitcode}) restarting")
+ self.restart()
+ else:
+ self.watchdog_seen = True
+
+ def stop(self, retry=True, block=True):
+ if self.proc is None:
+ return
+
+ if self.proc.exitcode is None:
+ if not self.shutting_down:
+ cloudlog.info(f"killing {self.name}")
+ sig = signal.SIGKILL if self.sigkill else signal.SIGINT
+ self.signal(sig)
+ self.shutting_down = True
+
+ if not block:
+ return
+
+ join_process(self.proc, 5)
+
+ # If process failed to die send SIGKILL or reboot
+ if self.proc.exitcode is None and retry:
+ if self.unkillable:
+ cloudlog.critical(f"unkillable process {self.name} failed to exit! rebooting in 15 if it doesn't die")
+ join_process(self.proc, 15)
+
+ if self.proc.exitcode is None:
+ cloudlog.critical(f"unkillable process {self.name} failed to die!")
+ os.system("date >> /data/unkillable_reboot")
+ os.sync()
+ HARDWARE.reboot()
+ raise RuntimeError
+ else:
+ cloudlog.info(f"killing {self.name} with SIGKILL")
+ self.signal(signal.SIGKILL)
+ self.proc.join()
+
+ ret = self.proc.exitcode
+ cloudlog.info(f"{self.name} is dead with {ret}")
+
+ if self.proc.exitcode is not None:
+ self.shutting_down = False
+ self.proc = None
+
+ return ret
+
+ def signal(self, sig):
+ if self.proc is None:
+ return
+
+ # Don't signal if already exited
+ if self.proc.exitcode is not None and self.proc.pid is not None:
+ return
+
+ cloudlog.info(f"sending signal {sig} to {self.name}")
+ os.kill(self.proc.pid, sig)
+
+ def get_process_state_msg(self):
+ state = log.ManagerState.ProcessState.new_message()
+ state.name = self.name
+ if self.proc:
+ state.running = self.proc.is_alive()
+ state.pid = self.proc.pid or 0
+ state.exitCode = self.proc.exitcode or 0
+ return state
+
+
+class NativeProcess(ManagerProcess):
+ def __init__(self, name, cwd, cmdline, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
+ self.name = name
+ self.cwd = cwd
+ self.cmdline = cmdline
+ self.enabled = enabled
+ self.persistent = persistent
+ self.driverview = driverview
+ self.unkillable = unkillable
+ self.sigkill = sigkill
+ self.watchdog_max_dt = watchdog_max_dt
+
+ def prepare(self):
+ pass
+
+ def start(self):
+ # In case we only tried a non blocking stop we need to stop it before restarting
+ if self.shutting_down:
+ self.stop()
+
+ if self.proc is not None:
+ return
+
+ cwd = os.path.join(BASEDIR, self.cwd)
+ cloudlog.info("starting process %s" % self.name)
+ self.proc = Process(name=self.name, target=nativelauncher, args=(self.cmdline, cwd))
+ self.proc.start()
+ self.watchdog_seen = False
+ self.shutting_down = False
+
+
+class PythonProcess(ManagerProcess):
+ def __init__(self, name, module, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
+ self.name = name
+ self.module = module
+ self.enabled = enabled
+ self.persistent = persistent
+ self.driverview = driverview
+ self.unkillable = unkillable
+ self.sigkill = sigkill
+ self.watchdog_max_dt = watchdog_max_dt
+
+ def prepare(self):
+ if self.enabled:
+ cloudlog.info("preimporting %s" % self.module)
+ importlib.import_module(self.module)
+
+ def start(self):
+ # In case we only tried a non blocking stop we need to stop it before restarting
+ if self.shutting_down:
+ self.stop()
+
+ if self.proc is not None:
+ return
+
+ cloudlog.info("starting python %s" % self.module)
+ self.proc = Process(name=self.name, target=launcher, args=(self.module,))
+ self.proc.start()
+ self.watchdog_seen = False
+ self.shutting_down = False
+
+
+class DaemonProcess(ManagerProcess):
+ """Python process that has to stay running accross manager restart.
+ This is used for athena so you don't lose SSH access when restarting manager."""
+ def __init__(self, name, module, param_name, enabled=True):
+ self.name = name
+ self.module = module
+ self.param_name = param_name
+ self.enabled = enabled
+ self.persistent = True
+
+ def prepare(self):
+ pass
+
+ def start(self):
+ params = Params()
+ pid = params.get(self.param_name, encoding='utf-8')
+
+ if pid is not None:
+ try:
+ os.kill(int(pid), 0)
+ with open(f'/proc/{pid}/cmdline') as f:
+ if self.module in f.read():
+ # daemon is running
+ return
+ except (OSError, FileNotFoundError):
+ # process is dead
+ pass
+
+ cloudlog.info("starting daemon %s" % self.name)
+ proc = subprocess.Popen(['python', '-m', self.module], # pylint: disable=subprocess-popen-preexec-fn
+ stdin=open('/dev/null', 'r'),
+ stdout=open('/dev/null', 'w'),
+ stderr=open('/dev/null', 'w'),
+ preexec_fn=os.setpgrp)
+
+ params.put(self.param_name, str(proc.pid))
+
+ def stop(self, retry=True, block=True):
+ pass
+
+
+def ensure_running(procs, started, driverview=False, not_run=None):
+ if not_run is None:
+ not_run = []
+
+ for p in procs:
+ if p.name in not_run:
+ p.stop(block=False)
+ elif not p.enabled:
+ p.stop(block=False)
+ elif p.persistent:
+ p.start()
+ elif p.driverview and driverview:
+ p.start()
+ elif started:
+ p.start()
+ else:
+ p.stop(block=False)
+
+ p.check_watchdog(started)
+
diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
new file mode 100644
index 00000000..f0ae7da2
--- /dev/null
+++ b/selfdrive/manager/process_config.py
@@ -0,0 +1,39 @@
+import os
+
+from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
+from selfdrive.hardware import EON, TICI, PC
+
+WEBCAM = os.getenv("WEBCAM") is not None
+
+procs = [
+ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
+ # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption
+ NativeProcess("camerad", "selfdrive/camerad", ["./camerad"], unkillable=True, driverview=True),
+ NativeProcess("clocksd", "selfdrive/clocksd", ["./clocksd"]),
+ NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), driverview=True),
+ NativeProcess("logcatd", "selfdrive/logcatd", ["./logcatd"]),
+ NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"]),
+ NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
+ NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]),
+ NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=EON, sigkill=EON),
+ NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),
+ NativeProcess("ui", "selfdrive/ui", ["./ui"], persistent=True, watchdog_max_dt=(10 if TICI else None)),
+ PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
+ PythonProcess("controlsd", "selfdrive.controls.controlsd"),
+ PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True),
+ PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True),
+ PythonProcess("locationd", "selfdrive.locationd.locationd"),
+ PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True),
+ PythonProcess("pandad", "selfdrive.pandad", persistent=True),
+ PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
+ PythonProcess("plannerd", "selfdrive.controls.plannerd"),
+ PythonProcess("radard", "selfdrive.controls.radard"),
+ PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON),
+ PythonProcess("thermald", "selfdrive.thermald.thermald", persistent=True),
+ PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, persistent=True),
+ PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, persistent=True),
+ PythonProcess("updated", "selfdrive.updated", enabled=not PC, persistent=True),
+ PythonProcess("uploader", "selfdrive.loggerd.uploader", persistent=True),
+]
+
+managed_processes = {p.name: p for p in procs}
diff --git a/selfdrive/manager/test/__init__.py b/selfdrive/manager/test/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/selfdrive/test/test_manager.py b/selfdrive/manager/test/test_manager.py
similarity index 69%
rename from selfdrive/test/test_manager.py
rename to selfdrive/manager/test/test_manager.py
index 9a666e96..e19311f0 100644
--- a/selfdrive/test/test_manager.py
+++ b/selfdrive/manager/test/test_manager.py
@@ -4,22 +4,24 @@ import signal
import time
import unittest
-os.environ['FAKEUPLOAD'] = "1"
-
-import selfdrive.manager as manager
+import selfdrive.manager.manager as manager
from selfdrive.hardware import EON
+from selfdrive.manager.process import DaemonProcess
+from selfdrive.manager.process_config import managed_processes
+
+os.environ['FAKEUPLOAD'] = "1"
# TODO: make eon fast
MAX_STARTUP_TIME = 30 if EON else 15
-ALL_PROCESSES = manager.persistent_processes + manager.car_started_processes
+ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and (p.name not in ['updated', 'pandad'])]
+
class TestManager(unittest.TestCase):
-
def setUp(self):
os.environ['PASSIVE'] = '0'
def tearDown(self):
- manager.cleanup_all_processes(None, None)
+ manager.manager_cleanup()
def test_manager_prepare(self):
os.environ['PREPAREONLY'] = '1'
@@ -36,24 +38,27 @@ class TestManager(unittest.TestCase):
# ensure all processes exit cleanly
def test_clean_exit(self):
manager.manager_prepare()
- for p in ALL_PROCESSES:
- manager.start_managed_process(p)
- time.sleep(10)
+ for p in ALL_PROCESSES:
+ managed_processes[p].start()
+
+ time.sleep(30)
for p in reversed(ALL_PROCESSES):
- exit_code = manager.kill_managed_process(p, retry=False)
+ exit_code = managed_processes[p].stop(retry=False)
if (not EON and p == 'ui') or (EON and p == 'logcatd'):
# TODO: make Qt UI exit gracefully and fix OMX encoder exiting
continue
+ # Make sure the process is actually dead
+ managed_processes[p].stop()
+
# TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code
exit_codes = [0, 1]
- if p in manager.kill_processes:
+ if managed_processes[p].sigkill:
exit_codes = [-signal.SIGKILL]
assert exit_code in exit_codes, f"{p} died with {exit_code}"
-
if __name__ == "__main__":
unittest.main()
diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc
index 48b16980..b21bbd78 100644
--- a/selfdrive/modeld/dmonitoringmodeld.cc
+++ b/selfdrive/modeld/dmonitoringmodeld.cc
@@ -1,10 +1,7 @@
#include
#include
-#include
-#include
#include
-#include "visionbuf.h"
#include "visionipc_client.h"
#include "common/swaglog.h"
#include "common/util.h"
@@ -15,51 +12,47 @@
#include
#endif
-
ExitHandler do_exit;
+void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
+ PubMaster pm({"driverState"});
+ double last = 0;
+
+ while (!do_exit) {
+ VisionIpcBufExtra extra = {};
+ VisionBuf *buf = vipc_client.recv(&extra);
+ if (buf == nullptr) continue;
+
+ double t1 = millis_since_boot();
+ DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height);
+ double t2 = millis_since_boot();
+
+ // send dm packet
+ dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output);
+
+ //printf("dmonitoring process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last);
+ last = t1;
+ }
+}
+
int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
- PubMaster pm({"driverState"});
-
// init the models
- DMonitoringModelState dmonitoringmodel;
- dmonitoring_init(&dmonitoringmodel);
+ DMonitoringModelState model;
+ dmonitoring_init(&model);
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true);
- while (!do_exit){
- if (!vipc_client.connect(false)){
- util::sleep_for(100);
- continue;
- }
- break;
+ while (!do_exit && !vipc_client.connect(false)) {
+ util::sleep_for(100);
}
- while (!do_exit) {
+ // run the models
+ if (vipc_client.connected) {
LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
-
- double last = 0;
- while (!do_exit) {
- VisionIpcBufExtra extra = {0};
- VisionBuf *buf = vipc_client.recv(&extra);
- if (buf == nullptr){
- continue;
- }
-
- double t1 = millis_since_boot();
- DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf->width, buf->height);
- double t2 = millis_since_boot();
-
- // send dm packet
- dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0, dmonitoringmodel.output);
-
- LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
- last = t1;
- }
+ run_model(model, vipc_client);
}
- dmonitoring_free(&dmonitoringmodel);
-
+ dmonitoring_free(&model);
return 0;
}
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index c43e9588..4eb64cfd 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -1,9 +1,8 @@
#include
#include
-#include
+#include
#include
-#include "visionbuf.h"
#include "visionipc_client.h"
#include "common/swaglog.h"
#include "common/clutil.h"
@@ -14,12 +13,12 @@
ExitHandler do_exit;
// globals
-bool run_model;
+bool live_calib_seen;
mat3 cur_transform;
-pthread_mutex_t transform_lock;
+std::mutex transform_lock;
-void* live_thread(void *arg) {
- set_thread_name("live");
+void calibration_thread() {
+ set_thread_name("calibration");
set_realtime_priority(50);
SubMaster sm({"liveCalibration"});
@@ -36,26 +35,8 @@ void* live_thread(void *arg) {
-1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
-1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
- Eigen::Matrix fcam_intrinsics;
-#ifndef QCOM2
- fcam_intrinsics <<
- 910.0, 0.0, 582.0,
- 0.0, 910.0, 437.0,
- 0.0, 0.0, 1.0;
- float db_s = 0.5; // debayering does a 2x downscale
-#else
- fcam_intrinsics <<
- 2648.0, 0.0, 1928.0/2,
- 0.0, 2648.0, 1208.0/2,
- 0.0, 0.0, 1.0;
- float db_s = 1.0;
-#endif
-
- mat3 yuv_transform = transform_scale_buffer((mat3){{
- 1.0, 0.0, 0.0,
- 0.0, 1.0, 0.0,
- 0.0, 0.0, 1.0,
- }}, db_s);
+ Eigen::Matrix fcam_intrinsics = Eigen::Matrix(fcam_intrinsic_matrix.v);
+ const mat3 yuv_transform = get_model_yuv_transform();
while (!do_exit) {
if (sm.update(100) > 0){
@@ -78,17 +59,77 @@ void* live_thread(void *arg) {
transform.v[i] = warp_matrix(i / 3, i % 3);
}
mat3 model_transform = matmul3(yuv_transform, transform);
- pthread_mutex_lock(&transform_lock);
+ std::lock_guard lk(transform_lock);
cur_transform = model_transform;
- run_model = true;
- pthread_mutex_unlock(&transform_lock);
+ live_calib_seen = true;
+ }
+ }
+}
+
+void run_model(ModelState &model, VisionIpcClient &vipc_client) {
+ // messaging
+ PubMaster pm({"modelV2", "cameraOdometry"});
+ SubMaster sm({"lateralPlan", "roadCameraState"});
+
+ // setup filter to track dropped frames
+ const float dt = 1. / MODEL_FREQ;
+ const float ts = 10.0; // filter time constant (s)
+ const float frame_filter_k = (dt / ts) / (1. + dt / ts);
+ float frames_dropped = 0;
+
+ uint32_t frame_id = 0, last_vipc_frame_id = 0;
+ double last = 0;
+ int desire = -1;
+ uint32_t run_count = 0;
+
+ while (!do_exit) {
+ VisionIpcBufExtra extra = {};
+ VisionBuf *buf = vipc_client.recv(&extra);
+ if (buf == nullptr) continue;
+
+ transform_lock.lock();
+ mat3 model_transform = cur_transform;
+ const bool run_model_this_iter = live_calib_seen;
+ transform_lock.unlock();
+
+ if (sm.update(0) > 0) {
+ // TODO: path planner timeout?
+ desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
+ frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
+ }
+
+ if (run_model_this_iter) {
+ run_count++;
+
+ float vec_desire[DESIRE_LEN] = {0};
+ if (desire >= 0 && desire < DESIRE_LEN) {
+ vec_desire[desire] = 1.0;
+ }
+
+ double mt1 = millis_since_boot();
+ ModelDataRaw model_buf = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
+ model_transform, vec_desire);
+ double mt2 = millis_since_boot();
+ float model_execution_time = (mt2 - mt1) / 1000.0;
+
+ // tracked dropped frames
+ uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
+ frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U);
+ if (run_count < 10) frames_dropped = 0; // let frame drops warm up
+ float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
+
+ model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time,
+ kj::ArrayPtr(model.output.data(), model.output.size()));
+ posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
+
+ //printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
+ last = mt1;
+ last_vipc_frame_id = extra.frame_id;
}
}
- return NULL;
}
int main(int argc, char **argv) {
- int err;
set_realtime_priority(54);
#ifdef QCOM
@@ -98,16 +139,8 @@ int main(int argc, char **argv) {
set_core_affinity(4);
#endif
- pthread_mutex_init(&transform_lock, NULL);
-
// start calibration thread
- pthread_t live_thread_handle;
- err = pthread_create(&live_thread_handle, NULL, live_thread, NULL);
- assert(err == 0);
-
- // messaging
- PubMaster pm({"modelV2", "cameraOdometry"});
- SubMaster sm({"lateralPlan", "roadCameraState"});
+ std::thread thread = std::thread(calibration_thread);
// cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
@@ -119,90 +152,21 @@ int main(int argc, char **argv) {
LOGW("models loaded, modeld starting");
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context);
-
- while (!do_exit){
- if (!vipc_client.connect(false)){
- util::sleep_for(100);
- continue;
- }
- break;
+ while (!do_exit && !vipc_client.connect(false)) {
+ util::sleep_for(100);
}
- // loop
- while (!do_exit) {
- VisionBuf *b = &vipc_client.buffers[0];
+ // run the models
+ // vipc_client.connected is false only when do_exit is true
+ if (vipc_client.connected) {
+ const VisionBuf *b = &vipc_client.buffers[0];
LOGW("connected with buffer size: %d (%d x %d)", b->len, b->width, b->height);
-
- // setup filter to track dropped frames
- const float dt = 1. / MODEL_FREQ;
- const float ts = 10.0; // filter time constant (s)
- const float frame_filter_k = (dt / ts) / (1. + dt / ts);
- float frames_dropped = 0;
-
- uint32_t frame_id = 0, last_vipc_frame_id = 0;
- double last = 0;
- int desire = -1;
- uint32_t run_count = 0;
-
- while (!do_exit) {
- VisionIpcBufExtra extra;
- VisionBuf *buf = vipc_client.recv(&extra);
- if (buf == nullptr){
- continue;
- }
-
- pthread_mutex_lock(&transform_lock);
- mat3 model_transform = cur_transform;
- const bool run_model_this_iter = run_model;
- pthread_mutex_unlock(&transform_lock);
-
- if (sm.update(0) > 0){
- // TODO: path planner timeout?
- desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
- frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
- }
-
- double mt1 = 0, mt2 = 0;
- if (run_model_this_iter) {
- run_count++;
-
- float vec_desire[DESIRE_LEN] = {0};
- if (desire >= 0 && desire < DESIRE_LEN) {
- vec_desire[desire] = 1.0;
- }
-
- mt1 = millis_since_boot();
-
- ModelDataRaw model_buf =
- model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
- model_transform, vec_desire);
- mt2 = millis_since_boot();
- float model_execution_time = (mt2 - mt1) / 1000.0;
-
- // tracked dropped frames
- uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
- frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U);
- if (run_count < 10) frames_dropped = 0; // let frame drops warm up
- float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
-
- model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time,
- kj::ArrayPtr(model.output.data(), model.output.size()));
- posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
-
- LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio);
- last = mt1;
- last_vipc_frame_id = extra.frame_id;
- }
-
- }
+ run_model(model, vipc_client);
}
model_free(&model);
-
- LOG("joining live_thread");
- err = pthread_join(live_thread_handle, NULL);
- assert(err == 0);
+ LOG("joining calibration thread");
+ thread.join();
CL_CHECK(clReleaseContext(context));
- pthread_mutex_destroy(&transform_lock);
return 0;
}
diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc
index 7280a156..4ce25580 100644
--- a/selfdrive/modeld/models/commonmodel.cc
+++ b/selfdrive/modeld/models/commonmodel.cc
@@ -1,5 +1,6 @@
#include
#include
+#include
#include "commonmodel.h"
#include "common/clutil.h"
#include "common/mat.h"
@@ -48,18 +49,10 @@ void frame_free(ModelFrame* frame) {
}
void softmax(const float* input, float* output, size_t len) {
- float max_val = -FLT_MAX;
- for(int i = 0; i < len; i++) {
- const float v = input[i];
- if( v > max_val ) {
- max_val = v;
- }
- }
-
+ const float max_val = *std::max_element(input, input + len);
float denominator = 0;
for(int i = 0; i < len; i++) {
- float const v = input[i];
- float const v_exp = expf(v - max_val);
+ float const v_exp = expf(input[i] - max_val);
denominator += v_exp;
output[i] = v_exp;
}
@@ -68,7 +61,6 @@ void softmax(const float* input, float* output, size_t len) {
for(int i = 0; i < len; i++) {
output[i] *= inv_denominator;
}
-
}
float sigmoid(float input) {
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index 2a857f27..7b0c2139 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -18,19 +18,19 @@
constexpr int DESIRE_LEN = 8;
constexpr int TRAFFIC_CONVENTION_LEN = 2;
constexpr int MODEL_FREQ = 20;
-struct ModelDataRaw {
- float *plan;
- float *lane_lines;
- float *lane_lines_prob;
- float *road_edges;
- float *lead;
- float *lead_prob;
- float *desire_state;
- float *meta;
- float *desire_pred;
- float *pose;
- };
+struct ModelDataRaw {
+ float *plan;
+ float *lane_lines;
+ float *lane_lines_prob;
+ float *road_edges;
+ float *lead;
+ float *lead_prob;
+ float *desire_state;
+ float *meta;
+ float *desire_pred;
+ float *pose;
+};
typedef struct ModelState {
ModelFrame frame;
diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc
index 481a0775..e11fcb8a 100644
--- a/selfdrive/modeld/thneed/thneed.cc
+++ b/selfdrive/modeld/thneed/thneed.cc
@@ -5,6 +5,7 @@
#include
#include
#include
+#include "common/timing.h"
#include "common/clutil.h"
#include "thneed.h"
@@ -17,12 +18,6 @@ map, string> g_args;
map, int> g_args_size;
map g_program_source;
-static inline uint64_t nanos_since_boot() {
- struct timespec t;
- clock_gettime(CLOCK_BOOTTIME, &t);
- return t.tv_sec * 1000000000ULL + t.tv_nsec;
-}
-
void hexdump(uint32_t *d, int len) {
assert((len%4) == 0);
printf(" dumping %p len 0x%x\n", d, len);
diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc
index 3ea79949..056267ac 100644
--- a/selfdrive/modeld/transforms/transform.cc
+++ b/selfdrive/modeld/transforms/transform.cc
@@ -28,7 +28,7 @@ void transform_queue(Transform* s,
cl_mem in_yuv, int in_width, int in_height,
cl_mem out_y, cl_mem out_u, cl_mem out_v,
int out_width, int out_height,
- mat3 projection) {
+ const mat3& projection) {
const int zero = 0;
// sampled using pixel center origin
diff --git a/selfdrive/modeld/transforms/transform.h b/selfdrive/modeld/transforms/transform.h
index 21933f4d..3bce6f8a 100644
--- a/selfdrive/modeld/transforms/transform.h
+++ b/selfdrive/modeld/transforms/transform.h
@@ -25,4 +25,4 @@ void transform_queue(Transform* s, cl_command_queue q,
cl_mem yuv, int in_width, int in_height,
cl_mem out_y, cl_mem out_u, cl_mem out_v,
int out_width, int out_height,
- mat3 projection);
+ const mat3& projection);
diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py
index b0d2cf73..d4cc9847 100755
--- a/selfdrive/pandad.py
+++ b/selfdrive/pandad.py
@@ -3,13 +3,16 @@
import os
import time
-from panda import BASEDIR as PANDA_BASEDIR, Panda, PandaDFU, build_st
+from panda import BASEDIR as PANDA_BASEDIR, Panda, PandaDFU
from common.basedir import BASEDIR
from common.gpio import gpio_init, gpio_set
from selfdrive.hardware import TICI
from selfdrive.hardware.tici.pins import GPIO_HUB_RST_N, GPIO_STM_BOOT0, GPIO_STM_RST_N
from selfdrive.swaglog import cloudlog
+PANDA_FW_FN = os.path.join(PANDA_BASEDIR, "board", "obj", "panda.bin.signed")
+
+
def set_panda_power(power=True):
if not TICI:
return
@@ -25,24 +28,9 @@ def set_panda_power(power=True):
gpio_set(GPIO_STM_RST_N, not power)
-def get_firmware_fn():
- signed_fn = os.path.join(PANDA_BASEDIR, "board", "obj", "panda.bin.signed")
- if os.path.exists(signed_fn):
- cloudlog.info("Using prebuilt signed firmware")
- return signed_fn
- else:
- cloudlog.info("Building panda firmware")
- fn = "obj/panda.bin"
- build_st(fn, clean=False)
- return os.path.join(PANDA_BASEDIR, "board", fn)
-
-
-def get_expected_signature(fw_fn=None):
- if fw_fn is None:
- fw_fn = get_firmware_fn()
-
+def get_expected_signature():
try:
- return Panda.get_signature_from_firmware(fw_fn)
+ return Panda.get_signature_from_firmware(PANDA_FW_FN)
except Exception:
cloudlog.exception("Error computing expected signature")
return b""
@@ -71,8 +59,7 @@ def update_panda():
time.sleep(1)
- fw_fn = get_firmware_fn()
- fw_signature = get_expected_signature(fw_fn)
+ fw_signature = get_expected_signature()
try:
serial = panda.get_serial()[0].decode("utf-8")
@@ -90,7 +77,7 @@ def update_panda():
if panda.bootstub or panda_signature != fw_signature:
cloudlog.info("Panda firmware out of date, update required")
- panda.flash(fw_fn)
+ panda.flash()
cloudlog.info("Done flashing")
if panda.bootstub:
@@ -111,6 +98,7 @@ def update_panda():
cloudlog.info("Resetting panda")
panda.reset()
+
def main():
set_panda_power()
update_panda()
diff --git a/selfdrive/registration.py b/selfdrive/registration.py
index d74cc091..b38853a9 100644
--- a/selfdrive/registration.py
+++ b/selfdrive/registration.py
@@ -7,6 +7,7 @@ import jwt
from datetime import datetime, timedelta
from common.api import api_get
from common.params import Params
+from common.spinner import Spinner
from common.file_helpers import mkdirs_exists_ok
from common.basedir import PERSIST
from selfdrive.hardware import HARDWARE
@@ -15,7 +16,7 @@ from selfdrive.version import version, terms_version, training_version, get_git_
get_git_branch, get_git_remote
-def register(spinner=None):
+def register(show_spinner=False):
params = Params()
params.put("Version", version)
params.put("TermsVersion", terms_version)
@@ -51,7 +52,8 @@ def register(spinner=None):
needs_registration = needs_registration or dongle_id is None
if needs_registration:
- if spinner is not None:
+ if show_spinner:
+ spinner = Spinner()
spinner.update("registering device")
# Create registration token, in the future, this key will make JWTs directly
@@ -85,7 +87,11 @@ def register(spinner=None):
cloudlog.exception("failed to authenticate")
time.sleep(1)
+ if show_spinner:
+ spinner.close()
+
return dongle_id
+
if __name__ == "__main__":
print(register())
diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc
index cb78ba0a..23e6d071 100644
--- a/selfdrive/sensord/sensors/bmx055_accel.cc
+++ b/selfdrive/sensord/sensors/bmx055_accel.cc
@@ -46,7 +46,7 @@ fail:
void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event){
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
- int len = read_register(BMX055_ACCEL_I2C_REG_FIFO, buffer, sizeof(buffer));
+ int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer));
assert(len == 6);
// 12 bit = +-2g
diff --git a/selfdrive/sensord/sensors/bmx055_accel.hpp b/selfdrive/sensord/sensors/bmx055_accel.hpp
index 4e613af7..2c9173f9 100644
--- a/selfdrive/sensord/sensors/bmx055_accel.hpp
+++ b/selfdrive/sensord/sensors/bmx055_accel.hpp
@@ -7,6 +7,7 @@
// Registers of the chip
#define BMX055_ACCEL_I2C_REG_ID 0x00
+#define BMX055_ACCEL_I2C_REG_X_LSB 0x02
#define BMX055_ACCEL_I2C_REG_TEMP 0x08
#define BMX055_ACCEL_I2C_REG_BW 0x10
#define BMX055_ACCEL_I2C_REG_HBW 0x13
diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc
index 38a2ff42..d7bf0040 100644
--- a/selfdrive/sensord/sensors/bmx055_gyro.cc
+++ b/selfdrive/sensord/sensors/bmx055_gyro.cc
@@ -56,7 +56,7 @@ fail:
void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event){
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
- int len = read_register(BMX055_GYRO_I2C_REG_FIFO, buffer, sizeof(buffer));
+ int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer));
assert(len == 6);
// 16 bit = +- 125 deg/s
diff --git a/selfdrive/sensord/sensors/bmx055_gyro.hpp b/selfdrive/sensord/sensors/bmx055_gyro.hpp
index 407ee160..f808139f 100644
--- a/selfdrive/sensord/sensors/bmx055_gyro.hpp
+++ b/selfdrive/sensord/sensors/bmx055_gyro.hpp
@@ -6,11 +6,12 @@
#define BMX055_GYRO_I2C_ADDR 0x68
// Registers of the chip
-#define BMX055_GYRO_I2C_REG_ID 0x00
-#define BMX055_GYRO_I2C_REG_RANGE 0x0F
-#define BMX055_GYRO_I2C_REG_BW 0x10
-#define BMX055_GYRO_I2C_REG_HBW 0x13
-#define BMX055_GYRO_I2C_REG_FIFO 0x3F
+#define BMX055_GYRO_I2C_REG_ID 0x00
+#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02
+#define BMX055_GYRO_I2C_REG_RANGE 0x0F
+#define BMX055_GYRO_I2C_REG_BW 0x10
+#define BMX055_GYRO_I2C_REG_HBW 0x13
+#define BMX055_GYRO_I2C_REG_FIFO 0x3F
// Constants
#define BMX055_GYRO_CHIP_ID 0x0F
diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc
index f0557031..41b41467 100644
--- a/selfdrive/sensord/sensors/bmx055_magn.cc
+++ b/selfdrive/sensord/sensors/bmx055_magn.cc
@@ -62,31 +62,19 @@ static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t
return (int16_t)retval;
}
-static int16_t parse_xy(uint8_t lsb, uint8_t msb){
- // 13 bit
- uint16_t combined = (uint16_t(msb) << 5) | uint16_t(lsb >> 3);
- return int16_t(combined << 3) / (1 << 3);
-}
-
-static int16_t parse_z(uint8_t lsb, uint8_t msb){
- // 15 bit
- uint16_t combined = (uint16_t(msb) << 7) | uint16_t(lsb >> 1);
- return int16_t(combined << 1) / (1 << 1);
-}
-
-static uint16_t parse_rhall(uint8_t lsb, uint8_t msb){
- // 14 bit
- return (uint16_t(msb) << 6) | uint16_t(lsb >> 2);
-}
-
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Magn::init(){
int ret;
uint8_t buffer[1];
uint8_t trim_x1y1[2] = {0};
- uint8_t trim_xyz_data[4] = {0};
- uint8_t trim_xy1xy2[10] = {0};
+ uint8_t trim_x2y2[2] = {0};
+ uint8_t trim_xy1xy2[2] = {0};
+ uint8_t trim_z1[2] = {0};
+ uint8_t trim_z2[2] = {0};
+ uint8_t trim_z3[2] = {0};
+ uint8_t trim_z4[2] = {0};
+ uint8_t trim_xyz1[2] = {0};
// suspend -> sleep
ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
@@ -110,83 +98,139 @@ int BMX055_Magn::init(){
// Load magnetometer trim
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
- if(ret < 0){
- goto fail;
- }
- ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4, trim_xyz_data, 4);
- if(ret < 0){
- goto fail;
- }
- ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2, trim_xy1xy2, 10);
- if(ret < 0){
- goto fail;
- }
+ if(ret < 0) goto fail;
+ ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
+ if(ret < 0) goto fail;
+ ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
+ if(ret < 0) goto fail;
+ ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
+ if(ret < 0) goto fail;
+ ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
+ if(ret < 0) goto fail;
+ ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
+ if(ret < 0) goto fail;
+ ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
+ if(ret < 0) goto fail;
+ ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
+ if(ret < 0) goto fail;
// Read trim data
- trim_data.dig_x1 = (int8_t)trim_x1y1[0];
- trim_data.dig_y1 = (int8_t)trim_x1y1[1];
+ trim_data.dig_x1 = trim_x1y1[0];
+ trim_data.dig_y1 = trim_x1y1[1];
- trim_data.dig_x2 = (int8_t)trim_xyz_data[2];
- trim_data.dig_y2 = (int8_t)trim_xyz_data[3];
+ trim_data.dig_x2 = trim_x2y2[0];
+ trim_data.dig_y2 = trim_x2y2[1];
- trim_data.dig_z1 = read_16_bit(trim_xy1xy2[2], trim_xy1xy2[3]);
- trim_data.dig_z2 = read_16_bit(trim_xy1xy2[0], trim_xy1xy2[1]);
- trim_data.dig_z3 = read_16_bit(trim_xy1xy2[6], trim_xy1xy2[7]);
- trim_data.dig_z4 = read_16_bit(trim_xyz_data[0], trim_xyz_data[1]);
+ trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped
+ trim_data.dig_xy2 = trim_xy1xy2[0];
- trim_data.dig_xy1 = trim_xy1xy2[9];
- trim_data.dig_xy2 = (int8_t)trim_xy1xy2[8];
+ trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]);
+ trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]);
+ trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]);
+ trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]);
- trim_data.dig_xyz1 = read_16_bit(trim_xy1xy2[4], trim_xy1xy2[5] & 0x7f);
+ trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f);
+ assert(trim_data.dig_xyz1 != 0);
- // TODO: perform self-test
+ perform_self_test();
- // 9 REPXY and 15 REPZ for 100 Hz
- // 3 REPXY and 3 REPZ for > 300 Hz
- ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (3 - 1) / 2);
+ // f_max = 1 / (145us * nXY + 500us * NZ + 980us)
+ // Chose NXY = 7, NZ = 12, which gives 125 Hz,
+ // and has the same ratio as the high accuracy preset
+ ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2);
if (ret < 0){
goto fail;
}
- ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 3 - 1);
+ ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1);
if (ret < 0){
goto fail;
}
+
return 0;
fail:
return ret;
}
+bool BMX055_Magn::perform_self_test(){
+ uint8_t buffer[8];
+ int16_t x, y;
+ int16_t neg_z, pos_z;
+
+ // Increase z reps for less false positives (~30 Hz ODR)
+ set_register(BMX055_MAGN_I2C_REG_REPXY, 1);
+ set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1);
+
+ // Clean existing measurement
+ read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+
+ uint8_t forced = BMX055_MAGN_FORCED;
+
+ // Negative current
+ set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
+ util::sleep_for(100);
+
+ read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+ parse_xyz(buffer, &x, &y, &neg_z);
+
+ // Positive current
+ set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
+ util::sleep_for(100);
+
+ read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+ parse_xyz(buffer, &x, &y, &pos_z);
+
+ // Put back in normal mode
+ set_register(BMX055_MAGN_I2C_REG_MAG, 0);
+
+ int16_t diff = pos_z - neg_z;
+ bool passed = (diff > 180) && (diff < 240);
+
+ if (!passed){
+ LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff);
+ }
+
+ return passed;
+}
+
+bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z){
+ bool ready = buffer[6] & 0x1;
+ if (ready){
+ int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3;
+ int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3;
+ int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1;
+ uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
+ assert(data_r != 0);
+
+ *x = compensate_x(trim_data, mdata_x, data_r);
+ *y = compensate_y(trim_data, mdata_y, data_r);
+ *z = compensate_z(trim_data, mdata_z, data_r);
+ }
+ return ready;
+}
+
void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){
uint64_t start_time = nanos_since_boot();
uint8_t buffer[8];
+ int16_t x, y, z;
int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
- bool ready = buffer[6] & 0x1;
- if (ready){
- int16_t x = parse_xy(buffer[0], buffer[1]);
- int16_t y = parse_xy(buffer[2], buffer[3]);
- int16_t z = parse_z(buffer[4], buffer[5]);
- int16_t rhall = parse_rhall(buffer[5], buffer[6]);
-
- x = compensate_x(trim_data, x, rhall);
- y = compensate_y(trim_data, y, rhall);
- z = compensate_z(trim_data, z, rhall);
-
- // TODO: convert to micro tesla:
- // https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c#L1614
-
+ if (parse_xyz(buffer, &x, &y, &z)){
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
event.setTimestamp(start_time);
+ // Axis convention
+ x = -x;
+ y = -y;
+
float xyz[] = {(float)x, (float)y, (float)z};
auto svec = event.initMagneticUncalibrated();
svec.setV(xyz);
diff --git a/selfdrive/sensord/sensors/bmx055_magn.hpp b/selfdrive/sensord/sensors/bmx055_magn.hpp
index 51fdd727..0e11934e 100644
--- a/selfdrive/sensord/sensors/bmx055_magn.hpp
+++ b/selfdrive/sensord/sensors/bmx055_magn.hpp
@@ -1,4 +1,5 @@
#pragma once
+#include
#include "sensors/i2c_sensor.hpp"
@@ -14,9 +15,22 @@
#define BMX055_MAGN_I2C_REG_REPXY 0x51
#define BMX055_MAGN_I2C_REG_REPZ 0x52
-#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D
-#define BMX055_MAGN_I2C_REG_DIG_Z4 0x62
-#define BMX055_MAGN_I2C_REG_DIG_Z2 0x68
+#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D
+#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E
+#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62
+#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63
+#define BMX055_MAGN_I2C_REG_DIG_X2 0x64
+#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65
+#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68
+#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69
+#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A
+#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B
+#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C
+#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D
+#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E
+#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F
+#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70
+#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71
// Constants
#define BMX055_MAGN_CHIP_ID 0x32
@@ -40,6 +54,8 @@ struct trim_data_t {
class BMX055_Magn : public I2CSensor{
uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;}
trim_data_t trim_data = {0};
+ bool perform_self_test();
+ bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z);
public:
BMX055_Magn(I2CBus *bus);
int init();
diff --git a/selfdrive/swaglog.py b/selfdrive/swaglog.py
index 48447527..08122b0c 100644
--- a/selfdrive/swaglog.py
+++ b/selfdrive/swaglog.py
@@ -1,19 +1,72 @@
-import os
import logging
+import os
+import time
+from pathlib import Path
+from logging.handlers import BaseRotatingHandler
-from logentries import LogentriesHandler
import zmq
-from common.logging_extra import SwagLogger, SwagFormatter
+from common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
+from selfdrive.hardware import PC
+if PC:
+ SWAGLOG_DIR = os.path.join(str(Path.home()), ".comma", "log")
+else:
+ SWAGLOG_DIR = "/data/log/"
-def get_le_handler():
- # setup logentries. we forward log messages to it
- le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17"
- return LogentriesHandler(le_token, use_tls=False, verbose=False)
+def get_file_handler():
+ Path(SWAGLOG_DIR).mkdir(parents=True, exist_ok=True)
+ base_filename = os.path.join(SWAGLOG_DIR, "swaglog")
+ handler = SwaglogRotatingFileHandler(base_filename)
+ return handler
+class SwaglogRotatingFileHandler(BaseRotatingHandler):
+ def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
+ super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
+ self.base_filename = base_filename
+ self.interval = interval # seconds
+ self.max_bytes = max_bytes
+ self.backup_count = backup_count
+ self.log_files = self.get_existing_logfiles()
+ log_indexes = [f.split(".")[-1] for f in self.log_files]
+ self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
+ self.last_rollover = None
+ self.doRollover()
-class LogMessageHandler(logging.Handler):
+ def _open(self):
+ self.last_rollover = time.monotonic()
+ self.last_file_idx += 1
+ next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
+ stream = open(next_filename, self.mode, encoding=self.encoding)
+ self.log_files.insert(0, next_filename)
+ return stream
+
+ def get_existing_logfiles(self):
+ log_files = list()
+ base_dir = os.path.dirname(self.base_filename)
+ for fn in os.listdir(base_dir):
+ fp = os.path.join(base_dir, fn)
+ if fp.startswith(self.base_filename) and os.path.isfile(fp):
+ log_files.append(fp)
+ return sorted(log_files)
+
+ def shouldRollover(self, record):
+ size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
+ time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
+ return size_exceeded or time_exceeded
+
+ def doRollover(self):
+ if self.stream:
+ self.stream.close()
+ self.stream = self._open()
+
+ if self.backup_count > 0:
+ while len(self.log_files) > self.backup_count:
+ to_delete = self.log_files.pop()
+ if os.path.exists(to_delete): # just being safe, should always exist
+ os.remove(to_delete)
+
+class UnixDomainSocketHandler(logging.Handler):
def __init__(self, formatter):
logging.Handler.__init__(self)
self.setFormatter(formatter)
@@ -40,11 +93,13 @@ class LogMessageHandler(logging.Handler):
pass
-def add_logentries_handler(log):
- """Function to add the logentries handler to swaglog.
- This can be used to send logs when logmessaged is not running."""
- handler = get_le_handler()
- handler.setFormatter(SwagFormatter(log))
+def add_file_handler(log):
+ """
+ Function to add the file log handler to swaglog.
+ This can be used to store logs when logmessaged is not running.
+ """
+ handler = get_file_handler()
+ handler.setFormatter(SwagLogFileFormatter(log))
log.addHandler(handler)
@@ -53,4 +108,5 @@ log.setLevel(logging.DEBUG)
outhandler = logging.StreamHandler()
log.addHandler(outhandler)
-log.addHandler(LogMessageHandler(SwagFormatter(log)))
+# logs are sent through IPC before writing to disk to prevent disk I/O blocking
+log.addHandler(UnixDomainSocketHandler(SwagFormatter(log)))
diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py
index de8b15b3..fda1a317 100644
--- a/selfdrive/test/helpers.py
+++ b/selfdrive/test/helpers.py
@@ -1,12 +1,11 @@
import time
-import subprocess
from functools import wraps
from nose.tools import nottest
-from selfdrive.hardware.eon.apk import update_apks, start_offroad, pm_apply_packages, android_packages
from selfdrive.hardware import PC
from selfdrive.version import training_version, terms_version
-from selfdrive.manager import start_managed_process, kill_managed_process, get_running
+from selfdrive.manager.process_config import managed_processes
+
def set_params_enabled():
from common.params import Params
@@ -18,54 +17,33 @@ def set_params_enabled():
params.put("Passive", "0")
params.put("CompletedTrainingVersion", training_version)
+
def phone_only(x):
if PC:
return nottest(x)
else:
return x
+
def with_processes(processes, init_time=0):
def wrapper(func):
@wraps(func)
def wrap(*args, **kwargs):
# start and assert started
for n, p in enumerate(processes):
- start_managed_process(p)
- if n < len(processes)-1:
+ managed_processes[p].start()
+ if n < len(processes) - 1:
time.sleep(init_time)
- assert all(get_running()[name].exitcode is None for name in processes)
+ assert all(managed_processes[name].proc.exitcode is None for name in processes)
# call the function
try:
func(*args, **kwargs)
# assert processes are still started
- assert all(get_running()[name].exitcode is None for name in processes)
+ assert all(managed_processes[name].proc.exitcode is None for name in processes)
finally:
- # kill and assert all stopped
for p in processes:
- kill_managed_process(p)
- assert len(get_running()) == 0
- return wrap
- return wrapper
-
-def with_apks():
- def wrapper(func):
- @wraps(func)
- def wrap():
- update_apks()
- pm_apply_packages('enable')
- start_offroad()
-
- func()
-
- try:
- for package in android_packages:
- apk_is_running = (subprocess.call(["pidof", package]) == 0)
- assert apk_is_running, package
- finally:
- pm_apply_packages('disable')
- for package in android_packages:
- apk_is_not_running = (subprocess.call(["pidof", package]) == 1)
- assert apk_is_not_running, package
+ managed_processes[p].stop()
+
return wrap
return wrapper
diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh
index b605a8e8..435a6af4 100755
--- a/selfdrive/test/setup_device_ci.sh
+++ b/selfdrive/test/setup_device_ci.sh
@@ -12,8 +12,18 @@ if [ -z "$TEST_DIR" ]; then
exit 1
fi
+# setup jenkins device
if [ ! -d "$SOURCE_DIR" ]; then
- git clone https://github.com/commaai/openpilot.git "$SOURCE_DIR"
+ # write continue.sh
+ CONTINUE_FILE="/data/data/com.termux/files/continue.sh"
+ echo "#!/usr/bin/bash" > $CONTINUE_FILE
+ echo "wpa_cli IFNAME=wlan0 SCAN" >> $CONTINUE_FILE
+ echo "sleep infinity" >> $CONTINUE_FILE
+
+ # write SSH keys
+ curl "https://github.com/commaci2.keys" > /data/params/d/GithubSshKeys
+
+ git clone --depth 1 https://github.com/commaai/openpilot.git "$SOURCE_DIR"
fi
# clear scons cache dirs that haven't been written to in one day
diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py
index c790f8d6..485ebd2d 100644
--- a/selfdrive/thermald/power_monitoring.py
+++ b/selfdrive/thermald/power_monitoring.py
@@ -169,13 +169,13 @@ class PowerMonitoring:
return disable_charging
# See if we need to shutdown
- def should_shutdown(self, pandaState, offroad_timestamp, started_seen, LEON):
+ def should_shutdown(self, pandaState, offroad_timestamp, started_seen):
if pandaState is None or offroad_timestamp is None:
return False
now = sec_since_boot()
panda_charging = (pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client)
- BATT_PERC_OFF = 10 if LEON else 3
+ BATT_PERC_OFF = 10
should_shutdown = False
# Wait until we have shut down charging before powering down
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index 23f8602b..9f769801 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -2,6 +2,7 @@
import datetime
import os
import time
+from pathlib import Path
from typing import Dict, Optional, Tuple
import psutil
@@ -13,8 +14,9 @@ from common.filter_simple import FirstOrderFilter
from common.numpy_fast import clip, interp
from common.params import Params
from common.realtime import DT_TRML, sec_since_boot
+from common.dict_helpers import strip_deprecated_keys
from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from selfdrive.hardware import EON, HARDWARE
+from selfdrive.hardware import EON, TICI, HARDWARE
from selfdrive.loggerd.config import get_available_percent
from selfdrive.pandad import get_expected_signature
from selfdrive.swaglog import cloudlog
@@ -23,6 +25,8 @@ from selfdrive.version import get_git_branch, terms_version, training_version
FW_SIGNATURE = get_expected_signature()
+DISABLE_LTE_ONROAD = os.path.exists("/persist/disable_lte_onroad") or TICI
+
ThermalStatus = log.DeviceState.ThermalStatus
NetworkType = log.DeviceState.NetworkType
NetworkStrength = log.DeviceState.NetworkStrength
@@ -34,9 +38,9 @@ DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect
prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {}
-LEON = False
last_eon_fan_val = None
+
def read_tz(x):
if x is None:
return 0
@@ -59,44 +63,24 @@ def read_thermal(thermal_config):
def setup_eon_fan():
- global LEON
-
os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
- bus = SMBus(7, force=True)
- try:
- bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts
- bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable
- bus.write_byte_data(0x21, 0x02, 0x2) # needed?
- bus.write_byte_data(0x21, 0x04, 0x4) # manual override source
- except IOError:
- print("LEON detected")
- LEON = True
- bus.close()
-
def set_eon_fan(val):
- global LEON, last_eon_fan_val
+ global last_eon_fan_val
if last_eon_fan_val is None or last_eon_fan_val != val:
bus = SMBus(7, force=True)
- if LEON:
- try:
- i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
- bus.write_i2c_block_data(0x3d, 0, [i])
- except IOError:
- # tusb320
- if val == 0:
- bus.write_i2c_block_data(0x67, 0xa, [0])
- #bus.write_i2c_block_data(0x67, 0x45, [1<<2])
- else:
- #bus.write_i2c_block_data(0x67, 0x45, [0])
- bus.write_i2c_block_data(0x67, 0xa, [0x20])
- bus.write_i2c_block_data(0x67, 0x8, [(val - 1) << 6])
- else:
- bus.write_byte_data(0x21, 0x04, 0x2)
- bus.write_byte_data(0x21, 0x03, (val*2)+1)
- bus.write_byte_data(0x21, 0x04, 0x4)
+ try:
+ i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
+ bus.write_i2c_block_data(0x3d, 0, [i])
+ except IOError:
+ # tusb320
+ if val == 0:
+ bus.write_i2c_block_data(0x67, 0xa, [0])
+ else:
+ bus.write_i2c_block_data(0x67, 0xa, [0x20])
+ bus.write_i2c_block_data(0x67, 0x8, [(val - 1) << 6])
bus.close()
last_eon_fan_val = val
@@ -154,6 +138,7 @@ def thermald_thread():
pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency
pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout)
location_sock = messaging.sub_sock('gpsLocationExternal')
+ managerState_sock = messaging.sub_sock('managerState', conflate=True)
fan_speed = 0
count = 0
@@ -179,6 +164,7 @@ def thermald_thread():
should_start_prev = False
handle_fan = None
is_uno = False
+ ui_running_prev = False
params = Params()
power_monitor = PowerMonitoring()
@@ -186,6 +172,19 @@ def thermald_thread():
thermal_config = HARDWARE.get_thermal_config()
+ # CPR3 logging
+ if EON:
+ base_path = "/sys/kernel/debug/cpr3-regulator/"
+ cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()]
+ cpr_data = {}
+ for cf in cpr_files:
+ with open(cf, "r") as f:
+ try:
+ cpr_data[str(cf)] = f.read().strip()
+ except Exception:
+ pass
+ cloudlog.event("CPR", data=cpr_data)
+
while 1:
pandaState = messaging.recv_sock(pandaState_sock, wait=True)
msg = read_thermal(thermal_config)
@@ -204,6 +203,10 @@ def thermald_thread():
no_panda_cnt = 0
startup_conditions["ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
+ startup_conditions["hardware_supported"] = pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda,
+ log.PandaState.PandaType.greyPanda]
+ set_offroad_alert_if_changed("Offroad_HardwareUnsupported", not startup_conditions["hardware_supported"])
+
# Setup fan handler on first connect to panda
if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
@@ -343,15 +346,13 @@ def thermald_thread():
startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"]))
- startup_conditions["hardware_supported"] = pandaState is not None and pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda,
- log.PandaState.PandaType.greyPanda]
- set_offroad_alert_if_changed("Offroad_HardwareUnsupported", pandaState is not None and not startup_conditions["hardware_supported"])
-
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
+ if TICI and DISABLE_LTE_ONROAD:
+ os.system("sudo systemctl stop --no-block lte")
off_ts = None
if started_ts is None:
@@ -363,6 +364,8 @@ def thermald_thread():
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
+ if TICI and DISABLE_LTE_ONROAD:
+ os.system("sudo systemctl start --no-block lte")
started_ts = None
if off_ts is None:
@@ -377,12 +380,20 @@ def thermald_thread():
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts)
# Check if we need to shut down
- if power_monitor.should_shutdown(pandaState, off_ts, started_seen, LEON):
+ if power_monitor.should_shutdown(pandaState, off_ts, started_seen):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
HARDWARE.shutdown()
+ # If UI has crashed, set the brightness to reasonable non-zero value
+ manager_state = messaging.recv_one_or_none(managerState_sock)
+ if manager_state is not None:
+ ui_running = "ui" in (p.name for p in manager_state.managerState.processes if p.running)
+ if ui_running_prev and not ui_running:
+ HARDWARE.set_screen_brightness(20)
+ ui_running_prev = ui_running
+
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
@@ -390,7 +401,8 @@ def thermald_thread():
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
- set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
+ if EON and not is_uno:
+ set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
@@ -400,9 +412,9 @@ def thermald_thread():
location = messaging.recv_sock(location_sock)
cloudlog.event("STATUS_PACKET",
count=count,
- pandaState=(pandaState.to_dict() if pandaState else None),
- location=(location.gpsLocationExternal.to_dict() if location else None),
- deviceState=msg.to_dict())
+ pandaState=(strip_deprecated_keys(pandaState.to_dict()) if pandaState else None),
+ location=(strip_deprecated_keys(location.gpsLocationExternal.to_dict()) if location else None),
+ deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py
index e9ec73b7..404d7141 100755
--- a/selfdrive/tombstoned.py
+++ b/selfdrive/tombstoned.py
@@ -93,6 +93,12 @@ def report_tombstone_android(fn, client):
if name_idx >= 0:
message = message[name_idx:]
+ executable = ""
+ start_exe_idx = message.find('>>> ')
+ end_exe_idx = message.find(' <<<')
+ if start_exe_idx >= 0 and end_exe_idx >= 0:
+ executable = message[start_exe_idx + 4:end_exe_idx]
+
# Cut off fault addr
fault_idx = message.find(', fault addr')
if fault_idx >= 0:
@@ -100,6 +106,17 @@ def report_tombstone_android(fn, client):
sentry_report(client, fn, message, contents)
+ # Copy crashlog to upload folder
+ clean_path = executable.replace('./', '').replace('/', '_')
+ date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S")
+
+ new_fn = f"{date}_{commit[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
+
+ crashlog_dir = os.path.join(ROOT, "crash")
+ mkdirs_exists_ok(crashlog_dir)
+
+ shutil.copy(fn, os.path.join(crashlog_dir, new_fn))
+
def report_tombstone_apport(fn, client):
f_size = os.path.getsize(fn)
diff --git a/selfdrive/ui/.gitignore b/selfdrive/ui/.gitignore
new file mode 100644
index 00000000..63f85bac
--- /dev/null
+++ b/selfdrive/ui/.gitignore
@@ -0,0 +1,9 @@
+moc_*
+*.moc
+
+qt/text
+qt/spinner
+qt/setup/setup
+qt/setup/reset
+qt/setup/wifi
+qt/setup/installer*
diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript
index 0dd5d56e..ebef246a 100644
--- a/selfdrive/ui/SConscript
+++ b/selfdrive/ui/SConscript
@@ -2,58 +2,58 @@ import os
Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc',
'cereal', 'transformations')
-
src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c']
-libs = [gpucommon, common, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', cereal, messaging, visionipc, transformations]
+libs = [gpucommon, common, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto',
+ cereal, messaging, visionipc, transformations]
-if qt_env is None:
- libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware',
- 'ui', 'CB', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid', 'OpenCL']
- linkflags = ['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib']
+if arch == 'aarch64':
+ libs += ['log', 'utils', 'gui', 'ui', 'CB', 'gsl', 'adreno_utils',
+ 'cutils', 'uuid']
- src += ["android/ui.cc", "android/sl_sound.cc"]
- env.Program('_ui', src,
- LINKFLAGS=linkflags,
- LIBS=libs)
-else:
- qt_libs = qt_env["LIBS"] + libs + ["pthread", "ssl", "crypto"]
+qt_base_libs = qt_env["LIBS"] + libs + ["pthread"]
+if arch == "Darwin":
+ del qt_base_libs[qt_base_libs.index('OpenCL')]
+ qt_env['FRAMEWORKS'] += ['OpenCL']
- widgets = qt_env.Library("qt_widgets",
- ["qt/qt_sound.cc", "qt/widgets/keyboard.cc", "qt/widgets/input_field.cc", "qt/widgets/drive_stats.cc", "qt/widgets/ssh_keys.cc",
- "qt/offroad/networking.cc", "qt/offroad/wifiManager.cc", "qt/widgets/toggle.cc", "qt/widgets/offroad_alerts.cc", "qt/widgets/setup.cc"],
- LIBS=qt_libs)
- qt_libs.append(widgets)
+widgets_src = ["qt/widgets/input.cc", "qt/widgets/drive_stats.cc",
+ "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", "qt/sound.cc",
+ "qt/widgets/offroad_alerts.cc", "qt/widgets/setup.cc", "qt/widgets/keyboard.cc",
+ "#phonelibs/qrcode/QrCode.cc"]
+if arch != 'aarch64':
+ widgets_src += ["qt/offroad/networking.cc", "qt/offroad/wifiManager.cc"]
+widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=qt_base_libs)
+qt_libs = qt_base_libs + [widgets]
- if arch == "Darwin":
- # fix OpenCL
- del qt_libs[qt_libs.index('OpenCL')]
- qt_env['FRAMEWORKS'] += ['OpenCL']
+# spinner and text window
+qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_base_libs)
+qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_base_libs)
- qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/api.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc", "#phonelibs/qrcode/QrCode.cc"] + src
- qt_env.Program("_ui", qt_src, LIBS=qt_libs)
+# build main UI
+qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/api.cc", "qt/offroad/settings.cc",
+ "qt/offroad/onboarding.cc"] + src
+qt_env.Program("_ui", qt_src, LIBS=qt_libs)
- # spinner and text window
- qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs)
- qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs)
+# setup, factory resetter, and installer
+if arch != 'aarch64' and "BUILD_SETUP" in os.environ:
+ qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs)
+ qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc"], LIBS=qt_libs + ['curl', 'common', 'json11'])
+ qt_env.Program("qt/setup/wifi", ["qt/setup/wifi.cc"], LIBS=qt_libs + ['common', 'json11'])
- # build setup, factory resetter, and installer
- if "BUILD_SETUP" in os.environ:
- qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs)
- qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc"], LIBS=qt_libs + ['curl', 'common', 'json11'])
+ installers = [
+ ("openpilot", "master"),
+ #("openpilot_test", "release3-staging"),
+ #("openpilot_internal", "master"),
+ #("dashcam", "dashcam3-staging"),
+ #("dashcam_test", "dashcam3-staging"),
+ ]
+ for name, branch in installers:
+ d = {'BRANCH': f"'\"{branch}\"'"}
+ if "internal" in name:
+ d['INTERNAL'] = "1"
- installers = [
- ("openpilot", "master"),
- #("openpilot_test", "release3-staging"),
- #("openpilot_internal", "master"),
- #("dashcam", "dashcam3-staging"),
- #("dashcam_test", "dashcam3-staging"),
- ]
- for name, branch in installers:
- d = {'BRANCH': f"'\"{branch}\"'"}
- if "internal" in name:
- import requests
- r = requests.get("https://github.com/commaci2.keys")
- r.raise_for_status()
- d['SSH_KEYS'] = f'\\"{r.text.strip()}\\"'
- qt_env.Program(f"qt/setup/installer_{name}", ["qt/setup/installer.cc"], LIBS=qt_libs, CPPDEFINES=d)
+ import requests
+ r = requests.get("https://github.com/commaci2.keys")
+ r.raise_for_status()
+ d['SSH_KEYS'] = f'\\"{r.text.strip()}\\"'
+ qt_env.Program(f"qt/setup/installer_{name}", ["qt/setup/installer.cc"], LIBS=qt_libs, CPPDEFINES=d)
diff --git a/selfdrive/ui/android/sl_sound.cc b/selfdrive/ui/android/sl_sound.cc
deleted file mode 100644
index 52604df9..00000000
--- a/selfdrive/ui/android/sl_sound.cc
+++ /dev/null
@@ -1,125 +0,0 @@
-#include
-#include
-#include
-#include "common/swaglog.h"
-#include "common/timing.h"
-
-#include "android/sl_sound.hpp"
-
-#define LogOnError(func, msg) \
- if ((func) != SL_RESULT_SUCCESS) { LOGW(msg); }
-
-#define ReturnOnError(func, msg) \
- if ((func) != SL_RESULT_SUCCESS) { LOGW(msg); return false; }
-
-struct SLSound::Player {
- SLObjectItf player;
- SLPlayItf playItf;
- std::atomic repeat;
-};
-
-SLSound::SLSound() {
- if (!init()){
- throw std::runtime_error("Failed to initialize sound");
- }
-}
-
-bool SLSound::init() {
- SLEngineOption engineOptions[] = {{SL_ENGINEOPTION_THREADSAFE, SL_BOOLEAN_TRUE}};
- const SLInterfaceID ids[1] = {SL_IID_VOLUME};
- const SLboolean req[1] = {SL_BOOLEAN_FALSE};
- SLEngineItf engineInterface = NULL;
- ReturnOnError(slCreateEngine(&engine_, 1, engineOptions, 0, NULL, NULL), "Failed to create OpenSL engine");
- ReturnOnError((*engine_)->Realize(engine_, SL_BOOLEAN_FALSE), "Failed to realize OpenSL engine");
- ReturnOnError((*engine_)->GetInterface(engine_, SL_IID_ENGINE, &engineInterface), "Failed to get OpenSL engine interface");
- ReturnOnError((*engineInterface)->CreateOutputMix(engineInterface, &outputMix_, 1, ids, req), "Failed to create output mix");
- ReturnOnError((*outputMix_)->Realize(outputMix_, SL_BOOLEAN_FALSE), "Failed to realize output mix");
-
- for (auto &kv : sound_map) {
- SLDataLocator_URI locUri = {SL_DATALOCATOR_URI, (SLchar *)kv.second.first};
- SLDataFormat_MIME formatMime = {SL_DATAFORMAT_MIME, NULL, SL_CONTAINERTYPE_UNSPECIFIED};
- SLDataSource audioSrc = {&locUri, &formatMime};
- SLDataLocator_OutputMix outMix = {SL_DATALOCATOR_OUTPUTMIX, outputMix_};
- SLDataSink audioSnk = {&outMix, NULL};
-
- SLObjectItf player = NULL;
- SLPlayItf playItf = NULL;
- ReturnOnError((*engineInterface)->CreateAudioPlayer(engineInterface, &player, &audioSrc, &audioSnk, 0, NULL, NULL), "Failed to create audio player");
- ReturnOnError((*player)->Realize(player, SL_BOOLEAN_FALSE), "Failed to realize audio player");
- ReturnOnError((*player)->GetInterface(player, SL_IID_PLAY, &playItf), "Failed to get player interface");
- ReturnOnError((*playItf)->SetPlayState(playItf, SL_PLAYSTATE_PAUSED), "Failed to initialize playstate to SL_PLAYSTATE_PAUSED");
-
- player_[kv.first] = new SLSound::Player{player, playItf};
- }
- return true;
-}
-
-void SLAPIENTRY slplay_callback(SLPlayItf playItf, void *context, SLuint32 event) {
- SLSound::Player *s = reinterpret_cast(context);
- if (event == SL_PLAYEVENT_HEADATEND && s->repeat != 0) {
- if (s->repeat > 0) --s->repeat;
- (*playItf)->SetPlayState(playItf, SL_PLAYSTATE_STOPPED);
- (*playItf)->SetMarkerPosition(playItf, 0);
- (*playItf)->SetPlayState(playItf, SL_PLAYSTATE_PLAYING);
- }
-}
-
-bool SLSound::play(AudibleAlert alert) {
- if (currentSound_ != AudibleAlert::NONE) {
- stop();
- }
-
- auto player = player_.at(alert);
- SLPlayItf playItf = player->playItf;
-
- int loops = sound_map[alert].second;
- player->repeat = loops > 0 ? loops - 1 : loops;
- if (player->repeat != 0) {
- ReturnOnError((*playItf)->RegisterCallback(playItf, slplay_callback, player), "Failed to register callback");
- ReturnOnError((*playItf)->SetCallbackEventsMask(playItf, SL_PLAYEVENT_HEADATEND), "Failed to set callback event mask");
- }
-
- // Reset the audio player
- ReturnOnError((*playItf)->ClearMarkerPosition(playItf), "Failed to clear marker position");
- uint32_t states[] = {SL_PLAYSTATE_PAUSED, SL_PLAYSTATE_STOPPED, SL_PLAYSTATE_PLAYING};
- for (auto state : states) {
- ReturnOnError((*playItf)->SetPlayState(playItf, state), "Failed to set SL_PLAYSTATE_PLAYING");
- }
- currentSound_ = alert;
- return true;
-}
-
-void SLSound::stop() {
- if (currentSound_ != AudibleAlert::NONE) {
- auto player = player_.at(currentSound_);
- player->repeat = 0;
- LogOnError((*(player->playItf))->SetPlayState(player->playItf, SL_PLAYSTATE_PAUSED), "Failed to set SL_PLAYSTATE_PAUSED");
- currentSound_ = AudibleAlert::NONE;
- }
-}
-
-void SLSound::setVolume(int volume) {
- if (last_volume_ == volume) return;
-
- double current_time = nanos_since_boot();
- if ((current_time - last_set_volume_time_) > (5 * (1e+9))) { // 5s timeout on updating the volume
- char volume_change_cmd[64];
- snprintf(volume_change_cmd, sizeof(volume_change_cmd), "service call audio 3 i32 3 i32 %d i32 1 &", volume);
- system(volume_change_cmd);
- last_volume_ = volume;
- last_set_volume_time_ = current_time;
- }
-}
-
-SLSound::~SLSound() {
- for (auto &kv : player_) {
- (*(kv.second->player))->Destroy(kv.second->player);
- delete kv.second;
- }
- if (outputMix_) {
- (*outputMix_)->Destroy(outputMix_);
- }
- if (engine_) {
- (*engine_)->Destroy(engine_);
- }
-}
diff --git a/selfdrive/ui/android/sl_sound.hpp b/selfdrive/ui/android/sl_sound.hpp
deleted file mode 100644
index 7925277b..00000000
--- a/selfdrive/ui/android/sl_sound.hpp
+++ /dev/null
@@ -1,26 +0,0 @@
-#pragma once
-#include
-#include
-
-#include "sound.hpp"
-
-
-class SLSound : public Sound {
-public:
- SLSound();
- ~SLSound();
- bool play(AudibleAlert alert);
- void stop();
- void setVolume(int volume);
-
-private:
- bool init();
- SLObjectItf engine_ = nullptr;
- SLObjectItf outputMix_ = nullptr;
- int last_volume_ = 0;
- double last_set_volume_time_ = 0.;
- AudibleAlert currentSound_ = AudibleAlert::NONE;
- struct Player;
- std::map player_;
- friend void SLAPIENTRY slplay_callback(SLPlayItf playItf, void *context, SLuint32 event);
-};
diff --git a/selfdrive/ui/android/spinner/Makefile b/selfdrive/ui/android/spinner/Makefile
deleted file mode 100644
index 7ceea5f6..00000000
--- a/selfdrive/ui/android/spinner/Makefile
+++ /dev/null
@@ -1,77 +0,0 @@
-CC = clang
-CXX = clang++
-
-ROOT_DIR = ../../..
-PHONELIBS = $(ROOT_DIR)/phonelibs
-COMMON = $(ROOT)/selfdrive/common
-
-WARN_FLAGS = -Werror=implicit-function-declaration \
- -Werror=incompatible-pointer-types \
- -Werror=int-conversion \
- -Werror=return-type \
- -Werror=format-extra-args
-
-CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
-CXXFLAGS = -std=c++1z -fPIC -O2 $(WARN_FLAGS)
-
-NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
-
-OPENGL_LIBS = -lGLESv3
-
-FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
-
-OBJS = spinner.o \
- $(COMMON)/framebuffer.o \
- $(COMMON)/util.o \
- $(PHONELIBS)/nanovg/nanovg.o \
- $(COMMON)/spinner.o \
- opensans_semibold.o \
- img_spinner_track.o \
- img_spinner_comma.o
-
-DEPS := $(OBJS:.o=.d)
-
-.PHONY: all
-all: spinner
-
-spinner: $(OBJS)
- @echo "[ LINK ] $@"
- $(CXX) -fPIC -o '$@' $^ \
- -s \
- $(FRAMEBUFFER_LIBS) \
- -L/system/vendor/lib64 \
- $(OPENGL_LIBS) \
- -lm -llog
-
-$(COMMON)/framebuffer.o: $(COMMON)/framebuffer.cc
- @echo "[ CXX ] $@"
- $(CXX) $(CXXFLAGS) -MMD \
- -I$(PHONELIBS)/android_frameworks_native/include \
- -I$(PHONELIBS)/android_system_core/include \
- -I$(PHONELIBS)/android_hardware_libhardware/include \
- -c -o '$@' '$<'
-
-opensans_semibold.o: $(ROOT_DIR)/selfdrive/assets/fonts/opensans_semibold.ttf
- @echo "[ bin2o ] $@"
- cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
-
-img_spinner_track.o: $(ROOT_DIR)/selfdrive/assets/img_spinner_track.png
- @echo "[ bin2o ] $@"
- cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
-
-img_spinner_comma.o: $(ROOT_DIR)/selfdrive/assets/img_spinner_comma.png
- @echo "[ bin2o ] $@"
- cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
-
-%.o: %.c
- @echo "[ CC ] $@"
- $(CC) $(CFLAGS) -MMD \
- -I../.. \
- $(NANOVG_FLAGS) \
- -c -o '$@' '$<'
-
-.PHONY: clean
-clean:
- rm -f spinner $(OBJS) $(DEPS)
-
--include $(DEPS)
diff --git a/selfdrive/ui/android/spinner/spinner b/selfdrive/ui/android/spinner/spinner
deleted file mode 100755
index 2acc7e78..00000000
Binary files a/selfdrive/ui/android/spinner/spinner and /dev/null differ
diff --git a/selfdrive/ui/android/spinner/spinner.c b/selfdrive/ui/android/spinner/spinner.c
deleted file mode 100644
index 33ebf7cd..00000000
--- a/selfdrive/ui/android/spinner/spinner.c
+++ /dev/null
@@ -1,182 +0,0 @@
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-
-#include "nanovg.h"
-#define NANOVG_GLES3_IMPLEMENTATION
-#include "nanovg_gl.h"
-#include "nanovg_gl_utils.h"
-
-#include "framebuffer.h"
-#include "spinner.h"
-
-#define SPINTEXT_LENGTH 128
-
-// external resources linked in
-extern const unsigned char _binary_opensans_semibold_ttf_start[];
-extern const unsigned char _binary_opensans_semibold_ttf_end[];
-
-extern const unsigned char _binary_img_spinner_track_png_start[];
-extern const unsigned char _binary_img_spinner_track_png_end[];
-
-extern const unsigned char _binary_img_spinner_comma_png_start[];
-extern const unsigned char _binary_img_spinner_comma_png_end[];
-
-bool stdin_input_available() {
- struct timeval timeout;
- timeout.tv_sec = 0;
- timeout.tv_usec = 0;
-
- fd_set fds;
- FD_ZERO(&fds);
- FD_SET(STDIN_FILENO, &fds);
- select(STDIN_FILENO+1, &fds, NULL, NULL, &timeout);
- return (FD_ISSET(0, &fds));
-}
-
-int main(int argc, char** argv) {
-
- bool draw_progress = false;
- float progress_val = 0.0;
-
- char spintext[SPINTEXT_LENGTH];
- spintext[0] = 0;
-
- const char* spintext_arg = NULL;
- if (argc >= 2) {
- strncpy(spintext, argv[1], SPINTEXT_LENGTH);
- }
-
- // spinner
- int fb_w, fb_h;
- FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
- &fb_w, &fb_h);
- assert(fb);
- framebuffer_set_power(fb, HWC_POWER_MODE_NORMAL);
-
- NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
- assert(vg);
-
- int font = nvgCreateFontMem(vg, "Bold", (unsigned char*)_binary_opensans_semibold_ttf_start, _binary_opensans_semibold_ttf_end-_binary_opensans_semibold_ttf_start, 0);
- assert(font >= 0);
-
- int spinner_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_track_png_start, _binary_img_spinner_track_png_end - _binary_img_spinner_track_png_start);
- assert(spinner_img >= 0);
- int spinner_img_s = 360;
- int spinner_img_x = ((fb_w/2)-(spinner_img_s/2));
- int spinner_img_y = 260;
- int spinner_img_xc = (fb_w/2);
- int spinner_img_yc = (fb_h/2)-100;
- int spinner_comma_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_comma_png_start, _binary_img_spinner_comma_png_end - _binary_img_spinner_comma_png_start);
- assert(spinner_comma_img >= 0);
-
- for (int cnt = 0; ; cnt++) {
- // Check stdin for new text
- if (stdin_input_available()){
- fgets(spintext, SPINTEXT_LENGTH, stdin);
- spintext[strcspn(spintext, "\n")] = 0;
-
- // Check if number (update progress bar)
- size_t len = strlen(spintext);
- bool is_number = len > 0;
- for (int i = 0; i < len; i++){
- if (!isdigit(spintext[i])){
- is_number = false;
- break;
- }
- }
-
- if (is_number) {
- progress_val = (float)(atoi(spintext)) / 100.0;
- progress_val = fmin(1.0, progress_val);
- progress_val = fmax(0.0, progress_val);
- }
-
- draw_progress = is_number;
- }
-
- glClearColor(0.1, 0.1, 0.1, 1.0);
- glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
- glEnable(GL_BLEND);
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
- nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
-
- // background
- nvgBeginPath(vg);
- NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
- nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
- nvgFillPaint(vg, bg);
- nvgRect(vg, 0, 0, fb_w, fb_h);
- nvgFill(vg);
-
- // spin track
- nvgSave(vg);
- nvgTranslate(vg, spinner_img_xc, spinner_img_yc);
- nvgRotate(vg, (3.75*M_PI * cnt/120.0));
- nvgTranslate(vg, -spinner_img_xc, -spinner_img_yc);
- NVGpaint spinner_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
- spinner_img_s, spinner_img_s, 0, spinner_img, 0.6f);
- nvgBeginPath(vg);
- nvgFillPaint(vg, spinner_imgPaint);
- nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
- nvgFill(vg);
- nvgRestore(vg);
-
- // comma
- NVGpaint comma_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
- spinner_img_s, spinner_img_s, 0, spinner_comma_img, 1.0f);
- nvgBeginPath(vg);
- nvgFillPaint(vg, comma_imgPaint);
- nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
- nvgFill(vg);
-
- if (draw_progress){
- // draw progress bar
- int progress_width = 1000;
- int progress_x = fb_w/2-progress_width/2;
- int progress_y = 775;
- int progress_height = 25;
-
- NVGpaint paint = nvgBoxGradient(
- vg, progress_x + 1, progress_y + 1,
- progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27));
- nvgBeginPath(vg);
- nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12);
- nvgFillPaint(vg, paint);
- nvgFill(vg);
-
- int bar_pos = ((progress_width - 2) * progress_val);
-
- paint = nvgBoxGradient(
- vg, progress_x, progress_y,
- bar_pos+1.5f, progress_height-1, 3, 4,
- nvgRGB(245, 245, 245), nvgRGB(105, 105, 105));
-
- nvgBeginPath(vg);
- nvgRoundedRect(
- vg, progress_x+1, progress_y+1,
- bar_pos, progress_height-2, 12);
- nvgFillPaint(vg, paint);
- nvgFill(vg);
- } else {
- // message
- nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
- nvgFontSize(vg, 96.0f);
- nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
- }
-
- nvgEndFrame(vg);
- framebuffer_swap(fb);
- assert(glGetError() == GL_NO_ERROR);
- }
-
- return 0;
-}
diff --git a/selfdrive/ui/android/text/Makefile b/selfdrive/ui/android/text/Makefile
deleted file mode 100644
index c83a7a05..00000000
--- a/selfdrive/ui/android/text/Makefile
+++ /dev/null
@@ -1,64 +0,0 @@
-CC = clang
-CXX = clang++
-
-PHONELIBS = ../../../../phonelibs
-COMMON = ../../../common
-
-WARN_FLAGS = -Werror=implicit-function-declaration \
- -Werror=incompatible-pointer-types \
- -Werror=int-conversion \
- -Werror=return-type \
- -Werror=format-extra-args
-
-CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
-CXXFLAGS = -std=c++1z -fPIC -O2 $(WARN_FLAGS)
-
-NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
-
-OPENGL_LIBS = -lGLESv3
-
-FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
-
-OBJS = text.o \
- $(COMMON)/framebuffer.o \
- $(COMMON)/util.o \
- $(COMMON)/touch.o \
- $(PHONELIBS)/nanovg/nanovg.o \
- opensans_regular.o \
-
-DEPS := $(OBJS:.o=.d)
-
-.PHONY: all
-all: text
-
-text: $(OBJS)
- @echo "[ LINK ] $@"
- $(CXX) -fPIC -o '$@' $^ \
- -s \
- $(FRAMEBUFFER_LIBS) \
- -L/system/vendor/lib64 \
- $(OPENGL_LIBS) \
- -lm -llog
-
-opensans_regular.o: ../../../assets/fonts/opensans_regular.ttf
- @echo "[ bin2o ] $@"
- cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
-
-%.o: %.cc
- mkdir -p $(@D)
- @echo "[ CXX ] $@"
- $(CXX) $(CPPFLAGS) $(CXXFLAGS) \
- -I../../../selfdrive \
- -I../../../ \
- -I$(PHONELIBS)/android_frameworks_native/include \
- -I$(PHONELIBS)/android_system_core/include \
- -I$(PHONELIBS)/android_hardware_libhardware/include \
- $(NANOVG_FLAGS) \
- -c -o '$@' '$<'
-
-
-.PHONY: clean
-clean:
- rm -f text $(OBJS) $(DEPS)
-
--include $(DEPS)
diff --git a/selfdrive/ui/android/text/text b/selfdrive/ui/android/text/text
deleted file mode 100755
index fc0a176b..00000000
Binary files a/selfdrive/ui/android/text/text and /dev/null differ
diff --git a/selfdrive/ui/android/text/text.cc b/selfdrive/ui/android/text/text.cc
deleted file mode 100644
index a532ed21..00000000
--- a/selfdrive/ui/android/text/text.cc
+++ /dev/null
@@ -1,133 +0,0 @@
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "nanovg.h"
-#define NANOVG_GLES3_IMPLEMENTATION
-#include "nanovg_gl.h"
-#include "nanovg_gl_utils.h"
-
-#include "common/framebuffer.h"
-#include "common/touch.h"
-
-
-#define COLOR_WHITE nvgRGBA(255, 255, 255, 255)
-#define MAX_TEXT_SIZE 2048
-
-extern const uint8_t bin_opensans_regular[] asm("_binary_opensans_regular_ttf_start");
-extern const uint8_t *bin_opensans_regular_end asm("_binary_opensans_regular_ttf_end");
-
-
-int main(int argc, char** argv) {
- int err;
-
- // spinner
- int fb_w, fb_h;
- std::unique_ptr fb = std::make_unique("text", 0x00001000, false,
- &fb_w, &fb_h);
- assert(fb);
-
- NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
- assert(vg);
-
- int font = nvgCreateFontMem(vg, "regular", (unsigned char*)bin_opensans_regular, (bin_opensans_regular_end - bin_opensans_regular), 0);
- assert(font >= 0);
-
- // Awake
- fb->set_power(HWC_POWER_MODE_NORMAL);
- set_brightness(255);
-
- glClearColor(0.1, 0.1, 0.1, 1.0);
- glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
- glEnable(GL_BLEND);
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
- nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
-
- // background
- nvgBeginPath(vg);
- NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
- nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
- nvgFillPaint(vg, bg);
- nvgRect(vg, 0, 0, fb_w, fb_h);
- nvgFill(vg);
-
-
- // Text
- nvgFillColor(vg, COLOR_WHITE);
- nvgFontSize(vg, 75.0f);
-
- if (argc >= 2) {
- float x = 150;
- float y = 150;
-
- // Copy text
- char * text = (char *)malloc(MAX_TEXT_SIZE);
- strncpy(text, argv[1], MAX_TEXT_SIZE);
-
- float lineh;
- nvgTextMetrics(vg, NULL, NULL, &lineh);
-
- // nvgTextBox strips leading whitespace. We have to reimplement
- char * next = strtok(text, "\n");
- while (next != NULL){
- nvgText(vg, x, y, next, NULL);
- y += lineh;
- next = strtok(NULL, "\n");
- }
- }
-
- // Button
- int b_x = 1500;
- int b_y = 800;
- int b_w = 300;
- int b_h = 150;
-
- nvgBeginPath(vg);
- nvgFillColor(vg, nvgRGBA(8, 8, 8, 255));
- nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
- nvgFill(vg);
-
- nvgFillColor(vg, nvgRGBA(255, 255, 255, 255));
- nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
- nvgText(vg, b_x+b_w/2, b_y+b_h/2, "Exit", NULL);
-
- nvgBeginPath(vg);
- nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50));
- nvgStrokeWidth(vg, 5);
- nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
- nvgStroke(vg);
-
- // Draw to screen
- nvgEndFrame(vg);
- fb->swap();
- assert(glGetError() == GL_NO_ERROR);
-
-
- // Wait for button
- TouchState touch;
- touch_init(&touch);
-
- while (true){
- int touch_x = -1, touch_y = -1;
- int res = touch_poll(&touch, &touch_x, &touch_y, 0);
- if (res){
-
- if (touch_x > b_x && touch_x < b_x + b_w){
- if (touch_y > b_y && touch_y < b_y + b_h){
- return 1;
- }
- }
- }
-
- usleep(1000000 / 60);
- }
-
- return 0;
-}
diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc
deleted file mode 100644
index 1c965ded..00000000
--- a/selfdrive/ui/android/ui.cc
+++ /dev/null
@@ -1,188 +0,0 @@
-#include
-#include
-#include
-#include
-
-#include
-
-#include "common/util.h"
-#include "common/params.h"
-#include "common/touch.h"
-#include "common/swaglog.h"
-
-#include "ui.hpp"
-#include "paint.hpp"
-#include "android/sl_sound.hpp"
-
-ExitHandler do_exit;
-static void ui_set_brightness(UIState *s, int brightness) {
- static int last_brightness = -1;
- if (last_brightness != brightness && (s->awake || brightness == 0)) {
- if (set_brightness(brightness)) {
- last_brightness = brightness;
- }
- }
-}
-
-static void handle_display_state(UIState *s, bool user_input) {
- static int awake_timeout = 0;
-
- constexpr float accel_samples = 5*UI_FREQ;
- static float accel_prev = 0., gyro_prev = 0.;
-
- bool should_wake = s->started || s->ignition || user_input;
- if (!should_wake) {
- // tap detection while display is off
- bool accel_trigger = abs(s->accel_sensor - accel_prev) > 0.2;
- bool gyro_trigger = abs(s->gyro_sensor - gyro_prev) > 0.15;
- should_wake = accel_trigger && gyro_trigger;
- gyro_prev = s->gyro_sensor;
- accel_prev = (accel_prev * (accel_samples - 1) + s->accel_sensor) / accel_samples;
- }
-
- // determine desired state
- if (should_wake) {
- awake_timeout = 30*UI_FREQ;
- } else if (awake_timeout > 0){
- --awake_timeout;
- should_wake = true;
- }
-
- // handle state transition
- if (s->awake != should_wake) {
- s->awake = should_wake;
- int display_mode = s->awake ? HWC_POWER_MODE_NORMAL : HWC_POWER_MODE_OFF;
- LOGW("setting display mode %d", display_mode);
- s->fb->set_power(display_mode);
-
- if (s->awake) {
- system("service call window 18 i32 1");
- }
- }
-}
-
-static void handle_vision_touch(UIState *s, int touch_x, int touch_y) {
- if (s->started && (touch_x >= s->viz_rect.x - bdr_s)
- && (s->active_app != cereal::UiLayoutState::App::SETTINGS)) {
- if (!s->scene.frontview) {
- s->sidebar_collapsed = !s->sidebar_collapsed;
- } else {
- Params().write_db_value("IsDriverViewEnabled", "0", 1);
- }
- }
-}
-
-static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) {
- if (!s->sidebar_collapsed && touch_x <= sbr_w) {
- if (settings_btn.ptInRect(touch_x, touch_y)) {
- s->active_app = cereal::UiLayoutState::App::SETTINGS;
- } else if (home_btn.ptInRect(touch_x, touch_y)) {
- if (s->started) {
- s->active_app = cereal::UiLayoutState::App::NONE;
- s->sidebar_collapsed = true;
- } else {
- s->active_app = cereal::UiLayoutState::App::HOME;
- }
- }
- }
-}
-
-static void update_offroad_layout_state(UIState *s, PubMaster *pm) {
- static int timeout = 0;
- static bool prev_collapsed = false;
- static cereal::UiLayoutState::App prev_app = cereal::UiLayoutState::App::NONE;
- if (timeout > 0) {
- timeout--;
- }
- if (prev_collapsed != s->sidebar_collapsed || prev_app != s->active_app || timeout == 0) {
- MessageBuilder msg;
- auto layout = msg.initEvent().initUiLayoutState();
- layout.setActiveApp(s->active_app);
- layout.setSidebarCollapsed(s->sidebar_collapsed);
- pm->send("offroadLayout", msg);
- LOGD("setting active app to %d with sidebar %d", (int)s->active_app, s->sidebar_collapsed);
- prev_collapsed = s->sidebar_collapsed;
- prev_app = s->active_app;
- timeout = 2 * UI_FREQ;
- }
-}
-
-int main(int argc, char* argv[]) {
- setpriority(PRIO_PROCESS, 0, -14);
- SLSound sound;
-
- UIState uistate = {};
- UIState *s = &uistate;
- ui_init(s);
- s->sound = &sound;
-
- TouchState touch = {0};
- touch_init(&touch);
- handle_display_state(s, true);
-
- PubMaster *pm = new PubMaster({"offroadLayout"});
-
- // light sensor scaling and volume params
- const bool LEON = util::read_file("/proc/cmdline").find("letv") != std::string::npos;
-
- float brightness_b = 0, brightness_m = 0;
- int result = read_param(&brightness_b, "BRIGHTNESS_B", true);
- result += read_param(&brightness_m, "BRIGHTNESS_M", true);
- if (result != 0) {
- brightness_b = LEON ? 10.0 : 5.0;
- brightness_m = LEON ? 2.6 : 1.3;
- write_param_float(brightness_b, "BRIGHTNESS_B", true);
- write_param_float(brightness_m, "BRIGHTNESS_M", true);
- }
- float smooth_brightness = brightness_b;
-
- const int MIN_VOLUME = LEON ? 12 : 9;
- const int MAX_VOLUME = LEON ? 15 : 12;
- s->sound->setVolume(MIN_VOLUME);
-
- while (!do_exit) {
- if (!s->started) {
- util::sleep_for(50);
- }
- double u1 = millis_since_boot();
-
- ui_update(s);
-
- // poll for touch events
- int touch_x = -1, touch_y = -1;
- int touched = touch_poll(&touch, &touch_x, &touch_y, 0);
- if (touched == 1) {
- handle_sidebar_touch(s, touch_x, touch_y);
- handle_vision_touch(s, touch_x, touch_y);
- }
-
- // Don't waste resources on drawing in case screen is off
- handle_display_state(s, touched == 1);
- if (!s->awake) {
- continue;
- }
-
- // up one notch every 5 m/s
- s->sound->setVolume(fmin(MAX_VOLUME, MIN_VOLUME + s->scene.car_state.getVEgo() / 5));
-
- // set brightness
- float clipped_brightness = fmin(512, (s->light_sensor*brightness_m) + brightness_b);
- smooth_brightness = fmin(255, clipped_brightness * 0.01 + smooth_brightness * 0.99);
- ui_set_brightness(s, (int)smooth_brightness);
-
- update_offroad_layout_state(s, pm);
-
- ui_draw(s);
- double u2 = millis_since_boot();
- if (!s->scene.frontview && (u2-u1 > 66)) {
- // warn on sub 15fps
- LOGW("slow frame(%llu) time: %.2f", (s->sm)->frame, u2-u1);
- }
- s->fb->swap();
- }
-
- handle_display_state(s, true);
- delete s->sm;
- delete pm;
- return 0;
-}
diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
index 678aad7f..563dec36 100644
--- a/selfdrive/ui/paint.cc
+++ b/selfdrive/ui/paint.cc
@@ -1,9 +1,6 @@
#include "ui.hpp"
#include
-#include