diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index b8564c1e8..8f54cdf07 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -18,7 +18,7 @@ You can test your changes on your machine by running `run_docker_tests.sh`. This
### Automated Testing
-All PRs and commits are automatically checked by Github Actions. Check out `.github/workflows/` for what Github Actions runs. Any new tests sould be added to Github Actions.
+All PRs and commits are automatically checked by Github Actions. Check out `.github/workflows/` for what Github Actions runs. Any new tests should be added to Github Actions.
### Code Style and Linting
@@ -28,7 +28,7 @@ Code is automatically checked for style by Github Actions as part of the automat
We've released a [Model Port guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for porting to Toyota/Lexus models.
-If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty. See our bounties at [comma.ai/bounties.html](https://comma.ai/bounties.html)
+If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty.
## Pull Requests
diff --git a/Jenkinsfile b/Jenkinsfile
index dd0e2c12f..df52f59e1 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -70,7 +70,7 @@ pipeline {
stage('PC tests') {
agent {
dockerfile {
- filename 'Dockerfile.openpilot'
+ filename 'Dockerfile.openpilotci'
args '--privileged --shm-size=1G --user=root'
}
}
@@ -132,6 +132,7 @@ pipeline {
["build cereal", "SCONS_CACHE=1 scons -j4 cereal/"],
["test sounds", "nosetests -s selfdrive/test/test_sounds.py"],
["test boardd loopback", "nosetests -s selfdrive/boardd/tests/test_boardd_loopback.py"],
+ ["test loggerd", "CI=1 python selfdrive/loggerd/tests/test_loggerd.py"],
//["test updater", "python installer/updater/test_updater.py"],
])
}
@@ -140,6 +141,13 @@ pipeline {
}
}
}
+
+ post {
+ always {
+ cleanWs()
+ }
+ }
+
}
}
diff --git a/README.md b/README.md
index f78a07088..17a095252 100644
--- a/README.md
+++ b/README.md
@@ -70,31 +70,30 @@ Supported Cars
| Honda | Accord Hybrid 2018-20 | All | Stock | 0mph | 3mph |
| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph |
| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph |
-| Honda | Civic Sedan/Coupe 2019-20 | Honda Sensing | Stock | 0mph | 2mph2 |
+| Honda | Civic Sedan/Coupe 2019-20 | All | Stock | 0mph | 2mph2 |
| Honda | CR-V 2015-16 | Touring | openpilot | 25mph1 | 12mph |
| Honda | CR-V 2017-20 | Honda Sensing | Stock | 0mph | 12mph |
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph |
| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph1 | 12mph |
| Honda | HR-V 2019 | Honda Sensing | openpilot | 25mph1 | 12mph |
-| Honda | Insight 2019-20 | Honda Sensing | Stock | 0mph | 3mph |
+| Honda | Insight 2019-20 | All | Stock | 0mph | 3mph |
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph1 | 0mph |
| Honda | Passport 2019 | All | openpilot | 25mph1 | 12mph |
-| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph1 | 12mph |
-| Honda | Pilot 2019 | All | openpilot | 25mph1 | 12mph |
+| Honda | Pilot 2016-19 | Honda Sensing | openpilot | 25mph1 | 12mph |
| Honda | Ridgeline 2017-20 | Honda Sensing | openpilot | 25mph1 | 12mph |
+| Hyundai | Palisade 2020 | All | Stock | 0mph | 0mph |
| Hyundai | Sonata 2020 | All | Stock | 0mph | 0mph |
-| Lexus | CT Hybrid 2017-18 | All | Stock3| 0mph | 0mph |
+| Lexus | CT Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph |
| Lexus | ES 2019 | All | openpilot | 0mph | 0mph |
| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph |
| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph |
| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph |
| Lexus | NX Hybrid 2018 | All | Stock3| 0mph | 0mph |
-| Lexus | RX 2016-17 | All | Stock3| 0mph | 0mph |
+| Lexus | RX 2016-18 | All | Stock3| 0mph | 0mph |
| Lexus | RX 2020 | All | openpilot | 0mph | 0mph |
| Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph |
| Lexus | RX Hybrid 2020 | All | openpilot | 0mph | 0mph |
-| Toyota | Avalon 2016 | TSS-P | Stock3| 20mph1 | 0mph |
-| Toyota | Avalon 2017-18 | All | Stock3| 20mph1 | 0mph |
+| Toyota | Avalon 2016-18 | TSS-P | Stock3| 20mph1 | 0mph |
| Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph |
| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph4 | 0mph |
| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph |
@@ -102,19 +101,16 @@ Supported Cars
| Toyota | Corolla 2017-19 | All | Stock3| 20mph1 | 0mph |
| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph |
| Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph |
-| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph |
+| Toyota | Corolla Hybrid 2020-21 | All | openpilot | 0mph | 0mph |
| Toyota | Highlander 2017-19 | All | Stock3| 0mph | 0mph |
-| Toyota | Highlander Hybrid 2017-19 | All | Stock3| 0mph | 0mph |
| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph |
+| Toyota | Highlander Hybrid 2017-19 | All | Stock3| 0mph | 0mph |
| Toyota | Highlander Hybrid 2020 | All | openpilot | 0mph | 0mph |
-| Toyota | Prius 2016 | TSS-P | Stock3| 0mph | 0mph |
-| Toyota | Prius 2017-20 | All | Stock3| 0mph | 0mph |
+| Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph |
| Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph |
-| Toyota | Rav4 2016 | TSS-P | Stock3| 20mph1 | 0mph |
-| Toyota | Rav4 2017-18 | All | Stock3| 20mph1 | 0mph |
+| Toyota | Rav4 2016-18 | TSS-P | Stock3| 20mph1 | 0mph |
| Toyota | Rav4 2019-20 | All | openpilot | 0mph | 0mph |
-| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock3| 0mph | 0mph |
-| Toyota | Rav4 Hybrid 2017-18 | All | Stock3| 0mph | 0mph |
+| Toyota | Rav4 Hybrid 2016-18 | TSS-P | Stock3| 0mph | 0mph |
| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph |
| Toyota | Sienna 2018-20 | All | Stock3| 0mph | 0mph |
@@ -143,24 +139,22 @@ Community Maintained Cars and Features
| Holden | Astra 20171 | Adaptive Cruise | openpilot | 0mph | 7mph |
| Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph |
| Hyundai | Genesis 2015-16 | SCC + LKAS | Stock | 19mph | 37mph |
-| Hyundai | Ioniq Electric Premium SE 2020| SCC + LKAS | Stock | 0mph | 32mph |
-| Hyundai | Ioniq Electric Limited 2019 | SCC + LKAS | Stock | 0mph | 32mph |
+| Hyundai | Ioniq Electric 2019-20 | SCC + LKAS | Stock | 0mph | 32mph |
| Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Kona EV 2019 | SCC + LKAS | Stock | 0mph | 0mph |
-| Hyundai | Palisade 2020 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2019 | All | Stock | 0mph | 0mph |
| Hyundai | Sonata 2019 | All | Stock | 0mph | 0mph |
| Hyundai | Veloster 2019 | SCC + LKAS | Stock | 5mph | 0mph |
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
| Kia | Forte 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
-| Kia | Optima 2017 | SCC + LKAS/LDWS | Stock | 0mph | 32mph |
+| Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph |
| Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Sorento 2018 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph |
-| Nissan | Leaf 2018-19 | Propilot | Stock | 0mph | 0mph |
-| Nissan | Rogue 2019 | Propilot | Stock | 0mph | 0mph |
-| Nissan | X-Trail 2017 | Propilot | Stock | 0mph | 0mph |
+| Nissan | Leaf 2018-19 | ProPILOT | Stock | 0mph | 0mph |
+| Nissan | Rogue 2019 | ProPILOT | Stock | 0mph | 0mph |
+| Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph |
| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Forester 2019 | EyeSight | Stock | 0mph | 0mph |
@@ -274,8 +268,23 @@ Safety and Testing
Testing on PC
------
+For simplified development and experimentation, openpilot runs in the CARLA driving simulator, which allows you to develop openpilot without a car.
+
+Steps:
+1) Start the CARLA server on first terminal
+```
+bash -c "$(curl https://raw.githubusercontent.com/commaai/openpilot/master/tools/sim/start_carla.sh)"
+```
+2) Start openpilot on second terminal
+```
+bash -c "$(curl https://raw.githubusercontent.com/commaai/openpilot/master/tools/sim/start_openpilot_docker.sh)"
+```
+3) Press 1 to engage openpilot
+
+See the full [README](tools/sim/README.md)
+
+You should also take a look at the tools directory in master: lots of tools you can use to replay driving data, test, and develop openpilot from your PC.
-Check out the tools directory in master: lots of tools you can use to replay driving data, test and develop openpilot from your pc.
Community and Contributing
------
diff --git a/RELEASES.md b/RELEASES.md
index 0c6dd3d4f..e8a157567 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,11 @@
+Version 0.7.9 (2020-10-09)
+========================
+ * Improved car battery power management
+ * Improved updater robustness
+ * Improved realtime performance
+ * Reduced UI and modeld lags
+ * Increased torque on 2020 Hyundai Sonata and Palisade
+
Version 0.7.8 (2020-08-19)
========================
* New driver monitoring model: improved face detection and better compatibility with sunglasses
diff --git a/SConstruct b/SConstruct
index 322bdd5f9..fc119262b 100644
--- a/SConstruct
+++ b/SConstruct
@@ -6,6 +6,8 @@ import subprocess
import sys
import platform
+TICI = os.path.isfile('/TICI')
+
AddOption('--test',
action='store_true',
help='build test files')
@@ -18,10 +20,11 @@ AddOption('--asan',
cython_dependencies = [Value(v) for v in (sys.version, distutils.__version__, Cython.__version__)]
Export('cython_dependencies')
-arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
+real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
if platform.system() == "Darwin":
arch = "Darwin"
-if arch == "aarch64" and not os.path.isdir("/system"):
+
+if arch == "aarch64" and TICI:
arch = "larch64"
webcam = bool(ARGUMENTS.get("use_webcam", 0))
@@ -44,7 +47,6 @@ if arch == "aarch64" or arch == "larch64":
libpath = [
"/usr/lib",
- "/data/data/com.termux/files/usr/lib",
"/system/vendor/lib64",
"/system/comma/usr/lib",
"#phonelibs/nanovg",
@@ -62,11 +64,12 @@ if arch == "aarch64" or arch == "larch64":
else:
libpath += [
"#phonelibs/snpe/aarch64",
- "#phonelibs/libyuv/lib"
+ "#phonelibs/libyuv/lib",
+ "/system/vendor/lib64"
]
cflags = ["-DQCOM", "-mcpu=cortex-a57"]
cxxflags = ["-DQCOM", "-mcpu=cortex-a57"]
- rpath = ["/system/vendor/lib64"]
+ rpath = []
if QCOM_REPLAY:
cflags += ["-DQCOM_REPLAY"]
@@ -131,8 +134,11 @@ env = Environment(
"-O2",
"-Wunused",
"-Werror",
+ "-Wno-unknown-warning-option",
"-Wno-deprecated-register",
"-Wno-inconsistent-missing-override",
+ "-Wno-c99-designator",
+ "-Wno-reorder-init-list",
] + cflags + ccflags_asan,
CPPPATH=cpppath + [
@@ -143,7 +149,6 @@ env = Environment(
"#phonelibs/openmax/include",
"#phonelibs/json11",
"#phonelibs/curl/include",
- #"#phonelibs/opencv/include", # use opencv4 instead
"#phonelibs/libgralloc/include",
"#phonelibs/android_frameworks_native/include",
"#phonelibs/android_hardware_libhardware/include",
@@ -156,6 +161,8 @@ env = Environment(
"#selfdrive/camerad/include",
"#selfdrive/loggerd/include",
"#selfdrive/modeld",
+ "#selfdrive/sensord",
+ "#selfdrive/ui",
"#cereal/messaging",
"#cereal",
"#opendbc/can",
@@ -176,6 +183,44 @@ env = Environment(
]
)
+qt_env = None
+if arch in ["x86_64", "Darwin", "larch64"]:
+ qt_env = env.Clone()
+
+ if arch == "Darwin":
+ qt_env['QTDIR'] = "/usr/local/opt/qt"
+ QT_BASE = "/usr/local/opt/qt/"
+ qt_dirs = [
+ QT_BASE + "include/",
+ QT_BASE + "include/QtWidgets",
+ QT_BASE + "include/QtGui",
+ QT_BASE + "include/QtCore",
+ QT_BASE + "include/QtDBus",
+ QT_BASE + "include/QtMultimedia",
+ ]
+ qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"]
+ else:
+ qt_dirs = [
+ f"/usr/include/{real_arch}-linux-gnu/qt5",
+ f"/usr/include/{real_arch}-linux-gnu/qt5/QtWidgets",
+ f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui",
+ f"/usr/include/{real_arch}-linux-gnu/qt5/QtCore",
+ f"/usr/include/{real_arch}-linux-gnu/qt5/QtDBus",
+ f"/usr/include/{real_arch}-linux-gnu/qt5/QtMultimedia",
+ f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.5.1/QtGui",
+ ]
+
+ qt_env.Tool('qt')
+ qt_env['CPPPATH'] += qt_dirs
+ qt_flags = [
+ "-D_REENTRANT",
+ "-DQT_NO_DEBUG",
+ "-DQT_WIDGETS_LIB",
+ "-DQT_GUI_LIB",
+ "-DQT_CORE_LIB"
+ ]
+ qt_env['CXXFLAGS'] += qt_flags
+
if os.environ.get('SCONS_CACHE'):
cache_dir = '/tmp/scons_cache'
@@ -214,7 +259,7 @@ def abspath(x):
# still needed for apks
zmq = 'zmq'
-Export('env', 'arch', 'zmq', 'SHARED', 'webcam', 'QCOM_REPLAY')
+Export('env', 'qt_env', 'arch', 'zmq', 'SHARED', 'webcam', 'QCOM_REPLAY')
# cereal and messaging are shared with the system
SConscript(['cereal/SConscript'])
@@ -255,16 +300,18 @@ SConscript(['selfdrive/controls/lib/longitudinal_mpc_model/SConscript'])
SConscript(['selfdrive/boardd/SConscript'])
SConscript(['selfdrive/proclogd/SConscript'])
+SConscript(['selfdrive/clocksd/SConscript'])
-SConscript(['selfdrive/ui/SConscript'])
SConscript(['selfdrive/loggerd/SConscript'])
SConscript(['selfdrive/locationd/SConscript'])
SConscript(['selfdrive/locationd/models/SConscript'])
+SConscript(['selfdrive/sensord/SConscript'])
+SConscript(['selfdrive/ui/SConscript'])
-if arch == "aarch64":
+if arch != "Darwin":
SConscript(['selfdrive/logcatd/SConscript'])
- SConscript(['selfdrive/sensord/SConscript'])
- SConscript(['selfdrive/clocksd/SConscript'])
-else:
+
+
+if arch == "x86_64":
SConscript(['tools/lib/index_log/SConscript'])
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 0db47f78b..17bc461b3 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -38,7 +38,6 @@ struct CarEvent @0x9b1657f34caf3ad3 {
pedalPressed @13;
cruiseDisabled @14;
radarCanError @15;
- dataNeededDEPRECATED @16;
speedTooLow @17;
outOfSpace @18;
overheat @19;
@@ -49,29 +48,22 @@ struct CarEvent @0x9b1657f34caf3ad3 {
pcmDisable @24;
noTarget @25;
radarFault @26;
- modelCommIssueDEPRECATED @27;
brakeHold @28;
parkBrake @29;
manualRestart @30;
lowSpeedLockout @31;
plannerError @32;
- ipasOverrideDEPRECATED @33;
debugAlert @34;
steerTempUnavailableMute @35;
resumeRequired @36;
preDriverDistracted @37;
promptDriverDistracted @38;
driverDistracted @39;
- geofenceDEPRECATED @40;
- driverMonitorOnDEPRECATED @41;
- driverMonitorOffDEPRECATED @42;
preDriverUnresponsive @43;
promptDriverUnresponsive @44;
driverUnresponsive @45;
belowSteerSpeed @46;
- calibrationProgressDEPRECATED @47;
lowBattery @48;
- invalidGiraffeHondaDEPRECATED @49;
vehicleModelInvalid @50;
controlsFailed @51;
sensorDataInvalid @52;
@@ -104,8 +96,6 @@ struct CarEvent @0x9b1657f34caf3ad3 {
fcw @79;
steerSaturated @80;
whitePandaUnsupported @81;
- startupWhitePanda @82;
- canErrorPersistentDEPRECATED @83;
belowEngageSpeed @84;
noGps @85;
focusRecoverActive @86;
@@ -113,6 +103,17 @@ struct CarEvent @0x9b1657f34caf3ad3 {
neosUpdateRequired @88;
modeldLagging @89;
deviceFalling @90;
+
+ dataNeededDEPRECATED @16;
+ modelCommIssueDEPRECATED @27;
+ ipasOverrideDEPRECATED @33;
+ geofenceDEPRECATED @40;
+ driverMonitorOnDEPRECATED @41;
+ driverMonitorOffDEPRECATED @42;
+ calibrationProgressDEPRECATED @47;
+ invalidGiraffeHondaDEPRECATED @49;
+ canErrorPersistentDEPRECATED @83;
+ startupWhitePandaDEPRECATED @82;
}
}
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 64a577bc5..c8419500a 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -186,6 +186,7 @@ struct SensorEventData {
gyroUncalibrated @12 :SensorVec;
proximity @13: Float32;
light @14: Float32;
+ temperature @15: Float32;
}
source @8 :SensorSource;
@@ -203,6 +204,8 @@ struct SensorEventData {
lsm6ds3 @5; # accelerometer (c2)
bmp280 @6; # barometer (c2)
mmc3416x @7; # magnetometer (c2)
+ bmx055 @8;
+ rpr0521 @9;
}
}
@@ -267,14 +270,15 @@ struct CanData {
}
struct ThermalData {
- cpu0 @0 :UInt16;
- cpu1 @1 :UInt16;
- cpu2 @2 :UInt16;
- cpu3 @3 :UInt16;
- mem @4 :UInt16;
- gpu @5 :UInt16;
- bat @6 :UInt32;
- pa0 @21 :UInt16;
+ # Deprecated
+ cpu0DEPRECATED @0 :UInt16;
+ cpu1DEPRECATED @1 :UInt16;
+ cpu2DEPRECATED @2 :UInt16;
+ cpu3DEPRECATED @3 :UInt16;
+ memDEPRECATED @4 :UInt16;
+ gpuDEPRECATED @5 :UInt16;
+ batDEPRECATED @6 :UInt32;
+ pa0DEPRECATED @21 :UInt16;
# not thermal
freeSpace @7 :Float32;
@@ -286,6 +290,7 @@ struct ThermalData {
networkType @22 :NetworkType;
offroadPowerUsage @23 :UInt32; # Power usage since going offroad in uWh
networkStrength @24 :NetworkStrength;
+ carBatteryCapacity @25 :UInt32; # Estimated remaining car battery capacity in uWh
fanSpeed @10 :UInt16;
started @11 :Bool;
@@ -298,6 +303,12 @@ struct ThermalData {
memUsedPercent @19 :Int8;
cpuPerc @20 :Int8;
+ cpu @26 :List(Float32);
+ gpu @27 :List(Float32);
+ mem @28 :Float32;
+ bat @29 :Float32;
+ ambient @30 :Float32;
+
enum ThermalStatus {
green @0; # all processes run
yellow @1; # critical processes run (kill uploader), engage still allowed
@@ -373,6 +384,8 @@ struct HealthData {
interruptRateTim3 @17;
registerDivergent @18;
interruptRateKlineInit @19;
+ interruptRateClockSource @20;
+ interruptRateTim9 @21;
# Update max fault type in boardd when adding faults
}
@@ -602,7 +615,7 @@ struct ControlsState @0x97ff69c53601abf1 {
output @3 :Float32;
lqrOutput @4 :Float32;
saturated @5 :Bool;
- }
+ }
}
struct LiveEventData {
@@ -675,6 +688,52 @@ struct ModelData {
}
}
+
+struct ModelDataV2 {
+ frameId @0 :UInt32;
+ frameAge @1 :UInt32;
+ frameDropPerc @2 :Float32;
+ timestampEof @3 :UInt64;
+
+ position @4 :XYZTData;
+ orientation @5 :XYZTData;
+ velocity @6 :XYZTData;
+ orientationRate @7 :XYZTData;
+ laneLines @8 :List(XYZTData);
+ laneLineProbs @9 :List(Float32);
+ roadEdges @10 :List(XYZTData);
+ leads @11 :List(LeadDataV2);
+
+ meta @12 :MetaData;
+
+ struct XYZTData {
+ x @0 :List(Float32);
+ y @1 :List(Float32);
+ z @2 :List(Float32);
+ t @3 :List(Float32);
+ xStd @4 :List(Float32);
+ yStd @5 :List(Float32);
+ zStd @6 :List(Float32);
+ }
+
+ struct LeadDataV2 {
+ prob @0 :Float32;
+ t @1 :Float32;
+ xyva @2 :List(Float32);
+ xyvaStd @3 :List(Float32);
+ }
+
+ struct MetaData {
+ engagedProb @0 :Float32;
+ desirePrediction @1 :List(Float32);
+ brakeDisengageProb @2 :Float32;
+ gasDisengageProb @3 :Float32;
+ steerOverrideProb @4 :Float32;
+ desireState @5 :List(Float32);
+ }
+}
+
+
struct CalibrationFeatures {
frameId @0 :UInt32;
@@ -1906,7 +1965,6 @@ struct DMonitoringState {
isDistracted @2 :Bool;
awarenessStatus @3 :Float32;
isRHD @4 :Bool;
- rhdChecked @5 :Bool;
posePitchOffset @6 :Float32;
posePitchValidCount @7 :UInt32;
poseYawOffset @8 :Float32;
@@ -1917,6 +1975,8 @@ struct DMonitoringState {
isLowStd @13 :Bool;
hiStdCount @14 :UInt32;
isPreview @15 :Bool;
+
+ rhdCheckedDEPRECATED @5 :Bool;
}
struct Boot {
@@ -2062,5 +2122,7 @@ struct Event {
dMonitoringState @71: DMonitoringState;
liveLocationKalman @72 :LiveLocationKalman;
sentinel @73 :Sentinel;
+ wideFrame @74: FrameData;
+ modelV2 @75 :ModelDataV2;
}
}
diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py
index 5e36bd8d8..0b1204b47 100644
--- a/cereal/messaging/__init__.py
+++ b/cereal/messaging/__init__.py
@@ -3,6 +3,8 @@ from .messaging_pyx import Context, Poller, SubSocket, PubSocket # pylint: disa
from .messaging_pyx import MultiplePublishersError, MessagingError # pylint: disable=no-name-in-module, import-error
import capnp
+from typing import Optional, List, Union
+
from cereal import log
from cereal.services import service_list
@@ -19,7 +21,7 @@ except ImportError:
context = Context()
-def new_message(service=None, size=None):
+def new_message(service: Optional[str] = None, size: Optional[int] = None) -> capnp.lib.capnp._DynamicStructBuilder:
dat = log.Event.new_message()
dat.logMonoTime = int(sec_since_boot() * 1e9)
dat.valid = True
@@ -30,15 +32,15 @@ def new_message(service=None, size=None):
dat.init(service, size)
return dat
-def pub_sock(endpoint):
+def pub_sock(endpoint: str) -> PubSocket:
sock = PubSocket()
sock.connect(context, endpoint)
return sock
-def sub_sock(endpoint, poller=None, addr="127.0.0.1", conflate=False, timeout=None):
+def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1",
+ conflate: bool = False, timeout: Optional[int] = None) -> SubSocket:
sock = SubSocket()
- addr = addr.encode('utf8')
- sock.connect(context, endpoint, addr, conflate)
+ sock.connect(context, endpoint, addr.encode('utf8'), conflate)
if timeout is not None:
sock.setTimeout(timeout)
@@ -48,9 +50,9 @@ def sub_sock(endpoint, poller=None, addr="127.0.0.1", conflate=False, timeout=No
return sock
-def drain_sock_raw(sock, wait_for_one=False):
+def drain_sock_raw(sock: SubSocket, wait_for_one: bool = False) -> List[bytes]:
"""Receive all message currently available on the queue"""
- ret = []
+ ret: List[bytes] = []
while 1:
if wait_for_one and len(ret) == 0:
dat = sock.receive()
@@ -64,9 +66,9 @@ def drain_sock_raw(sock, wait_for_one=False):
return ret
-def drain_sock(sock, wait_for_one=False):
+def drain_sock(sock: SubSocket, wait_for_one: bool = False) -> List[capnp.lib.capnp._DynamicStructReader]:
"""Receive all message currently available on the queue"""
- ret = []
+ ret: List[capnp.lib.capnp._DynamicStructReader] = []
while 1:
if wait_for_one and len(ret) == 0:
dat = sock.receive()
@@ -83,7 +85,7 @@ def drain_sock(sock, wait_for_one=False):
# TODO: print when we drop packets?
-def recv_sock(sock, wait=False):
+def recv_sock(sock: SubSocket, wait: bool = False) -> Union[None, capnp.lib.capnp._DynamicStructReader]:
"""Same as drain sock, but only returns latest message. Consider using conflate instead."""
dat = None
@@ -103,19 +105,19 @@ def recv_sock(sock, wait=False):
return dat
-def recv_one(sock):
+def recv_one(sock: SubSocket) -> Union[None, capnp.lib.capnp._DynamicStructReader]:
dat = sock.receive()
if dat is not None:
dat = log.Event.from_bytes(dat)
return dat
-def recv_one_or_none(sock):
+def recv_one_or_none(sock: SubSocket) -> Union[None, capnp.lib.capnp._DynamicStructReader]:
dat = sock.receive(non_blocking=True)
if dat is not None:
dat = log.Event.from_bytes(dat)
return dat
-def recv_one_retry(sock):
+def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader:
"""Keep receiving until we get a message"""
while True:
dat = sock.receive()
@@ -123,8 +125,8 @@ def recv_one_retry(sock):
return log.Event.from_bytes(dat)
class SubMaster():
- def __init__(self, services, ignore_alive=None, addr="127.0.0.1"):
- self.poller = Poller()
+ def __init__(self, services: List[str], poll: Optional[List[str]] = None,
+ ignore_alive: Optional[List[str]] = None, addr:str ="127.0.0.1"):
self.frame = -1
self.updated = {s: False for s in services}
self.rcv_time = {s: 0. for s in services}
@@ -133,8 +135,12 @@ class SubMaster():
self.sock = {}
self.freq = {}
self.data = {}
- self.logMonoTime = {}
self.valid = {}
+ self.logMonoTime = {}
+
+ self.poller = Poller()
+ self.non_polled_services = [s for s in services if poll is not None and
+ len(poll) and s not in poll]
if ignore_alive is not None:
self.ignore_alive = ignore_alive
@@ -143,30 +149,33 @@ class SubMaster():
for s in services:
if addr is not None:
- self.sock[s] = sub_sock(s, poller=self.poller, addr=addr, conflate=True)
+ p = self.poller if s not in self.non_polled_services else None
+ self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True)
self.freq[s] = service_list[s].frequency
try:
data = new_message(s)
except capnp.lib.capnp.KjException: # pylint: disable=c-extension-no-member
- # lists
- data = new_message(s, 0)
+ data = new_message(s, 0) # lists
self.data[s] = getattr(data, s)
self.logMonoTime[s] = 0
self.valid[s] = data.valid
- def __getitem__(self, s):
+ def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader:
return self.data[s]
- def update(self, timeout=1000):
+ def update(self, timeout: int = 1000) -> None:
msgs = []
for sock in self.poller.poll(timeout):
msgs.append(recv_one_or_none(sock))
+
+ # non-blocking receive for non-polled sockets
+ for s in self.non_polled_services:
+ msgs.append(recv_one_or_none(self.sock[s]))
self.update_msgs(sec_since_boot(), msgs)
- def update_msgs(self, cur_time, msgs):
- # TODO: add optional input that specify the service to wait for
+ def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None:
self.frame += 1
self.updated = dict.fromkeys(self.updated, False)
for msg in msgs:
@@ -189,30 +198,28 @@ class SubMaster():
else:
self.alive[s] = True
- def all_alive(self, service_list=None):
+ def all_alive(self, service_list=None) -> bool:
if service_list is None: # check all
service_list = self.alive.keys()
return all(self.alive[s] for s in service_list if s not in self.ignore_alive)
- def all_valid(self, service_list=None):
+ def all_valid(self, service_list=None) -> bool:
if service_list is None: # check all
service_list = self.valid.keys()
return all(self.valid[s] for s in service_list)
- def all_alive_and_valid(self, service_list=None):
+ def all_alive_and_valid(self, service_list=None) -> bool:
if service_list is None: # check all
service_list = self.alive.keys()
return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list)
-
class PubMaster():
- def __init__(self, services):
+ def __init__(self, services: List[str]):
self.sock = {}
for s in services:
self.sock[s] = pub_sock(s)
- def send(self, s, dat):
- # accept either bytes or capnp builder
+ def send(self, s: str, dat: Union[bytes, capnp.lib.capnp._DynamicStructBuilder]) -> None:
if not isinstance(dat, bytes):
dat = dat.to_bytes()
self.sock[s].send(dat)
diff --git a/cereal/messaging/impl_msgq.cc b/cereal/messaging/impl_msgq.cc
index 0a51d12c1..7da9a227c 100644
--- a/cereal/messaging/impl_msgq.cc
+++ b/cereal/messaging/impl_msgq.cc
@@ -15,6 +15,18 @@ void sig_handler(int signal) {
msgq_do_exit = 1;
}
+static size_t get_size(std::string endpoint){
+ size_t sz = DEFAULT_SEGMENT_SIZE;
+
+#if !defined(QCOM) && !defined(QCOM2)
+ if (endpoint == "frame" || endpoint == "frontFrame" || endpoint == "wideFrame"){
+ sz *= 10;
+ }
+#endif
+
+ return sz;
+}
+
MSGQContext::MSGQContext() {
}
@@ -49,13 +61,12 @@ MSGQMessage::~MSGQMessage() {
this->close();
}
-
int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate){
assert(context);
assert(address == "127.0.0.1");
q = new msgq_queue_t;
- int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE);
+ int r = msgq_new_queue(q, endpoint.c_str(), get_size(endpoint));
if (r != 0){
return r;
}
@@ -143,7 +154,7 @@ int MSGQPubSocket::connect(Context *context, std::string endpoint){
assert(context);
q = new msgq_queue_t;
- int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE);
+ int r = msgq_new_queue(q, endpoint.c_str(), get_size(endpoint));
if (r != 0){
return r;
}
diff --git a/cereal/messaging/messaging.hpp b/cereal/messaging/messaging.hpp
index fb0f96af4..9ade8bf2b 100644
--- a/cereal/messaging/messaging.hpp
+++ b/cereal/messaging/messaging.hpp
@@ -6,6 +6,10 @@
#include
#include "../gen/cpp/log.capnp.h"
+#ifdef __APPLE__
+#define CLOCK_BOOTTIME CLOCK_MONOTONIC
+#endif
+
#define MSG_MULTIPLE_PUBLISHERS 100
class Context {
@@ -82,11 +86,34 @@ private:
std::map services_;
};
+class MessageBuilder : public capnp::MallocMessageBuilder {
+public:
+ MessageBuilder() = default;
+
+ cereal::Event::Builder initEvent(bool valid = true) {
+ cereal::Event::Builder event = initRoot();
+ struct timespec t;
+ clock_gettime(CLOCK_BOOTTIME, &t);
+ uint64_t current_time = t.tv_sec * 1000000000ULL + t.tv_nsec;
+ event.setLogMonoTime(current_time);
+ event.setValid(valid);
+ return event;
+ }
+
+ kj::ArrayPtr toBytes() {
+ heapArray_ = capnp::messageToFlatArray(*this);
+ return heapArray_.asBytes();
+ }
+
+private:
+ kj::Array heapArray_;
+};
+
class PubMaster {
public:
PubMaster(const std::initializer_list &service_list);
inline int send(const char *name, capnp::byte *data, size_t size) { return sockets_.at(name)->send((char *)data, size); }
- int send(const char *name, capnp::MessageBuilder &msg);
+ int send(const char *name, MessageBuilder &msg);
~PubMaster();
private:
diff --git a/cereal/messaging/messaging_pyx_setup.py b/cereal/messaging/messaging_pyx_setup.py
index 992b39159..8e67a104c 100644
--- a/cereal/messaging/messaging_pyx_setup.py
+++ b/cereal/messaging/messaging_pyx_setup.py
@@ -6,6 +6,7 @@ from distutils.core import Extension, setup # pylint: disable=import-error,no-n
from Cython.Build import cythonize
from Cython.Distutils import build_ext
+TICI = os.path.isfile('/TICI')
def get_ext_filename_without_platform_suffix(filename):
name, ext = os.path.splitext(filename)
@@ -30,11 +31,11 @@ class BuildExtWithoutPlatformSuffix(build_ext):
sourcefiles = ['messaging_pyx.pyx']
-extra_compile_args = ["-std=c++14"]
+extra_compile_args = ["-std=c++14", "-Wno-nullability-completeness"]
libraries = ['zmq']
ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg
-if ARCH == "aarch64" and os.path.isdir("/system"):
+if ARCH == "aarch64" and not TICI:
# android
extra_compile_args += ["-Wno-deprecated-register"]
libraries += ['gnustl_shared']
diff --git a/cereal/messaging/msgq.cc b/cereal/messaging/msgq.cc
index 263010168..53bdb9af0 100644
--- a/cereal/messaging/msgq.cc
+++ b/cereal/messaging/msgq.cc
@@ -21,6 +21,8 @@
#include
+#include "services.h"
+
#include "msgq.hpp"
void sigusr2_handler(int signal) {
@@ -81,11 +83,20 @@ void msgq_wait_for_subscriber(msgq_queue_t *q){
return;
}
-
+bool service_exists(std::string path){
+ for (const auto& it : services) {
+ if (it.name == path) {
+ return true;
+ }
+ }
+ return false;
+}
int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){
assert(size < 0xFFFFFFFF); // Buffer must be smaller than 2^32 bytes
-
+ if (!service_exists(std::string(path))){
+ std::cout << "Warning, " << std::string(path) << " is not in service list." << std::endl;
+ }
std::signal(SIGUSR2, sigusr2_handler);
const char * prefix = "/dev/shm/";
@@ -102,15 +113,15 @@ int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){
delete[] full_path;
int rc = ftruncate(fd, size + sizeof(msgq_header_t));
- if (rc < 0)
+ if (rc < 0){
return -1;
-
+ }
char * mem = (char*)mmap(NULL, size + sizeof(msgq_header_t), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
close(fd);
- if (mem == NULL)
+ if (mem == NULL){
return -1;
-
+ }
q->mmap_p = mem;
msgq_header_t *header = (msgq_header_t *)mem;
@@ -418,8 +429,6 @@ int msgq_msg_recv(msgq_msg_t * msg, msgq_queue_t * q){
int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout){
- assert(timeout >= 0);
-
int num = 0;
// Check if messages ready
diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc
index a0f6c1ef4..53821aa21 100644
--- a/cereal/messaging/socketmaster.cc
+++ b/cereal/messaging/socketmaster.cc
@@ -3,10 +3,6 @@
#include "messaging.hpp"
#include "services.h"
-#ifdef __APPLE__
-#define CLOCK_BOOTTIME CLOCK_MONOTONIC
-#endif
-
static inline uint64_t nanos_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
@@ -164,9 +160,8 @@ PubMaster::PubMaster(const std::initializer_list &service_list) {
}
}
-int PubMaster::send(const char *name, capnp::MessageBuilder &msg) {
- auto words = capnp::messageToFlatArray(msg);
- auto bytes = words.asBytes();
+int PubMaster::send(const char *name, MessageBuilder &msg) {
+ auto bytes = msg.toBytes();
return send(name, bytes.begin(), bytes.size());
}
diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml
index 490e903f1..b140251cf 100644
--- a/cereal/service_list.yaml
+++ b/cereal/service_list.yaml
@@ -78,6 +78,8 @@ frontFrame: [8072, true, 10.]
dMonitoringState: [8073, true, 5., 1]
offroadLayout: [8074, false, 0.]
wideEncodeIdx: [8075, true, 20.]
+wideFrame: [8076, true, 20.]
+modelV2: [8077, true, 20., 20]
testModel: [8040, false, 0.]
testLiveLocation: [8045, false, 0.]
diff --git a/common/android.py b/common/android.py
deleted file mode 100644
index 43bb0f3c1..000000000
--- a/common/android.py
+++ /dev/null
@@ -1,286 +0,0 @@
-import os
-import binascii
-import itertools
-import re
-import struct
-import subprocess
-import random
-from cereal import log
-
-NetworkType = log.ThermalData.NetworkType
-NetworkStrength = log.ThermalData.NetworkStrength
-
-ANDROID = os.path.isfile('/EON')
-
-def get_sound_card_online():
- return (os.path.isfile('/proc/asound/card0/state') and
- open('/proc/asound/card0/state').read().strip() == 'ONLINE')
-
-def getprop(key):
- if not ANDROID:
- return ""
- return subprocess.check_output(["getprop", key], encoding='utf8').strip()
-
-def get_imei(slot):
- slot = str(slot)
- if slot not in ("0", "1"):
- raise ValueError("SIM slot must be 0 or 1")
-
- ret = parse_service_call_string(service_call(["iphonesubinfo", "3" , "i32", str(slot)]))
- if not ret:
- # allow non android to be identified differently
- ret = "%015d" % random.randint(0, 1 << 32)
- return ret
-
-def get_serial():
- ret = getprop("ro.serialno")
- if ret == "":
- ret = "cccccccc"
- return ret
-
-def get_subscriber_info():
- ret = parse_service_call_string(service_call(["iphonesubinfo", "7"]))
- if ret is None or len(ret) < 8:
- return ""
- return ret
-
-def reboot(reason=None):
- if reason is None:
- reason_args = ["null"]
- else:
- reason_args = ["s16", reason]
-
- subprocess.check_output([
- "service", "call", "power", "16", # IPowerManager.reboot
- "i32", "0", # no confirmation,
- *reason_args,
- "i32", "1" # wait
- ])
-
-def service_call(call):
- if not ANDROID:
- return None
-
- ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip()
- if 'Parcel' not in ret:
- return None
-
- return parse_service_call_bytes(ret)
-
-def parse_service_call_unpack(r, fmt):
- try:
- return struct.unpack(fmt, r)[0]
- except Exception:
- return None
-
-def parse_service_call_string(r):
- try:
- r = r[8:] # Cut off length field
- r = r.decode('utf_16_be')
-
- # All pairs of two characters seem to be swapped. Not sure why
- result = ""
- for a, b, in itertools.zip_longest(r[::2], r[1::2], fillvalue='\x00'):
- result += b + a
-
- result = result.replace('\x00', '')
-
- return result
- except Exception:
- return None
-
-def parse_service_call_bytes(ret):
- try:
- r = b""
- for hex_part in re.findall(r'[ (]([0-9a-f]{8})', ret):
- r += binascii.unhexlify(hex_part)
- return r
- except Exception:
- return None
-
-def get_network_type():
- if not ANDROID:
- return NetworkType.none
-
- wifi_check = parse_service_call_string(service_call(["connectivity", "2"]))
- if wifi_check is None:
- return NetworkType.none
- elif 'WIFI' in wifi_check:
- return NetworkType.wifi
- else:
- cell_check = parse_service_call_unpack(service_call(['phone', '59']), ">q")
- # from TelephonyManager.java
- cell_networks = {
- 0: NetworkType.none,
- 1: NetworkType.cell2G,
- 2: NetworkType.cell2G,
- 3: NetworkType.cell3G,
- 4: NetworkType.cell2G,
- 5: NetworkType.cell3G,
- 6: NetworkType.cell3G,
- 7: NetworkType.cell3G,
- 8: NetworkType.cell3G,
- 9: NetworkType.cell3G,
- 10: NetworkType.cell3G,
- 11: NetworkType.cell2G,
- 12: NetworkType.cell3G,
- 13: NetworkType.cell4G,
- 14: NetworkType.cell4G,
- 15: NetworkType.cell3G,
- 16: NetworkType.cell2G,
- 17: NetworkType.cell3G,
- 18: NetworkType.cell4G,
- 19: NetworkType.cell4G
- }
- return cell_networks.get(cell_check, NetworkType.none)
-
-def get_network_strength(network_type):
- network_strength = NetworkStrength.unknown
-
- # from SignalStrength.java
- def get_lte_level(rsrp, rssnr):
- INT_MAX = 2147483647
- if rsrp == INT_MAX:
- lvl_rsrp = NetworkStrength.unknown
- elif rsrp >= -95:
- lvl_rsrp = NetworkStrength.great
- elif rsrp >= -105:
- lvl_rsrp = NetworkStrength.good
- elif rsrp >= -115:
- lvl_rsrp = NetworkStrength.moderate
- else:
- lvl_rsrp = NetworkStrength.poor
- if rssnr == INT_MAX:
- lvl_rssnr = NetworkStrength.unknown
- elif rssnr >= 45:
- lvl_rssnr = NetworkStrength.great
- elif rssnr >= 10:
- lvl_rssnr = NetworkStrength.good
- elif rssnr >= -30:
- lvl_rssnr = NetworkStrength.moderate
- else:
- lvl_rssnr = NetworkStrength.poor
- return max(lvl_rsrp, lvl_rssnr)
-
- def get_tdscdma_level(tdscmadbm):
- lvl = NetworkStrength.unknown
- if tdscmadbm > -25:
- lvl = NetworkStrength.unknown
- elif tdscmadbm >= -49:
- lvl = NetworkStrength.great
- elif tdscmadbm >= -73:
- lvl = NetworkStrength.good
- elif tdscmadbm >= -97:
- lvl = NetworkStrength.moderate
- elif tdscmadbm >= -110:
- lvl = NetworkStrength.poor
- return lvl
-
- def get_gsm_level(asu):
- if asu <= 2 or asu == 99:
- lvl = NetworkStrength.unknown
- elif asu >= 12:
- lvl = NetworkStrength.great
- elif asu >= 8:
- lvl = NetworkStrength.good
- elif asu >= 5:
- lvl = NetworkStrength.moderate
- else:
- lvl = NetworkStrength.poor
- return lvl
-
- def get_evdo_level(evdodbm, evdosnr):
- lvl_evdodbm = NetworkStrength.unknown
- lvl_evdosnr = NetworkStrength.unknown
- if evdodbm >= -65:
- lvl_evdodbm = NetworkStrength.great
- elif evdodbm >= -75:
- lvl_evdodbm = NetworkStrength.good
- elif evdodbm >= -90:
- lvl_evdodbm = NetworkStrength.moderate
- elif evdodbm >= -105:
- lvl_evdodbm = NetworkStrength.poor
- if evdosnr >= 7:
- lvl_evdosnr = NetworkStrength.great
- elif evdosnr >= 5:
- lvl_evdosnr = NetworkStrength.good
- elif evdosnr >= 3:
- lvl_evdosnr = NetworkStrength.moderate
- elif evdosnr >= 1:
- lvl_evdosnr = NetworkStrength.poor
- return max(lvl_evdodbm, lvl_evdosnr)
-
- def get_cdma_level(cdmadbm, cdmaecio):
- lvl_cdmadbm = NetworkStrength.unknown
- lvl_cdmaecio = NetworkStrength.unknown
- if cdmadbm >= -75:
- lvl_cdmadbm = NetworkStrength.great
- elif cdmadbm >= -85:
- lvl_cdmadbm = NetworkStrength.good
- elif cdmadbm >= -95:
- lvl_cdmadbm = NetworkStrength.moderate
- elif cdmadbm >= -100:
- lvl_cdmadbm = NetworkStrength.poor
- if cdmaecio >= -90:
- lvl_cdmaecio = NetworkStrength.great
- elif cdmaecio >= -110:
- lvl_cdmaecio = NetworkStrength.good
- elif cdmaecio >= -130:
- lvl_cdmaecio = NetworkStrength.moderate
- elif cdmaecio >= -150:
- lvl_cdmaecio = NetworkStrength.poor
- return max(lvl_cdmadbm, lvl_cdmaecio)
-
- if network_type == NetworkType.none:
- return network_strength
- if network_type == NetworkType.wifi:
- out = subprocess.check_output('dumpsys connectivity', shell=True).decode('utf-8')
- network_strength = NetworkStrength.unknown
- for line in out.split('\n'):
- signal_str = "SignalStrength: "
- if signal_str in line:
- lvl_idx_start = line.find(signal_str) + len(signal_str)
- lvl_idx_end = line.find(']', lvl_idx_start)
- lvl = int(line[lvl_idx_start : lvl_idx_end])
- if lvl >= -50:
- network_strength = NetworkStrength.great
- elif lvl >= -60:
- network_strength = NetworkStrength.good
- elif lvl >= -70:
- network_strength = NetworkStrength.moderate
- else:
- network_strength = NetworkStrength.poor
- return network_strength
- else:
- # check cell strength
- out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('utf-8')
- for line in out.split('\n'):
- if "mSignalStrength" in line:
- arr = line.split(' ')
- ns = 0
- if ("gsm" in arr[14]):
- rsrp = int(arr[9])
- rssnr = int(arr[11])
- ns = get_lte_level(rsrp, rssnr)
- if ns == NetworkStrength.unknown:
- tdscmadbm = int(arr[13])
- ns = get_tdscdma_level(tdscmadbm)
- if ns == NetworkStrength.unknown:
- asu = int(arr[1])
- ns = get_gsm_level(asu)
- else:
- cdmadbm = int(arr[3])
- cdmaecio = int(arr[4])
- evdodbm = int(arr[5])
- evdosnr = int(arr[7])
- lvl_cdma = get_cdma_level(cdmadbm, cdmaecio)
- lvl_edmo = get_evdo_level(evdodbm, evdosnr)
- if lvl_edmo == NetworkStrength.unknown:
- ns = lvl_cdma
- elif lvl_cdma == NetworkStrength.unknown:
- ns = lvl_edmo
- else:
- ns = min(lvl_cdma, lvl_edmo)
- network_strength = max(network_strength, ns)
-
- return network_strength
diff --git a/common/basedir.py b/common/basedir.py
index 4d62fdc19..d98509ed6 100644
--- a/common/basedir.py
+++ b/common/basedir.py
@@ -1,10 +1,10 @@
import os
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
-from common.android import ANDROID
-if ANDROID:
- PERSIST = "/persist"
- PARAMS = "/data/params"
-else:
+from common.hardware import PC
+if PC:
PERSIST = os.path.join(BASEDIR, "persist")
PARAMS = os.path.join(BASEDIR, "persist", "params")
+else:
+ PERSIST = "/persist"
+ PARAMS = "/data/params"
diff --git a/common/gpio.py b/common/gpio.py
new file mode 100644
index 000000000..73603f262
--- /dev/null
+++ b/common/gpio.py
@@ -0,0 +1,22 @@
+GPIO_HUB_RST_N = 30
+GPIO_UBLOX_RST_N = 32
+GPIO_UBLOX_SAFEBOOT_N = 33
+GPIO_UBLOX_PWR_EN = 34
+GPIO_STM_RST_N = 124
+GPIO_STM_BOOT0 = 134
+
+
+def gpio_init(pin, output):
+ try:
+ with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f:
+ f.write(b"out" if output else b"in")
+ except Exception as e:
+ print(f"Failed to set gpio {pin} direction: {e}")
+
+
+def gpio_set(pin, high):
+ try:
+ with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f:
+ f.write(b"1" if high else b"0")
+ except Exception as e:
+ print(f"Failed to set gpio {pin} value: {e}")
diff --git a/common/hardware.py b/common/hardware.py
new file mode 100644
index 000000000..67d0c44d1
--- /dev/null
+++ b/common/hardware.py
@@ -0,0 +1,57 @@
+import os
+import random
+from typing import cast
+
+from cereal import log
+from common.hardware_android import Android
+from common.hardware_tici import Tici
+from common.hardware_base import HardwareBase
+
+EON = os.path.isfile('/EON')
+TICI = os.path.isfile('/TICI')
+PC = not (EON or TICI)
+ANDROID = EON
+
+
+NetworkType = log.ThermalData.NetworkType
+NetworkStrength = log.ThermalData.NetworkStrength
+
+
+class Pc(HardwareBase):
+ def get_sound_card_online(self):
+ return True
+
+ def get_imei(self, slot):
+ return "%015d" % random.randint(0, 1 << 32)
+
+ def get_serial(self):
+ return "cccccccc"
+
+ def get_subscriber_info(self):
+ return ""
+
+ def reboot(self, reason=None):
+ print("REBOOT!")
+
+ def get_network_type(self):
+ return NetworkType.wifi
+
+ def get_sim_info(self):
+ return {
+ 'sim_id': '',
+ 'mcc_mnc': None,
+ 'network_type': ["Unknown"],
+ 'sim_state': ["ABSENT"],
+ 'data_connected': False
+ }
+
+ def get_network_strength(self, network_type):
+ return NetworkStrength.unknown
+
+
+if EON:
+ HARDWARE = cast(HardwareBase, Android())
+elif TICI:
+ HARDWARE = cast(HardwareBase, Tici())
+else:
+ HARDWARE = cast(HardwareBase, Pc())
diff --git a/common/hardware_android.py b/common/hardware_android.py
new file mode 100644
index 000000000..91e1d1423
--- /dev/null
+++ b/common/hardware_android.py
@@ -0,0 +1,302 @@
+import binascii
+import itertools
+import os
+import re
+import struct
+import subprocess
+
+from cereal import log
+from common.hardware_base import HardwareBase
+
+NetworkType = log.ThermalData.NetworkType
+NetworkStrength = log.ThermalData.NetworkStrength
+
+
+def service_call(call):
+ try:
+ ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip()
+ if 'Parcel' not in ret:
+ return None
+ return parse_service_call_bytes(ret)
+ except subprocess.CalledProcessError:
+ return None
+
+
+def parse_service_call_unpack(r, fmt):
+ try:
+ return struct.unpack(fmt, r)[0]
+ except Exception:
+ return None
+
+
+def parse_service_call_string(r):
+ try:
+ r = r[8:] # Cut off length field
+ r = r.decode('utf_16_be')
+
+ # All pairs of two characters seem to be swapped. Not sure why
+ result = ""
+ for a, b, in itertools.zip_longest(r[::2], r[1::2], fillvalue='\x00'):
+ result += b + a
+
+ result = result.replace('\x00', '')
+
+ return result
+ except Exception:
+ return None
+
+
+def parse_service_call_bytes(ret):
+ try:
+ r = b""
+ for hex_part in re.findall(r'[ (]([0-9a-f]{8})', ret):
+ r += binascii.unhexlify(hex_part)
+ return r
+ except Exception:
+ return None
+
+
+def getprop(key):
+ return subprocess.check_output(["getprop", key], encoding='utf8').strip()
+
+
+class Android(HardwareBase):
+ def get_sound_card_online(self):
+ return (os.path.isfile('/proc/asound/card0/state') and
+ open('/proc/asound/card0/state').read().strip() == 'ONLINE')
+
+ def get_imei(self, slot):
+ slot = str(slot)
+ if slot not in ("0", "1"):
+ raise ValueError("SIM slot must be 0 or 1")
+
+ return parse_service_call_string(service_call(["iphonesubinfo", "3", "i32", str(slot)]))
+
+ def get_serial(self):
+ ret = getprop("ro.serialno")
+ if ret == "":
+ ret = "cccccccc"
+ return ret
+
+ def get_subscriber_info(self):
+ ret = parse_service_call_string(service_call(["iphonesubinfo", "7"]))
+ if ret is None or len(ret) < 8:
+ return ""
+ return ret
+
+ def reboot(self, reason=None):
+ # e.g. reason="recovery" to go into recover mode
+ if reason is None:
+ reason_args = ["null"]
+ else:
+ reason_args = ["s16", reason]
+
+ subprocess.check_output([
+ "service", "call", "power", "16", # IPowerManager.reboot
+ "i32", "0", # no confirmation,
+ *reason_args,
+ "i32", "1" # wait
+ ])
+
+ def get_sim_info(self):
+ # Used for athena
+ # TODO: build using methods from this class
+ sim_state = getprop("gsm.sim.state").split(",")
+ network_type = getprop("gsm.network.type").split(',')
+ mcc_mnc = getprop("gsm.sim.operator.numeric") or None
+
+ sim_id = parse_service_call_string(service_call(['iphonesubinfo', '11']))
+ cell_data_state = parse_service_call_unpack(service_call(['phone', '46']), ">q")
+ cell_data_connected = (cell_data_state == 2)
+
+ return {
+ 'sim_id': sim_id,
+ 'mcc_mnc': mcc_mnc,
+ 'network_type': network_type,
+ 'sim_state': sim_state,
+ 'data_connected': cell_data_connected
+ }
+
+ def get_network_type(self):
+ wifi_check = parse_service_call_string(service_call(["connectivity", "2"]))
+ if wifi_check is None:
+ return NetworkType.none
+ elif 'WIFI' in wifi_check:
+ return NetworkType.wifi
+ else:
+ cell_check = parse_service_call_unpack(service_call(['phone', '59']), ">q")
+ # from TelephonyManager.java
+ cell_networks = {
+ 0: NetworkType.none,
+ 1: NetworkType.cell2G,
+ 2: NetworkType.cell2G,
+ 3: NetworkType.cell3G,
+ 4: NetworkType.cell2G,
+ 5: NetworkType.cell3G,
+ 6: NetworkType.cell3G,
+ 7: NetworkType.cell3G,
+ 8: NetworkType.cell3G,
+ 9: NetworkType.cell3G,
+ 10: NetworkType.cell3G,
+ 11: NetworkType.cell2G,
+ 12: NetworkType.cell3G,
+ 13: NetworkType.cell4G,
+ 14: NetworkType.cell4G,
+ 15: NetworkType.cell3G,
+ 16: NetworkType.cell2G,
+ 17: NetworkType.cell3G,
+ 18: NetworkType.cell4G,
+ 19: NetworkType.cell4G
+ }
+ return cell_networks.get(cell_check, NetworkType.none)
+
+ def get_network_strength(self, network_type):
+ network_strength = NetworkStrength.unknown
+
+ # from SignalStrength.java
+ def get_lte_level(rsrp, rssnr):
+ INT_MAX = 2147483647
+ if rsrp == INT_MAX:
+ lvl_rsrp = NetworkStrength.unknown
+ elif rsrp >= -95:
+ lvl_rsrp = NetworkStrength.great
+ elif rsrp >= -105:
+ lvl_rsrp = NetworkStrength.good
+ elif rsrp >= -115:
+ lvl_rsrp = NetworkStrength.moderate
+ else:
+ lvl_rsrp = NetworkStrength.poor
+ if rssnr == INT_MAX:
+ lvl_rssnr = NetworkStrength.unknown
+ elif rssnr >= 45:
+ lvl_rssnr = NetworkStrength.great
+ elif rssnr >= 10:
+ lvl_rssnr = NetworkStrength.good
+ elif rssnr >= -30:
+ lvl_rssnr = NetworkStrength.moderate
+ else:
+ lvl_rssnr = NetworkStrength.poor
+ return max(lvl_rsrp, lvl_rssnr)
+
+ def get_tdscdma_level(tdscmadbm):
+ lvl = NetworkStrength.unknown
+ if tdscmadbm > -25:
+ lvl = NetworkStrength.unknown
+ elif tdscmadbm >= -49:
+ lvl = NetworkStrength.great
+ elif tdscmadbm >= -73:
+ lvl = NetworkStrength.good
+ elif tdscmadbm >= -97:
+ lvl = NetworkStrength.moderate
+ elif tdscmadbm >= -110:
+ lvl = NetworkStrength.poor
+ return lvl
+
+ def get_gsm_level(asu):
+ if asu <= 2 or asu == 99:
+ lvl = NetworkStrength.unknown
+ elif asu >= 12:
+ lvl = NetworkStrength.great
+ elif asu >= 8:
+ lvl = NetworkStrength.good
+ elif asu >= 5:
+ lvl = NetworkStrength.moderate
+ else:
+ lvl = NetworkStrength.poor
+ return lvl
+
+ def get_evdo_level(evdodbm, evdosnr):
+ lvl_evdodbm = NetworkStrength.unknown
+ lvl_evdosnr = NetworkStrength.unknown
+ if evdodbm >= -65:
+ lvl_evdodbm = NetworkStrength.great
+ elif evdodbm >= -75:
+ lvl_evdodbm = NetworkStrength.good
+ elif evdodbm >= -90:
+ lvl_evdodbm = NetworkStrength.moderate
+ elif evdodbm >= -105:
+ lvl_evdodbm = NetworkStrength.poor
+ if evdosnr >= 7:
+ lvl_evdosnr = NetworkStrength.great
+ elif evdosnr >= 5:
+ lvl_evdosnr = NetworkStrength.good
+ elif evdosnr >= 3:
+ lvl_evdosnr = NetworkStrength.moderate
+ elif evdosnr >= 1:
+ lvl_evdosnr = NetworkStrength.poor
+ return max(lvl_evdodbm, lvl_evdosnr)
+
+ def get_cdma_level(cdmadbm, cdmaecio):
+ lvl_cdmadbm = NetworkStrength.unknown
+ lvl_cdmaecio = NetworkStrength.unknown
+ if cdmadbm >= -75:
+ lvl_cdmadbm = NetworkStrength.great
+ elif cdmadbm >= -85:
+ lvl_cdmadbm = NetworkStrength.good
+ elif cdmadbm >= -95:
+ lvl_cdmadbm = NetworkStrength.moderate
+ elif cdmadbm >= -100:
+ lvl_cdmadbm = NetworkStrength.poor
+ if cdmaecio >= -90:
+ lvl_cdmaecio = NetworkStrength.great
+ elif cdmaecio >= -110:
+ lvl_cdmaecio = NetworkStrength.good
+ elif cdmaecio >= -130:
+ lvl_cdmaecio = NetworkStrength.moderate
+ elif cdmaecio >= -150:
+ lvl_cdmaecio = NetworkStrength.poor
+ return max(lvl_cdmadbm, lvl_cdmaecio)
+
+ if network_type == NetworkType.none:
+ return network_strength
+ if network_type == NetworkType.wifi:
+ out = subprocess.check_output('dumpsys connectivity', shell=True).decode('utf-8')
+ network_strength = NetworkStrength.unknown
+ for line in out.split('\n'):
+ signal_str = "SignalStrength: "
+ if signal_str in line:
+ lvl_idx_start = line.find(signal_str) + len(signal_str)
+ lvl_idx_end = line.find(']', lvl_idx_start)
+ lvl = int(line[lvl_idx_start : lvl_idx_end])
+ if lvl >= -50:
+ network_strength = NetworkStrength.great
+ elif lvl >= -60:
+ network_strength = NetworkStrength.good
+ elif lvl >= -70:
+ network_strength = NetworkStrength.moderate
+ else:
+ network_strength = NetworkStrength.poor
+ return network_strength
+ else:
+ # check cell strength
+ out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('utf-8')
+ for line in out.split('\n'):
+ if "mSignalStrength" in line:
+ arr = line.split(' ')
+ ns = 0
+ if ("gsm" in arr[14]):
+ rsrp = int(arr[9])
+ rssnr = int(arr[11])
+ ns = get_lte_level(rsrp, rssnr)
+ if ns == NetworkStrength.unknown:
+ tdscmadbm = int(arr[13])
+ ns = get_tdscdma_level(tdscmadbm)
+ if ns == NetworkStrength.unknown:
+ asu = int(arr[1])
+ ns = get_gsm_level(asu)
+ else:
+ cdmadbm = int(arr[3])
+ cdmaecio = int(arr[4])
+ evdodbm = int(arr[5])
+ evdosnr = int(arr[7])
+ lvl_cdma = get_cdma_level(cdmadbm, cdmaecio)
+ lvl_edmo = get_evdo_level(evdodbm, evdosnr)
+ if lvl_edmo == NetworkStrength.unknown:
+ ns = lvl_cdma
+ elif lvl_cdma == NetworkStrength.unknown:
+ ns = lvl_edmo
+ else:
+ ns = min(lvl_cdma, lvl_edmo)
+ network_strength = max(network_strength, ns)
+
+ return network_strength
diff --git a/common/hardware_base.py b/common/hardware_base.py
new file mode 100644
index 000000000..24bb52a5e
--- /dev/null
+++ b/common/hardware_base.py
@@ -0,0 +1,41 @@
+from abc import abstractmethod
+
+
+class HardwareBase:
+ @staticmethod
+ def get_cmdline():
+ with open('/proc/cmdline') as f:
+ cmdline = f.read()
+ return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2}
+
+ @abstractmethod
+ def get_sound_card_online(self):
+ pass
+
+ @abstractmethod
+ def get_imei(self, slot):
+ pass
+
+ @abstractmethod
+ def get_serial(self):
+ pass
+
+ @abstractmethod
+ def get_subscriber_info(self):
+ pass
+
+ @abstractmethod
+ def reboot(self, reason=None):
+ pass
+
+ @abstractmethod
+ def get_network_type(self):
+ pass
+
+ @abstractmethod
+ def get_sim_info(self):
+ pass
+
+ @abstractmethod
+ def get_network_strength(self, network_type):
+ pass
diff --git a/common/hardware_tici.py b/common/hardware_tici.py
new file mode 100644
index 000000000..223bfc1cc
--- /dev/null
+++ b/common/hardware_tici.py
@@ -0,0 +1,58 @@
+import serial
+
+from common.hardware_base import HardwareBase
+from cereal import log
+
+
+NetworkType = log.ThermalData.NetworkType
+NetworkStrength = log.ThermalData.NetworkStrength
+
+
+def run_at_command(cmd, timeout=0.1):
+ with serial.Serial("/dev/ttyUSB2", timeout=timeout) as ser:
+ ser.write(cmd + b"\r\n")
+ ser.readline() # Modem echos request
+ return ser.readline().decode().rstrip()
+
+
+class Tici(HardwareBase):
+ def get_sound_card_online(self):
+ return True
+
+ def get_imei(self, slot):
+ if slot != 0:
+ return ""
+
+ for _ in range(10):
+ try:
+ imei = run_at_command(b"AT+CGSN")
+ if len(imei) == 15:
+ return imei
+ except serial.SerialException:
+ pass
+
+ raise RuntimeError("Error getting IMEI")
+
+ def get_serial(self):
+ return self.get_cmdline()['androidboot.serialno']
+
+ def get_subscriber_info(self):
+ return ""
+
+ def reboot(self, reason=None):
+ print("REBOOT!")
+
+ def get_network_type(self):
+ return NetworkType.wifi
+
+ def get_sim_info(self):
+ return {
+ 'sim_id': '',
+ 'mcc_mnc': None,
+ 'network_type': ["Unknown"],
+ 'sim_state': ["ABSENT"],
+ 'data_connected': False
+ }
+
+ def get_network_strength(self, network_type):
+ return NetworkStrength.unknown
diff --git a/common/params.py b/common/params.py
index e10670502..d30f2d0df 100755
--- a/common/params.py
+++ b/common/params.py
@@ -53,12 +53,12 @@ keys = {
"AccessToken": [TxType.CLEAR_ON_MANAGER_START],
"AthenadPid": [TxType.PERSISTENT],
"CalibrationParams": [TxType.PERSISTENT],
+ "CarBatteryCapacity": [TxType.PERSISTENT],
"CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"CommunityFeaturesToggle": [TxType.PERSISTENT],
"CompletedTrainingVersion": [TxType.PERSISTENT],
- "ControlsParams": [TxType.PERSISTENT],
"DisablePowerDown": [TxType.PERSISTENT],
"DisableUpdates": [TxType.PERSISTENT],
"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
@@ -71,7 +71,6 @@ keys = {
"HasCompletedSetup": [TxType.PERSISTENT],
"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START],
"IsLdwEnabled": [TxType.PERSISTENT],
- "IsGeofenceEnabled": [TxType.PERSISTENT],
"IsMetric": [TxType.PERSISTENT],
"IsOffroad": [TxType.CLEAR_ON_MANAGER_START],
"IsRHD": [TxType.PERSISTENT],
@@ -81,10 +80,7 @@ keys = {
"LastAthenaPingTime": [TxType.PERSISTENT],
"LastUpdateTime": [TxType.PERSISTENT],
"LastUpdateException": [TxType.PERSISTENT],
- "LimitSetSpeed": [TxType.PERSISTENT],
- "LimitSetSpeedNeural": [TxType.PERSISTENT],
"LiveParameters": [TxType.PERSISTENT],
- "LongitudinalControl": [TxType.PERSISTENT],
"OpenpilotEnabledToggle": [TxType.PERSISTENT],
"LaneChangeEnabled": [TxType.PERSISTENT],
"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
@@ -94,7 +90,6 @@ keys = {
"RecordFront": [TxType.PERSISTENT],
"ReleaseNotes": [TxType.PERSISTENT],
"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START],
- "SpeedLimitOffset": [TxType.PERSISTENT],
"SubscriberInfo": [TxType.PERSISTENT],
"TermsVersion": [TxType.PERSISTENT],
"TrainingVersion": [TxType.PERSISTENT],
@@ -122,14 +117,15 @@ def fsync_dir(path):
class FileLock():
- def __init__(self, path, create):
+ def __init__(self, path, create, lock_ex):
self._path = path
self._create = create
self._fd = None
+ self._lock_ex = lock_ex
def acquire(self):
self._fd = os.open(self._path, os.O_CREAT if self._create else 0)
- fcntl.flock(self._fd, fcntl.LOCK_EX)
+ fcntl.flock(self._fd, fcntl.LOCK_EX if self._lock_ex else fcntl.LOCK_SH)
def release(self):
if self._fd is not None:
@@ -157,8 +153,8 @@ class DBAccessor():
except KeyError:
return None
- def _get_lock(self, create):
- lock = FileLock(os.path.join(self._path, ".lock"), create)
+ def _get_lock(self, create, lock_ex):
+ lock = FileLock(os.path.join(self._path, ".lock"), create, lock_ex)
lock.acquire()
return lock
@@ -190,7 +186,7 @@ class DBAccessor():
class DBReader(DBAccessor):
def __enter__(self):
try:
- lock = self._get_lock(False)
+ lock = self._get_lock(False, False)
except OSError as e:
# Do not create lock if it does not exist.
if e.errno == errno.ENOENT:
@@ -228,7 +224,7 @@ class DBWriter(DBAccessor):
try:
os.chmod(self._path, 0o777)
- self._lock = self._get_lock(True)
+ self._lock = self._get_lock(True, True)
self._vals = self._read_values_locked()
except Exception:
os.umask(self._prev_umask)
@@ -317,7 +313,7 @@ def write_db(params_path, key, value):
value = value.encode('utf8')
prev_umask = os.umask(0)
- lock = FileLock(params_path + "/.lock", True)
+ lock = FileLock(params_path + "/.lock", True, True)
lock.acquire()
try:
diff --git a/common/realtime.py b/common/realtime.py
index e73443864..7f4ad5c0d 100644
--- a/common/realtime.py
+++ b/common/realtime.py
@@ -1,12 +1,10 @@
"""Utilities for reading real time clocks and keeping soft real time constraints."""
+import gc
import os
import time
-import platform
-import subprocess
import multiprocessing
-from cffi import FFI
-from common.android import ANDROID
+from common.hardware import PC
from common.common_pyx import sec_since_boot # pylint: disable=no-name-in-module, import-error
@@ -17,37 +15,26 @@ DT_DMON = 0.1 # driver monitoring
DT_TRML = 0.5 # thermald and manager
-ffi = FFI()
-ffi.cdef("long syscall(long number, ...);")
-libc = ffi.dlopen(None)
-
-def _get_tid():
- if platform.machine() == "x86_64":
- NR_gettid = 186
- elif platform.machine() == "aarch64":
- NR_gettid = 178
- else:
- raise NotImplementedError
-
- return libc.syscall(NR_gettid)
+class Priority:
+ MIN_REALTIME = 52 # highest android process priority is 51
+ CTRL_LOW = MIN_REALTIME
+ CTRL_HIGH = MIN_REALTIME + 1
def set_realtime_priority(level):
- if os.getuid() != 0:
- print("not setting priority, not root")
- return
+ if not PC:
+ os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))
- return subprocess.call(['chrt', '-f', '-p', str(level), str(_get_tid())])
def set_core_affinity(core):
- if os.getuid() != 0:
- print("not setting affinity, not root")
- return
+ if not PC:
+ os.sched_setaffinity(0, [core,])
- if ANDROID:
- return subprocess.call(['taskset', '-p', str(core), str(_get_tid())])
- else:
- return -1
+
+def config_rt_process(core, priority):
+ gc.disable()
+ set_realtime_priority(priority)
+ set_core_affinity(core)
class Ratekeeper():
diff --git a/common/text_window.py b/common/text_window.py
index b815d8022..1aa3b885e 100755
--- a/common/text_window.py
+++ b/common/text_window.py
@@ -5,7 +5,7 @@ import subprocess
from common.basedir import BASEDIR
-class TextWindow():
+class TextWindow:
def __init__(self, s, noop=False):
# text window is only implemented for android currently
self.text_proc = None
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index af5483562..7124ef3c4 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -8,6 +8,60 @@ source "$BASEDIR/launch_env.sh"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
+function two_init {
+ # Restrict Android and other system processes to the first two cores
+ echo 0-1 > /dev/cpuset/background/cpus
+ echo 0-1 > /dev/cpuset/system-background/cpus
+ echo 0-1 > /dev/cpuset/foreground/boost/cpus
+ echo 0-1 > /dev/cpuset/foreground/cpus
+ echo 0-1 > /dev/cpuset/android/cpus
+
+ # openpilot gets all the cores
+ echo 0-3 > /dev/cpuset/app/cpus
+
+ # Collect RIL and other possibly long-running I/O interrupts onto CPU 1
+ echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio)
+ echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage)
+ echo 1 > /proc/irq/35/smp_affinity_list # wifi (wlan_pci)
+ # USB traffic needs realtime handling on cpu 3
+ [ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco
+ [ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T
+
+
+ # Check for NEOS update
+ if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then
+ if [ -f "$DIR/scripts/continue.sh" ]; then
+ cp "$DIR/scripts/continue.sh" "/data/data/com.termux/files/continue.sh"
+ fi
+
+ if [ ! -f "$BASEDIR/prebuilt" ]; then
+ # Clean old build products, but preserve the scons cache
+ cd $DIR
+ scons --clean
+ git clean -xdf
+ git submodule foreach --recursive git clean -xdf
+ fi
+
+ "$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json"
+ else
+ if [[ $(uname -v) == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020" ]]; then
+ "$DIR/installer/updater/updater" "file://$DIR/installer/updater/update_kernel.json"
+ fi
+ 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 launch {
# Wifi scan
wpa_cli IFNAME=wlan0 SCAN
@@ -54,56 +108,9 @@ function launch {
fi
fi
- # Android and other system processes are not permitted to run on CPU 3
- # NEOS installed app processes can run anywhere
- echo 0-2 > /dev/cpuset/background/cpus
- echo 0-2 > /dev/cpuset/system-background/cpus
- [ -d "/dev/cpuset/foreground/boost/cpus" ] && echo 0-2 > /dev/cpuset/foreground/boost/cpus # Not present in < NEOS 15
- echo 0-2 > /dev/cpuset/foreground/cpus
- echo 0-2 > /dev/cpuset/android/cpus
- echo 0-3 > /dev/cpuset/app/cpus
-
- # Collect RIL and other possibly long-running I/O interrupts onto CPU 1
- echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio)
- echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage)
- echo 1 > /proc/irq/35/smp_affinity_list # wifi (wlan_pci)
- # USB traffic needs realtime handling on cpu 3
- [ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco
- [ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T
-
-
- # Check for NEOS update
- if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then
- if [ -f "$DIR/scripts/continue.sh" ]; then
- cp "$DIR/scripts/continue.sh" "/data/data/com.termux/files/continue.sh"
- fi
-
- if [ ! -f "$BASEDIR/prebuilt" ]; then
- # Clean old build products, but preserve the scons cache
- cd $DIR
- scons --clean
- git clean -xdf
- git submodule foreach --recursive git clean -xdf
- fi
-
- "$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json"
- else
- if [[ $(uname -v) == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020" ]]; then
- "$DIR/installer/updater/updater" "file://$DIR/installer/updater/update_kernel.json"
- fi
- 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.
- # TODO: we should really grow per-platform detect and setup routines
- 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
+ # comma two init
+ if [ -f /EON ]; then
+ two_init
fi
# handle pythonpath
diff --git a/opendbc/can/common_pyx_setup.py b/opendbc/can/common_pyx_setup.py
index 72a0cde7c..04f95359a 100644
--- a/opendbc/can/common_pyx_setup.py
+++ b/opendbc/can/common_pyx_setup.py
@@ -34,7 +34,7 @@ class BuildExtWithoutPlatformSuffix(build_ext):
return get_ext_filename_without_platform_suffix(filename)
-extra_compile_args = ["-std=c++14"]
+extra_compile_args = ["-std=c++14", "-Wno-nullability-completeness"]
ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg
if ARCH == "aarch64":
extra_compile_args += ["-Wno-deprecated-register"]
diff --git a/opendbc/can/packer_pyx.pyx b/opendbc/can/packer_pyx.pyx
index 00f83eca6..14e2f81d3 100644
--- a/opendbc/can/packer_pyx.pyx
+++ b/opendbc/can/packer_pyx.pyx
@@ -20,9 +20,11 @@ cdef class CANPacker:
map[int, int] address_to_size
def __init__(self, dbc_name):
- self.packer = new cpp_CANPacker(dbc_name)
self.dbc = dbc_lookup(dbc_name)
-
+ if not self.dbc:
+ raise RuntimeError("Can't lookup" + dbc_name)
+
+ self.packer = new cpp_CANPacker(dbc_name)
num_msgs = self.dbc[0].num_msgs
for i in range(num_msgs):
msg = self.dbc[0].msgs[i]
@@ -37,7 +39,7 @@ cdef class CANPacker:
for name, value in values.iteritems():
n = name.encode('utf8')
- names.append(n) # TODO: find better way to keep reference to temp string arround
+ names.append(n) # TODO: find better way to keep reference to temp string around
spv.name = n
spv.value = value
diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc
index 5addd4168..c6d85d9ee 100644
--- a/opendbc/can/parser.cc
+++ b/opendbc/can/parser.cc
@@ -187,7 +187,7 @@ void CANParser::UpdateCans(uint64_t sec, const capnp::List::Rea
continue;
}
- if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
+ if (cmsg.getDat().size() > 8) continue; //shouldn't ever happen
uint8_t dat[8] = {0};
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
diff --git a/opendbc/can/parser.py b/opendbc/can/parser.py
index 107a839b1..04722115c 100644
--- a/opendbc/can/parser.py
+++ b/opendbc/can/parser.py
@@ -1,2 +1,2 @@
-from opendbc.can.parser_pyx import CANParser # pylint: disable=no-name-in-module, import-error
-assert CANParser
+from opendbc.can.parser_pyx import CANParser, CANDefine # pylint: disable=no-name-in-module, import-error
+assert CANParser, CANDefine
diff --git a/opendbc/can/parser_pyx.pyx b/opendbc/can/parser_pyx.pyx
index bd5adf6ff..913f58230 100644
--- a/opendbc/can/parser_pyx.pyx
+++ b/opendbc/can/parser_pyx.pyx
@@ -17,7 +17,6 @@ from collections import defaultdict
cdef int CAN_INVALID_CNT = 5
-
cdef class CANParser:
cdef:
cpp_CANParser *can
@@ -37,10 +36,11 @@ cdef class CANParser:
def __init__(self, dbc_name, signals, checks=None, bus=0):
if checks is None:
checks = []
-
self.can_valid = True
self.dbc_name = dbc_name
self.dbc = dbc_lookup(dbc_name)
+ if not self.dbc:
+ raise RuntimeError("Can't lookup" + dbc_name)
self.vl = {}
self.ts = {}
@@ -110,7 +110,7 @@ cdef class CANParser:
for cv in can_values:
- # Cast char * directly to unicde
+ # Cast char * directly to unicode
name = self.address_to_msg_name[cv.address].c_str()
cv_name = cv.name
@@ -148,7 +148,9 @@ cdef class CANDefine():
def __init__(self, dbc_name):
self.dbc_name = dbc_name
self.dbc = dbc_lookup(dbc_name)
-
+ if not self.dbc:
+ raise RuntimeError("Can't lookup" + dbc_name)
+
num_vals = self.dbc[0].num_vals
address_to_msg_name = {}
diff --git a/opendbc/can/process_dbc.py b/opendbc/can/process_dbc.py
index 5564021af..9025331dd 100755
--- a/opendbc/can/process_dbc.py
+++ b/opendbc/can/process_dbc.py
@@ -81,7 +81,7 @@ def process(in_fn, out_fn):
if sig.start_bit % 8 != checksum_start_bit:
sys.exit("%s: CHECKSUM starts at wrong bit" % dbc_msg_name)
if little_endian != sig.is_little_endian:
- sys.exit("%s: CHECKSUM has wrong endianess" % dbc_msg_name)
+ sys.exit("%s: CHECKSUM has wrong endianness" % dbc_msg_name)
# counter rules
if sig.name == "COUNTER":
if counter_size is not None and sig.size != counter_size:
@@ -90,7 +90,7 @@ def process(in_fn, out_fn):
print(counter_start_bit, sig.start_bit)
sys.exit("%s: COUNTER starts at wrong bit" % dbc_msg_name)
if little_endian != sig.is_little_endian:
- sys.exit("%s: COUNTER has wrong endianess" % dbc_msg_name)
+ sys.exit("%s: COUNTER has wrong endianness" % dbc_msg_name)
# pedal rules
if address in [0x200, 0x201]:
if sig.name == "COUNTER_PEDAL" and sig.size != 4:
diff --git a/opendbc/chrysler_pacifica_2017_hybrid.dbc b/opendbc/chrysler_pacifica_2017_hybrid.dbc
index 4bd919655..54e4819f2 100644
--- a/opendbc/chrysler_pacifica_2017_hybrid.dbc
+++ b/opendbc/chrysler_pacifica_2017_hybrid.dbc
@@ -409,7 +409,7 @@ CM_ SG_ 571 CHECKSUM "standard checksum";
CM_ SG_ 825 BEEP_339 "sent every 0.5s. 0050 is no beep. To beep send 4355 or 4155. Used by ParkSense warning.";
CM_ SG_ 270 ELECTRIC_MOTOR "0x7fff indicates electric motor not in use";
CM_ SG_ 291 ENERGY_GAIN_LOSS "unsure what this actually is";
-CM_ SG_ 291 ENERGY_SMOOTHER_CURVE "unusre what it is, but smoother";
+CM_ SG_ 291 ENERGY_SMOOTHER_CURVE "unsure what it is, but smoother";
CM_ SG_ 308 ACCEL_134 "only set when human presses accel pedal";
CM_ SG_ 532 NOISY_SLOWLY_DECREASING "perhaps battery but do not know";
CM_ SG_ 816 TRACTION_OFF "set when traction off button is enabled";
diff --git a/opendbc/gm_global_a_object.dbc b/opendbc/gm_global_a_object.dbc
index 1532e90da..4643a640a 100644
--- a/opendbc/gm_global_a_object.dbc
+++ b/opendbc/gm_global_a_object.dbc
@@ -37,7 +37,7 @@ BU_: K109_FCM B233B_LRR NEO VIS_FO VIS2_FO K124_ASCM Vector__XXX EOCM_F_FO EOCM2
VAL_TABLE_ RangeMode 1 "Active" 0 "Inactive" ;
VAL_TABLE_ TrkConf 3 "Confident" 2 "Speculative" 1 "Highly speculative" 0 "Invalid" ;
VAL_TABLE_ TrkMeasStatus 3 "Measured current cycle" 2 "Latent track not detected" 1 "New object" 0 "No object" ;
-VAL_TABLE_ TrkDynProp 4 "Moving in opposite direction" 3 "Moving in same direction" 2 "Has moved but currenty stopped" 1 "Has never moved," 0 "Unkown" ;
+VAL_TABLE_ TrkDynProp 4 "Moving in opposite direction" 3 "Moving in same direction" 2 "Has moved but currently stopped" 1 "Has never moved," 0 "Unknown" ;
VAL_TABLE_ FrntVsnInPthVehBrkNwSt 10 "Active" 5 "Inactive" ;
VAL_TABLE_ FrntVsnClostPedBrkNwSt 10 "Active" 5 "Inactive" ;
VAL_TABLE_ LaneSnsLLnPosValid 1 "Invalid" 0 "Valid" ;
diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc
index 72ce7b1c6..6cc7eba41 100644
--- a/opendbc/lexus_ct200h_2018_pt_generated.dbc
+++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc
index c08b1fee6..e1e009ef0 100644
--- a/opendbc/lexus_is_2018_pt_generated.dbc
+++ b/opendbc/lexus_is_2018_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc
index 278bb2a2f..7e422ad2a 100644
--- a/opendbc/lexus_nx300h_2018_pt_generated.dbc
+++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc
index 55b268be8..c8bb8c387 100644
--- a/opendbc/lexus_rx_350_2016_pt_generated.dbc
+++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
index c353ab475..4fe9f00aa 100644
--- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
+++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc
index d6f546e3b..295bb102f 100644
--- a/opendbc/toyota_avalon_2017_pt_generated.dbc
+++ b/opendbc/toyota_avalon_2017_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
index 393a47828..56fb32ff6 100644
--- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc
index 70be61d02..7ad41ef3a 100644
--- a/opendbc/toyota_corolla_2017_pt_generated.dbc
+++ b/opendbc/toyota_corolla_2017_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
@@ -375,7 +375,7 @@ BO_ 705 GAS_PEDAL: 8 XXX
SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX
BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
- SG_ STEER_TORQUE_EPS : 47|16@0- (1.0,0) [-20000|20000] "" XXX
+ SG_ STEER_TORQUE_EPS : 47|16@0- (0.88,0) [-20000|20000] "" XXX
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc
index 4c7d18386..8db51ffdb 100644
--- a/opendbc/toyota_highlander_2017_pt_generated.dbc
+++ b/opendbc/toyota_highlander_2017_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
index ccaf67791..b9dd821ef 100644
--- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
index 09fe04b34..0569da070 100644
--- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
@@ -328,7 +328,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc
index e6f4a5305..ce38bf50a 100644
--- a/opendbc/toyota_nodsu_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_pt_generated.dbc
@@ -328,7 +328,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc
index 419a88bb9..aef24541c 100644
--- a/opendbc/toyota_prius_2017_pt_generated.dbc
+++ b/opendbc/toyota_prius_2017_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc
index 2b7e38b72..ece2f15ee 100644
--- a/opendbc/toyota_rav4_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_2017_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
index ae1985f6c..4822e1260 100644
--- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
index da5337fef..49735696f 100644
--- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
+++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
@@ -311,7 +311,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
-CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isnt perfect";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
diff --git a/panda/__init__.py b/panda/__init__.py
index deeea46eb..09466ddd5 100644
--- a/panda/__init__.py
+++ b/panda/__init__.py
@@ -1,3 +1,3 @@
# flake8: noqa
# pylint: skip-file
-from .python import Panda, PandaWifiStreaming, PandaDFU, ESPROM, CesantaFlasher, 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, build_st, PandaSerial
diff --git a/panda/board/board.h b/panda/board/board.h
index 84fca5469..19392c9ea 100644
--- a/panda/board/board.h
+++ b/panda/board/board.h
@@ -9,6 +9,7 @@
#ifdef PANDA
#include "drivers/fan.h"
#include "drivers/rtc.h"
+ #include "drivers/clock_source.h"
#include "boards/white.h"
#include "boards/grey.h"
#include "boards/black.h"
@@ -23,7 +24,7 @@ void detect_board_type(void) {
// SPI lines floating: white (TODO: is this reliable? Not really, we have to enable ESP/GPS to be able to detect this on the UART)
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 1);
- if(!detect_with_pull(GPIOB, 1, PULL_UP) && detect_with_pull(GPIOB, 15, PULL_UP)){
+ if(!detect_with_pull(GPIOB, 1, PULL_UP) && !detect_with_pull(GPIOB, 7, PULL_UP)){
hw_type = HW_TYPE_DOS;
current_board = &board_dos;
} else if((detect_with_pull(GPIOA, 4, PULL_DOWN)) || (detect_with_pull(GPIOA, 5, PULL_DOWN)) || (detect_with_pull(GPIOA, 6, PULL_DOWN)) || (detect_with_pull(GPIOA, 7, PULL_DOWN))){
@@ -53,22 +54,10 @@ void detect_board_type(void) {
// ///// Configuration detection ///// //
bool has_external_debug_serial = 0;
-bool is_entering_bootmode = 0;
void detect_configuration(void) {
// detect if external serial debugging is present
has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN);
-
- #ifdef PANDA
- if(hw_type == HW_TYPE_WHITE_PANDA) {
- // check if the ESP is trying to put me in boot mode
- is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP);
- } else {
- is_entering_bootmode = 0;
- }
- #else
- is_entering_bootmode = 0;
- #endif
}
// ///// Board functions ///// //
diff --git a/panda/board/board_declarations.h b/panda/board/board_declarations.h
index d5e9e06a8..963539e00 100644
--- a/panda/board/board_declarations.h
+++ b/panda/board/board_declarations.h
@@ -1,10 +1,10 @@
// ******************** Prototypes ********************
typedef void (*board_init)(void);
-typedef void (*board_enable_can_transciever)(uint8_t transciever, bool enabled);
-typedef void (*board_enable_can_transcievers)(bool enabled);
+typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled);
+typedef void (*board_enable_can_transceivers)(bool enabled);
typedef void (*board_set_led)(uint8_t color, bool enabled);
typedef void (*board_set_usb_power_mode)(uint8_t mode);
-typedef void (*board_set_esp_gps_mode)(uint8_t mode);
+typedef void (*board_set_gps_mode)(uint8_t mode);
typedef void (*board_set_can_mode)(uint8_t mode);
typedef void (*board_usb_power_mode_tick)(uint32_t uptime);
typedef bool (*board_check_ignition)(void);
@@ -12,16 +12,18 @@ typedef uint32_t (*board_read_current)(void);
typedef void (*board_set_ir_power)(uint8_t percentage);
typedef void (*board_set_fan_power)(uint8_t percentage);
typedef void (*board_set_phone_power)(bool enabled);
+typedef void (*board_set_clock_source_mode)(uint8_t mode);
+typedef void (*board_set_siren)(bool enabled);
struct board {
const char *board_type;
const harness_configuration *harness_config;
board_init init;
- board_enable_can_transciever enable_can_transciever;
- board_enable_can_transcievers enable_can_transcievers;
+ board_enable_can_transceiver enable_can_transceiver;
+ board_enable_can_transceivers enable_can_transceivers;
board_set_led set_led;
board_set_usb_power_mode set_usb_power_mode;
- board_set_esp_gps_mode set_esp_gps_mode;
+ board_set_gps_mode set_gps_mode;
board_set_can_mode set_can_mode;
board_usb_power_mode_tick usb_power_mode_tick;
board_check_ignition check_ignition;
@@ -29,6 +31,8 @@ struct board {
board_set_ir_power set_ir_power;
board_set_fan_power set_fan_power;
board_set_phone_power set_phone_power;
+ board_set_clock_source_mode set_clock_source_mode;
+ board_set_siren set_siren;
};
// ******************* Definitions ********************
@@ -52,10 +56,10 @@ struct board {
#define USB_POWER_CDP 2U
#define USB_POWER_DCP 3U
-// ESP modes
-#define ESP_GPS_DISABLED 0U
-#define ESP_GPS_ENABLED 1U
-#define ESP_GPS_BOOTMODE 2U
+// GPS modes
+#define GPS_DISABLED 0U
+#define GPS_ENABLED 1U
+#define GPS_BOOTMODE 2U
// CAN modes
#define CAN_MODE_NORMAL 0U
@@ -72,4 +76,4 @@ bool board_has_gmlan(void);
bool board_has_obd(void);
bool board_has_lin(void);
bool board_has_rtc(void);
-bool board_has_relay(void);
\ No newline at end of file
+bool board_has_relay(void);
diff --git a/panda/board/boards/black.h b/panda/board/boards/black.h
index 57305980c..fa5a542d0 100644
--- a/panda/board/boards/black.h
+++ b/panda/board/boards/black.h
@@ -2,8 +2,8 @@
// Black Panda + Harness //
// ///////////////////// //
-void black_enable_can_transciever(uint8_t transciever, bool enabled) {
- switch (transciever){
+void black_enable_can_transceiver(uint8_t transceiver, bool enabled) {
+ switch (transceiver){
case 1U:
set_gpio_output(GPIOC, 1, !enabled);
break;
@@ -17,18 +17,18 @@ void black_enable_can_transciever(uint8_t transciever, bool enabled) {
set_gpio_output(GPIOB, 10, !enabled);
break;
default:
- puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+ puts("Invalid CAN transceiver ("); puth(transceiver); puts("): enabling failed\n");
break;
}
}
-void black_enable_can_transcievers(bool enabled) {
+void black_enable_can_transceivers(bool enabled) {
for(uint8_t i=1U; i<=4U; i++){
// Leave main CAN always on for CAN-based ignition detection
if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
- black_enable_can_transciever(i, true);
+ black_enable_can_transceiver(i, true);
} else {
- black_enable_can_transciever(i, enabled);
+ black_enable_can_transceiver(i, enabled);
}
}
}
@@ -77,24 +77,24 @@ void black_set_usb_power_mode(uint8_t mode) {
}
}
-void black_set_esp_gps_mode(uint8_t mode) {
+void black_set_gps_mode(uint8_t mode) {
switch (mode) {
- case ESP_GPS_DISABLED:
+ case GPS_DISABLED:
// GPS OFF
set_gpio_output(GPIOC, 14, 0);
set_gpio_output(GPIOC, 5, 0);
break;
- case ESP_GPS_ENABLED:
+ case GPS_ENABLED:
// GPS ON
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 1);
break;
- case ESP_GPS_BOOTMODE:
+ case GPS_BOOTMODE:
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 0);
break;
default:
- puts("Invalid ESP/GPS mode\n");
+ puts("Invalid GPS mode\n");
break;
}
}
@@ -154,6 +154,14 @@ void black_set_phone_power(bool enabled){
UNUSED(enabled);
}
+void black_set_clock_source_mode(uint8_t mode){
+ UNUSED(mode);
+}
+
+void black_set_siren(bool enabled){
+ UNUSED(enabled);
+}
+
void black_init(void) {
common_init_gpio();
@@ -167,7 +175,7 @@ void black_init(void) {
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
// Set default state of GPS
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ current_board->set_gps_mode(GPS_ENABLED);
// C10: OBD_SBU1_RELAY (harness relay driving output)
// C11: OBD_SBU2_RELAY (harness relay driving output)
@@ -190,8 +198,8 @@ void black_init(void) {
// Initialize harness
harness_init();
- // Enable CAN transcievers
- black_enable_can_transcievers(true);
+ // Enable CAN transceivers
+ black_enable_can_transceivers(true);
// Disable LEDs
black_set_led(LED_RED, false);
@@ -228,16 +236,18 @@ const board board_black = {
.board_type = "Black",
.harness_config = &black_harness_config,
.init = black_init,
- .enable_can_transciever = black_enable_can_transciever,
- .enable_can_transcievers = black_enable_can_transcievers,
+ .enable_can_transceiver = black_enable_can_transceiver,
+ .enable_can_transceivers = black_enable_can_transceivers,
.set_led = black_set_led,
.set_usb_power_mode = black_set_usb_power_mode,
- .set_esp_gps_mode = black_set_esp_gps_mode,
+ .set_gps_mode = black_set_gps_mode,
.set_can_mode = black_set_can_mode,
.usb_power_mode_tick = black_usb_power_mode_tick,
.check_ignition = black_check_ignition,
.read_current = black_read_current,
.set_fan_power = black_set_fan_power,
.set_ir_power = black_set_ir_power,
- .set_phone_power = black_set_phone_power
+ .set_phone_power = black_set_phone_power,
+ .set_clock_source_mode = black_set_clock_source_mode,
+ .set_siren = black_set_siren
};
diff --git a/panda/board/boards/common.h b/panda/board/boards/common.h
index 734380962..2f35c34fe 100644
--- a/panda/board/boards/common.h
+++ b/panda/board/boards/common.h
@@ -23,7 +23,7 @@ void common_init_gpio(void){
set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS);
GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
- // A9,A10: USART 1 for talking to the ESP / GPS
+ // A9,A10: USART 1 for talking to the GPS
set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
@@ -65,7 +65,7 @@ void peripherals_init(void){
RCC->APB1ENR |= RCC_APB1ENR_PWREN; // for RTC config
RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
- //RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+ RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // clock source timer
RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
@@ -81,4 +81,4 @@ bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
bool ret = get_gpio_input(GPIO, pin);
set_gpio_pullup(GPIO, pin, PULL_NONE);
return ret;
-}
\ No newline at end of file
+}
diff --git a/panda/board/boards/dos.h b/panda/board/boards/dos.h
index afccd373d..307ae7247 100644
--- a/panda/board/boards/dos.h
+++ b/panda/board/boards/dos.h
@@ -2,8 +2,8 @@
// Dos + Harness //
// ///////////// //
-void dos_enable_can_transciever(uint8_t transciever, bool enabled) {
- switch (transciever){
+void dos_enable_can_transceiver(uint8_t transceiver, bool enabled) {
+ switch (transceiver){
case 1U:
set_gpio_output(GPIOC, 1, !enabled);
break;
@@ -17,18 +17,18 @@ void dos_enable_can_transciever(uint8_t transciever, bool enabled) {
set_gpio_output(GPIOB, 10, !enabled);
break;
default:
- puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+ puts("Invalid CAN transceiver ("); puth(transceiver); puts("): enabling failed\n");
break;
}
}
-void dos_enable_can_transcievers(bool enabled) {
+void dos_enable_can_transceivers(bool enabled) {
for(uint8_t i=1U; i<=4U; i++){
// Leave main CAN always on for CAN-based ignition detection
if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
- uno_enable_can_transciever(i, true);
+ uno_enable_can_transceiver(i, true);
} else {
- uno_enable_can_transciever(i, enabled);
+ uno_enable_can_transceiver(i, enabled);
}
}
}
@@ -54,20 +54,18 @@ void dos_set_gps_load_switch(bool enabled) {
}
void dos_set_bootkick(bool enabled){
- UNUSED(enabled);
+ set_gpio_output(GPIOC, 4, !enabled);
}
-void dos_bootkick(void) {}
-
void dos_set_phone_power(bool enabled){
UNUSED(enabled);
}
void dos_set_usb_power_mode(uint8_t mode) {
- UNUSED(mode);
+ dos_set_bootkick(mode == USB_POWER_CDP);
}
-void dos_set_esp_gps_mode(uint8_t mode) {
+void dos_set_gps_mode(uint8_t mode) {
UNUSED(mode);
}
@@ -101,11 +99,6 @@ void dos_set_can_mode(uint8_t mode){
void dos_usb_power_mode_tick(uint32_t uptime){
UNUSED(uptime);
- if(bootkick_timer != 0U){
- bootkick_timer--;
- } else {
- dos_set_bootkick(false);
- }
}
bool dos_check_ignition(void){
@@ -132,6 +125,14 @@ uint32_t dos_read_current(void){
return 0U;
}
+void dos_set_clock_source_mode(uint8_t mode){
+ clock_source_init(mode);
+}
+
+void dos_set_siren(bool enabled){
+ set_gpio_output(GPIOC, 12, enabled);
+}
+
void dos_init(void) {
common_init_gpio();
@@ -171,8 +172,8 @@ void dos_init(void) {
// Initialize RTC
rtc_init();
- // Enable CAN transcievers
- dos_enable_can_transcievers(true);
+ // Enable CAN transceivers
+ dos_enable_can_transceivers(true);
// Disable LEDs
dos_set_led(LED_RED, false);
@@ -189,6 +190,9 @@ void dos_init(void) {
// init multiplexer
can_set_obd(car_harness_status, false);
+
+ // Init clock source as internal free running
+ dos_set_clock_source_mode(CLOCK_SOURCE_MODE_FREE_RUNNING);
}
const harness_configuration dos_harness_config = {
@@ -209,16 +213,18 @@ const board board_dos = {
.board_type = "Dos",
.harness_config = &dos_harness_config,
.init = dos_init,
- .enable_can_transciever = dos_enable_can_transciever,
- .enable_can_transcievers = dos_enable_can_transcievers,
+ .enable_can_transceiver = dos_enable_can_transceiver,
+ .enable_can_transceivers = dos_enable_can_transceivers,
.set_led = dos_set_led,
.set_usb_power_mode = dos_set_usb_power_mode,
- .set_esp_gps_mode = dos_set_esp_gps_mode,
+ .set_gps_mode = dos_set_gps_mode,
.set_can_mode = dos_set_can_mode,
.usb_power_mode_tick = dos_usb_power_mode_tick,
.check_ignition = dos_check_ignition,
.read_current = dos_read_current,
.set_fan_power = dos_set_fan_power,
.set_ir_power = dos_set_ir_power,
- .set_phone_power = dos_set_phone_power
+ .set_phone_power = dos_set_phone_power,
+ .set_clock_source_mode = dos_set_clock_source_mode,
+ .set_siren = dos_set_siren
};
diff --git a/panda/board/boards/grey.h b/panda/board/boards/grey.h
index 1c7278d27..bd5b1559e 100644
--- a/panda/board/boards/grey.h
+++ b/panda/board/boards/grey.h
@@ -8,22 +8,22 @@ void grey_init(void) {
white_grey_common_init();
// Set default state of GPS
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ current_board->set_gps_mode(GPS_ENABLED);
}
-void grey_set_esp_gps_mode(uint8_t mode) {
+void grey_set_gps_mode(uint8_t mode) {
switch (mode) {
- case ESP_GPS_DISABLED:
+ case GPS_DISABLED:
// GPS OFF
set_gpio_output(GPIOC, 14, 0);
set_gpio_output(GPIOC, 5, 0);
break;
- case ESP_GPS_ENABLED:
+ case GPS_ENABLED:
// GPS ON
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 1);
break;
- case ESP_GPS_BOOTMODE:
+ case GPS_BOOTMODE:
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 0);
break;
@@ -37,16 +37,18 @@ const board board_grey = {
.board_type = "Grey",
.harness_config = &white_harness_config,
.init = grey_init,
- .enable_can_transciever = white_enable_can_transciever,
- .enable_can_transcievers = white_enable_can_transcievers,
+ .enable_can_transceiver = white_enable_can_transceiver,
+ .enable_can_transceivers = white_enable_can_transceivers,
.set_led = white_set_led,
.set_usb_power_mode = white_set_usb_power_mode,
- .set_esp_gps_mode = grey_set_esp_gps_mode,
+ .set_gps_mode = grey_set_gps_mode,
.set_can_mode = white_set_can_mode,
.usb_power_mode_tick = white_usb_power_mode_tick,
.check_ignition = white_check_ignition,
.read_current = white_read_current,
.set_fan_power = white_set_fan_power,
.set_ir_power = white_set_ir_power,
- .set_phone_power = white_set_phone_power
-};
\ No newline at end of file
+ .set_phone_power = white_set_phone_power,
+ .set_clock_source_mode = white_set_clock_source_mode,
+ .set_siren = white_set_siren
+};
diff --git a/panda/board/boards/pedal.h b/panda/board/boards/pedal.h
index c67d39151..31abe9c5c 100644
--- a/panda/board/boards/pedal.h
+++ b/panda/board/boards/pedal.h
@@ -2,19 +2,19 @@
// Pedal //
// ///// //
-void pedal_enable_can_transciever(uint8_t transciever, bool enabled) {
- switch (transciever){
+void pedal_enable_can_transceiver(uint8_t transceiver, bool enabled) {
+ switch (transceiver){
case 1:
set_gpio_output(GPIOB, 3, !enabled);
break;
default:
- puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+ puts("Invalid CAN transceiver ("); puth(transceiver); puts("): enabling failed\n");
break;
}
}
-void pedal_enable_can_transcievers(bool enabled) {
- pedal_enable_can_transciever(1U, enabled);
+void pedal_enable_can_transceivers(bool enabled) {
+ pedal_enable_can_transceiver(1U, enabled);
}
void pedal_set_led(uint8_t color, bool enabled) {
@@ -35,7 +35,7 @@ void pedal_set_usb_power_mode(uint8_t mode){
puts("Trying to set USB power mode on pedal. This is not supported.\n");
}
-void pedal_set_esp_gps_mode(uint8_t mode) {
+void pedal_set_gps_mode(uint8_t mode) {
UNUSED(mode);
puts("Trying to set ESP/GPS mode on pedal. This is not supported.\n");
}
@@ -77,6 +77,14 @@ void pedal_set_phone_power(bool enabled){
UNUSED(enabled);
}
+void pedal_set_clock_source_mode(uint8_t mode){
+ UNUSED(mode);
+}
+
+void pedal_set_siren(bool enabled){
+ UNUSED(enabled);
+}
+
void pedal_init(void) {
common_init_gpio();
@@ -86,8 +94,8 @@ void pedal_init(void) {
// DAC outputs on A4 and A5
// apparently they don't need GPIO setup
- // Enable transciever
- pedal_enable_can_transcievers(true);
+ // Enable transceiver
+ pedal_enable_can_transceivers(true);
// Disable LEDs
pedal_set_led(LED_RED, false);
@@ -102,16 +110,18 @@ const board board_pedal = {
.board_type = "Pedal",
.harness_config = &pedal_harness_config,
.init = pedal_init,
- .enable_can_transciever = pedal_enable_can_transciever,
- .enable_can_transcievers = pedal_enable_can_transcievers,
+ .enable_can_transceiver = pedal_enable_can_transceiver,
+ .enable_can_transceivers = pedal_enable_can_transceivers,
.set_led = pedal_set_led,
.set_usb_power_mode = pedal_set_usb_power_mode,
- .set_esp_gps_mode = pedal_set_esp_gps_mode,
+ .set_gps_mode = pedal_set_gps_mode,
.set_can_mode = pedal_set_can_mode,
.usb_power_mode_tick = pedal_usb_power_mode_tick,
.check_ignition = pedal_check_ignition,
.read_current = pedal_read_current,
.set_fan_power = pedal_set_fan_power,
.set_ir_power = pedal_set_ir_power,
- .set_phone_power = pedal_set_phone_power
-};
\ No newline at end of file
+ .set_phone_power = pedal_set_phone_power,
+ .set_clock_source_mode = pedal_set_clock_source_mode,
+ .set_siren = pedal_set_siren
+};
diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h
index de430cb57..96eb6268b 100644
--- a/panda/board/boards/uno.h
+++ b/panda/board/boards/uno.h
@@ -4,8 +4,8 @@
#define BOOTKICK_TIME 3U
uint8_t bootkick_timer = 0U;
-void uno_enable_can_transciever(uint8_t transciever, bool enabled) {
- switch (transciever){
+void uno_enable_can_transceiver(uint8_t transceiver, bool enabled) {
+ switch (transceiver){
case 1U:
set_gpio_output(GPIOC, 1, !enabled);
break;
@@ -19,18 +19,18 @@ void uno_enable_can_transciever(uint8_t transciever, bool enabled) {
set_gpio_output(GPIOB, 10, !enabled);
break;
default:
- puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+ puts("Invalid CAN transceiver ("); puth(transceiver); puts("): enabling failed\n");
break;
}
}
-void uno_enable_can_transcievers(bool enabled) {
+void uno_enable_can_transceivers(bool enabled) {
for(uint8_t i=1U; i<=4U; i++){
// Leave main CAN always on for CAN-based ignition detection
if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
- uno_enable_can_transciever(i, true);
+ uno_enable_can_transceiver(i, true);
} else {
- uno_enable_can_transciever(i, enabled);
+ uno_enable_can_transceiver(i, enabled);
}
}
}
@@ -89,21 +89,21 @@ void uno_set_usb_power_mode(uint8_t mode) {
}
}
-void uno_set_esp_gps_mode(uint8_t mode) {
+void uno_set_gps_mode(uint8_t mode) {
switch (mode) {
- case ESP_GPS_DISABLED:
+ case GPS_DISABLED:
// GPS OFF
set_gpio_output(GPIOB, 1, 0);
set_gpio_output(GPIOC, 5, 0);
uno_set_gps_load_switch(false);
break;
- case ESP_GPS_ENABLED:
+ case GPS_ENABLED:
// GPS ON
set_gpio_output(GPIOB, 1, 1);
set_gpio_output(GPIOC, 5, 1);
uno_set_gps_load_switch(true);
break;
- case ESP_GPS_BOOTMODE:
+ case GPS_BOOTMODE:
set_gpio_output(GPIOB, 1, 1);
set_gpio_output(GPIOC, 5, 0);
uno_set_gps_load_switch(true);
@@ -175,6 +175,14 @@ uint32_t uno_read_current(void){
return 0U;
}
+void uno_set_clock_source_mode(uint8_t mode){
+ UNUSED(mode);
+}
+
+void uno_set_siren(bool enabled){
+ UNUSED(enabled);
+}
+
void uno_init(void) {
common_init_gpio();
@@ -188,7 +196,7 @@ void uno_init(void) {
set_gpio_mode(GPIOC, 3, MODE_ANALOG);
// Set default state of GPS
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ current_board->set_gps_mode(GPS_ENABLED);
// C10: OBD_SBU1_RELAY (harness relay driving output)
// C11: OBD_SBU2_RELAY (harness relay driving output)
@@ -223,8 +231,8 @@ void uno_init(void) {
// Initialize RTC
rtc_init();
- // Enable CAN transcievers
- uno_enable_can_transcievers(true);
+ // Enable CAN transceivers
+ uno_enable_can_transceivers(true);
// Disable LEDs
uno_set_led(LED_RED, false);
@@ -271,16 +279,18 @@ const board board_uno = {
.board_type = "Uno",
.harness_config = &uno_harness_config,
.init = uno_init,
- .enable_can_transciever = uno_enable_can_transciever,
- .enable_can_transcievers = uno_enable_can_transcievers,
+ .enable_can_transceiver = uno_enable_can_transceiver,
+ .enable_can_transceivers = uno_enable_can_transceivers,
.set_led = uno_set_led,
.set_usb_power_mode = uno_set_usb_power_mode,
- .set_esp_gps_mode = uno_set_esp_gps_mode,
+ .set_gps_mode = uno_set_gps_mode,
.set_can_mode = uno_set_can_mode,
.usb_power_mode_tick = uno_usb_power_mode_tick,
.check_ignition = uno_check_ignition,
.read_current = uno_read_current,
.set_fan_power = uno_set_fan_power,
.set_ir_power = uno_set_ir_power,
- .set_phone_power = uno_set_phone_power
+ .set_phone_power = uno_set_phone_power,
+ .set_clock_source_mode = uno_set_clock_source_mode,
+ .set_siren = uno_set_siren
};
diff --git a/panda/board/boards/white.h b/panda/board/boards/white.h
index 1be0702f9..44d8f512b 100644
--- a/panda/board/boards/white.h
+++ b/panda/board/boards/white.h
@@ -2,8 +2,8 @@
// White Panda //
// /////////// //
-void white_enable_can_transciever(uint8_t transciever, bool enabled) {
- switch (transciever){
+void white_enable_can_transceiver(uint8_t transceiver, bool enabled) {
+ switch (transceiver){
case 1U:
set_gpio_output(GPIOC, 1, !enabled);
break;
@@ -14,15 +14,15 @@ void white_enable_can_transciever(uint8_t transciever, bool enabled) {
set_gpio_output(GPIOA, 0, !enabled);
break;
default:
- puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+ puts("Invalid CAN transceiver ("); puth(transceiver); puts("): enabling failed\n");
break;
}
}
-void white_enable_can_transcievers(bool enabled) {
- uint8_t t1 = enabled ? 1U : 2U; // leave transciever 1 enabled to detect CAN ignition
+void white_enable_can_transceivers(bool enabled) {
+ uint8_t t1 = enabled ? 1U : 2U; // leave transceiver 1 enabled to detect CAN ignition
for(uint8_t i=t1; i<=3U; i++) {
- white_enable_can_transciever(i, enabled);
+ white_enable_can_transceiver(i, enabled);
}
}
@@ -71,21 +71,21 @@ void white_set_usb_power_mode(uint8_t mode){
}
}
-void white_set_esp_gps_mode(uint8_t mode) {
+void white_set_gps_mode(uint8_t mode) {
switch (mode) {
- case ESP_GPS_DISABLED:
+ case GPS_DISABLED:
// ESP OFF
set_gpio_output(GPIOC, 14, 0);
set_gpio_output(GPIOC, 5, 0);
break;
#ifndef EON
- case ESP_GPS_ENABLED:
+ case GPS_ENABLED:
// ESP ON
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 1);
break;
#endif
- case ESP_GPS_BOOTMODE:
+ case GPS_BOOTMODE:
set_gpio_output(GPIOC, 14, 1);
set_gpio_output(GPIOC, 5, 0);
break;
@@ -242,6 +242,14 @@ void white_set_phone_power(bool enabled){
UNUSED(enabled);
}
+void white_set_clock_source_mode(uint8_t mode){
+ UNUSED(mode);
+}
+
+void white_set_siren(bool enabled){
+ UNUSED(enabled);
+}
+
void white_grey_common_init(void) {
common_init_gpio();
@@ -291,8 +299,8 @@ void white_grey_common_init(void) {
set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
set_gpio_pullup(GPIOC, 11, PULL_UP);
- // Enable CAN transcievers
- white_enable_can_transcievers(true);
+ // Enable CAN transceivers
+ white_enable_can_transceivers(true);
// Disable LEDs
white_set_led(LED_RED, false);
@@ -316,12 +324,8 @@ void white_grey_common_init(void) {
void white_init(void) {
white_grey_common_init();
- // Set default state of ESP
- #ifdef EON
- current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
- #else
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
- #endif
+ // Set ESP off by default
+ current_board->set_gps_mode(GPS_DISABLED);
}
const harness_configuration white_harness_config = {
@@ -332,16 +336,18 @@ const board board_white = {
.board_type = "White",
.harness_config = &white_harness_config,
.init = white_init,
- .enable_can_transciever = white_enable_can_transciever,
- .enable_can_transcievers = white_enable_can_transcievers,
+ .enable_can_transceiver = white_enable_can_transceiver,
+ .enable_can_transceivers = white_enable_can_transceivers,
.set_led = white_set_led,
.set_usb_power_mode = white_set_usb_power_mode,
- .set_esp_gps_mode = white_set_esp_gps_mode,
+ .set_gps_mode = white_set_gps_mode,
.set_can_mode = white_set_can_mode,
.usb_power_mode_tick = white_usb_power_mode_tick,
.check_ignition = white_check_ignition,
.read_current = white_read_current,
.set_fan_power = white_set_fan_power,
.set_ir_power = white_set_ir_power,
- .set_phone_power = white_set_phone_power
+ .set_phone_power = white_set_phone_power,
+ .set_clock_source_mode = white_set_clock_source_mode,
+ .set_siren = white_set_siren
};
diff --git a/panda/board/drivers/clock_source.h b/panda/board/drivers/clock_source.h
new file mode 100644
index 000000000..966dee452
--- /dev/null
+++ b/panda/board/drivers/clock_source.h
@@ -0,0 +1,100 @@
+
+#define CLOCK_SOURCE_MODE_DISABLED 0U
+#define CLOCK_SOURCE_MODE_FREE_RUNNING 1U
+#define CLOCK_SOURCE_MODE_EXTERNAL_SYNC 2U
+
+#define CLOCK_SOURCE_PERIOD_MS 50U
+#define CLOCK_SOURCE_PULSE_LEN_MS 2U
+
+uint8_t clock_source_mode = CLOCK_SOURCE_MODE_DISABLED;
+
+void EXTI0_IRQ_Handler(void) {
+ volatile unsigned int pr = EXTI->PR & (1U << 0);
+ if (pr != 0U) {
+ if(clock_source_mode == CLOCK_SOURCE_MODE_EXTERNAL_SYNC){
+ // TODO: Implement!
+ }
+ }
+ EXTI->PR = (1U << 0);
+}
+
+void TIM1_UP_TIM10_IRQ_Handler(void) {
+ if((TIM1->SR & TIM_SR_UIF) != 0) {
+ if(clock_source_mode != CLOCK_SOURCE_MODE_DISABLED) {
+ // Start clock pulse
+ set_gpio_output(GPIOB, 14, true);
+ set_gpio_output(GPIOB, 15, true);
+ }
+
+ // Reset interrupt
+ TIM1->SR &= ~(TIM_SR_UIF);
+ }
+}
+
+void TIM1_CC_IRQ_Handler(void) {
+ if((TIM1->SR & TIM_SR_CC1IF) != 0) {
+ if(clock_source_mode != CLOCK_SOURCE_MODE_DISABLED) {
+ // End clock pulse
+ set_gpio_output(GPIOB, 14, false);
+ set_gpio_output(GPIOB, 15, false);
+ }
+
+ // Reset interrupt
+ TIM1->SR &= ~(TIM_SR_CC1IF);
+ }
+}
+
+void clock_source_init(uint8_t mode){
+ // Setup external clock signal interrupt
+ REGISTER_INTERRUPT(EXTI0_IRQn, EXTI0_IRQ_Handler, 110U, FAULT_INTERRUPT_RATE_CLOCK_SOURCE)
+ register_set(&(SYSCFG->EXTICR[0]), SYSCFG_EXTICR1_EXTI0_PB, 0xFU);
+ register_set_bits(&(EXTI->IMR), (1U << 0));
+ register_set_bits(&(EXTI->RTSR), (1U << 0));
+ register_clear_bits(&(EXTI->FTSR), (1U << 0));
+
+ // Setup timer
+ REGISTER_INTERRUPT(TIM1_UP_TIM10_IRQn, TIM1_UP_TIM10_IRQ_Handler, (1200U / CLOCK_SOURCE_PERIOD_MS) , FAULT_INTERRUPT_RATE_TIM1)
+ REGISTER_INTERRUPT(TIM1_CC_IRQn, TIM1_CC_IRQ_Handler, (1200U / CLOCK_SOURCE_PERIOD_MS) , FAULT_INTERRUPT_RATE_TIM1)
+ register_set(&(TIM1->PSC), (9600-1), 0xFFFFU); // Tick on 0.1 ms
+ register_set(&(TIM1->ARR), ((CLOCK_SOURCE_PERIOD_MS*10U) - 1U), 0xFFFFU); // Period
+ register_set(&(TIM1->CCMR1), 0U, 0xFFFFU); // No output on compare
+ register_set_bits(&(TIM1->CCER), TIM_CCER_CC1E); // Enable compare 1
+ register_set(&(TIM1->CCR1), (CLOCK_SOURCE_PULSE_LEN_MS*10U), 0xFFFFU); // Compare 1 value
+ register_set_bits(&(TIM1->DIER), TIM_DIER_UIE | TIM_DIER_CC1IE); // Enable interrupts
+ register_set(&(TIM1->CR1), TIM_CR1_CEN, 0x3FU); // Enable timer
+
+ // Set mode
+ switch(mode) {
+ case CLOCK_SOURCE_MODE_DISABLED:
+ // No clock signal
+ NVIC_DisableIRQ(EXTI0_IRQn);
+ NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn);
+ NVIC_DisableIRQ(TIM1_CC_IRQn);
+
+ // Disable pulse if we were in the middle of it
+ set_gpio_output(GPIOB, 14, false);
+ set_gpio_output(GPIOB, 15, false);
+
+ clock_source_mode = CLOCK_SOURCE_MODE_DISABLED;
+ break;
+ case CLOCK_SOURCE_MODE_FREE_RUNNING:
+ // Clock signal is based on internal timer
+ NVIC_DisableIRQ(EXTI0_IRQn);
+ NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+ NVIC_EnableIRQ(TIM1_CC_IRQn);
+
+ clock_source_mode = CLOCK_SOURCE_MODE_FREE_RUNNING;
+ break;
+ case CLOCK_SOURCE_MODE_EXTERNAL_SYNC:
+ // Clock signal is based on external timer
+ NVIC_EnableIRQ(EXTI0_IRQn);
+ NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+ NVIC_EnableIRQ(TIM1_CC_IRQn);
+
+ clock_source_mode = CLOCK_SOURCE_MODE_EXTERNAL_SYNC;
+ break;
+ default:
+ puts("Unknown clock source mode: "); puth(mode); puts("\n");
+ break;
+ }
+}
\ No newline at end of file
diff --git a/panda/board/drivers/uart.h b/panda/board/drivers/uart.h
index 4ab791f60..e4f23ead0 100644
--- a/panda/board/drivers/uart.h
+++ b/panda/board/drivers/uart.h
@@ -50,8 +50,8 @@ void debug_ring_callback(uart_ring *ring);
// ******************************** UART buffers ********************************
-// esp_gps = USART1
-UART_BUFFER(esp_gps, FIFO_SIZE_DMA, FIFO_SIZE_INT, USART1, NULL, true)
+// gps = USART1
+UART_BUFFER(gps, FIFO_SIZE_DMA, FIFO_SIZE_INT, USART1, NULL, true)
// lin1, K-LINE = UART5
// lin2, L-LINE = USART3
@@ -68,7 +68,7 @@ uart_ring *get_ring_by_number(int a) {
ring = &uart_ring_debug;
break;
case 1:
- ring = &uart_ring_esp_gps;
+ ring = &uart_ring_gps;
break;
case 2:
ring = &uart_ring_lin1;
@@ -185,8 +185,8 @@ void uart_interrupt_handler(uart_ring *q) {
// Reset IDLE flag
UART_READ_DR(q->uart)
- if(q == &uart_ring_esp_gps){
- dma_pointer_handler(&uart_ring_esp_gps, DMA2_Stream5->NDTR);
+ if(q == &uart_ring_gps){
+ dma_pointer_handler(&uart_ring_gps, DMA2_Stream5->NDTR);
} else {
#ifdef DEBUG_UART
puts("No IDLE dma_pointer_handler implemented for this UART.");
@@ -197,7 +197,7 @@ void uart_interrupt_handler(uart_ring *q) {
EXIT_CRITICAL();
}
-void USART1_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_esp_gps); }
+void USART1_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_gps); }
void USART2_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_debug); }
void USART3_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_lin2); }
void UART5_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_lin1); }
@@ -219,7 +219,7 @@ void DMA2_Stream5_IRQ_Handler(void) {
}
// Re-calculate write pointer and reset flags
- dma_pointer_handler(&uart_ring_esp_gps, DMA2_Stream5->NDTR);
+ dma_pointer_handler(&uart_ring_gps, DMA2_Stream5->NDTR);
DMA2->HIFCR = DMA_HIFCR_CTCIF5 | DMA_HIFCR_CHTIF5;
EXIT_CRITICAL();
@@ -229,7 +229,7 @@ void DMA2_Stream5_IRQ_Handler(void) {
void dma_rx_init(uart_ring *q) {
// Initialization is UART-dependent
- if(q == &uart_ring_esp_gps){
+ if(q == &uart_ring_gps){
// DMA2, stream 5, channel 4
// Disable FIFO mode (enable direct)
diff --git a/panda/board/faults.h b/panda/board/faults.h
index 2c031cf29..0825586d1 100644
--- a/panda/board/faults.h
+++ b/panda/board/faults.h
@@ -3,26 +3,28 @@
#define FAULT_STATUS_PERMANENT 2U
// Fault types
-#define FAULT_RELAY_MALFUNCTION (1U << 0)
-#define FAULT_UNUSED_INTERRUPT_HANDLED (1U << 1)
-#define FAULT_INTERRUPT_RATE_CAN_1 (1U << 2)
-#define FAULT_INTERRUPT_RATE_CAN_2 (1U << 3)
-#define FAULT_INTERRUPT_RATE_CAN_3 (1U << 4)
-#define FAULT_INTERRUPT_RATE_TACH (1U << 5)
-#define FAULT_INTERRUPT_RATE_GMLAN (1U << 6)
-#define FAULT_INTERRUPT_RATE_INTERRUPTS (1U << 7)
-#define FAULT_INTERRUPT_RATE_SPI_DMA (1U << 8)
-#define FAULT_INTERRUPT_RATE_SPI_CS (1U << 9)
-#define FAULT_INTERRUPT_RATE_UART_1 (1U << 10)
-#define FAULT_INTERRUPT_RATE_UART_2 (1U << 11)
-#define FAULT_INTERRUPT_RATE_UART_3 (1U << 12)
-#define FAULT_INTERRUPT_RATE_UART_5 (1U << 13)
-#define FAULT_INTERRUPT_RATE_UART_DMA (1U << 14)
-#define FAULT_INTERRUPT_RATE_USB (1U << 15)
-#define FAULT_INTERRUPT_RATE_TIM1 (1U << 16)
-#define FAULT_INTERRUPT_RATE_TIM3 (1U << 17)
-#define FAULT_REGISTER_DIVERGENT (1U << 18)
-#define FAULT_INTERRUPT_RATE_KLINE_INIT (1U << 19)
+#define FAULT_RELAY_MALFUNCTION (1U << 0)
+#define FAULT_UNUSED_INTERRUPT_HANDLED (1U << 1)
+#define FAULT_INTERRUPT_RATE_CAN_1 (1U << 2)
+#define FAULT_INTERRUPT_RATE_CAN_2 (1U << 3)
+#define FAULT_INTERRUPT_RATE_CAN_3 (1U << 4)
+#define FAULT_INTERRUPT_RATE_TACH (1U << 5)
+#define FAULT_INTERRUPT_RATE_GMLAN (1U << 6)
+#define FAULT_INTERRUPT_RATE_INTERRUPTS (1U << 7)
+#define FAULT_INTERRUPT_RATE_SPI_DMA (1U << 8)
+#define FAULT_INTERRUPT_RATE_SPI_CS (1U << 9)
+#define FAULT_INTERRUPT_RATE_UART_1 (1U << 10)
+#define FAULT_INTERRUPT_RATE_UART_2 (1U << 11)
+#define FAULT_INTERRUPT_RATE_UART_3 (1U << 12)
+#define FAULT_INTERRUPT_RATE_UART_5 (1U << 13)
+#define FAULT_INTERRUPT_RATE_UART_DMA (1U << 14)
+#define FAULT_INTERRUPT_RATE_USB (1U << 15)
+#define FAULT_INTERRUPT_RATE_TIM1 (1U << 16)
+#define FAULT_INTERRUPT_RATE_TIM3 (1U << 17)
+#define FAULT_REGISTER_DIVERGENT (1U << 18)
+#define FAULT_INTERRUPT_RATE_KLINE_INIT (1U << 19)
+#define FAULT_INTERRUPT_RATE_CLOCK_SOURCE (1U << 20)
+#define FAULT_INTERRUPT_RATE_TIM9 (1U << 21)
// Permanent faults
#define PERMANENT_FAULTS 0U
diff --git a/panda/board/gpio.h b/panda/board/gpio.h
index 2b937eced..b50de50c3 100644
--- a/panda/board/gpio.h
+++ b/panda/board/gpio.h
@@ -64,13 +64,9 @@ void early(void) {
if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
#ifdef PANDA
- current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
+ current_board->set_gps_mode(GPS_DISABLED);
#endif
current_board->set_led(LED_GREEN, 1);
jump_to_bootloader();
}
-
- if (is_entering_bootmode) {
- enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
- }
-}
\ No newline at end of file
+}
diff --git a/panda/board/main.c b/panda/board/main.c
index 497ea5dcd..c6e4f0fce 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -400,24 +400,24 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
// **** 0xd9: set ESP power
case 0xd9:
if (setup->b.wValue.w == 1U) {
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ current_board->set_gps_mode(GPS_ENABLED);
} else if (setup->b.wValue.w == 2U) {
- current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
+ current_board->set_gps_mode(GPS_BOOTMODE);
} else {
- current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
+ current_board->set_gps_mode(GPS_DISABLED);
}
break;
// **** 0xda: reset ESP, with optional boot mode
case 0xda:
- current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
+ current_board->set_gps_mode(GPS_DISABLED);
delay(1000000);
if (setup->b.wValue.w == 1U) {
- current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
+ current_board->set_gps_mode(GPS_BOOTMODE);
} else {
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ current_board->set_gps_mode(GPS_ENABLED);
}
delay(1000000);
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ current_board->set_gps_mode(GPS_ENABLED);
break;
// **** 0xdb: set GMLAN (white/grey) or OBD CAN (black) multiplexing mode
case 0xdb:
@@ -493,7 +493,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
}
// TODO: Remove this again and fix boardd code to hande the message bursts instead of single chars
- if (ur == &uart_ring_esp_gps) {
+ if (ur == &uart_ring_gps) {
dma_pointer_handler(ur, DMA2_Stream5->NDTR);
}
@@ -606,6 +606,14 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
}
}
break;
+ // **** 0xf5: set clock source mode
+ case 0xf5:
+ current_board->set_clock_source_mode(setup->b.wValue.w);
+ break;
+ // **** 0xf6: set siren enabled
+ case 0xf6:
+ siren_enabled = (setup->b.wValue.w != 0U);
+ break;
default:
puts("NO HANDLER ");
puth(setup->b.bRequest);
@@ -663,87 +671,97 @@ void __attribute__ ((noinline)) enable_fpu(void) {
#define EON_HEARTBEAT_IGNITION_CNT_ON 5U
#define EON_HEARTBEAT_IGNITION_CNT_OFF 2U
-// called at 1Hz
+// called at 8Hz
+uint8_t loop_counter = 0U;
void TIM1_BRK_TIM9_IRQ_Handler(void) {
if (TIM9->SR != 0) {
- can_live = pending_can_live;
+ // siren
+ current_board->set_siren((loop_counter & 1U) && siren_enabled);
- current_board->usb_power_mode_tick(uptime_cnt);
+ // decimated to 1Hz
+ if(loop_counter == 0U){
+ can_live = pending_can_live;
- //puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n");
+ current_board->usb_power_mode_tick(uptime_cnt);
- // reset this every 16th pass
- if ((uptime_cnt & 0xFU) == 0U) {
- pending_can_live = 0;
- }
- #ifdef DEBUG
- puts("** blink ");
- puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" ");
- puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts(" ");
- puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
- #endif
+ //puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n");
- // Tick drivers
- fan_tick();
-
- // set green LED to be controls allowed
- current_board->set_led(LED_GREEN, controls_allowed);
-
- // turn off the blue LED, turned on by CAN
- // unless we are in power saving mode
- current_board->set_led(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
-
- // increase heartbeat counter and cap it at the uint32 limit
- if (heartbeat_counter < __UINT32_MAX__) {
- heartbeat_counter += 1U;
- }
-
- #ifdef EON
- // check heartbeat counter if we are running EON code.
- // if the heartbeat has been gone for a while, go to SILENT safety mode and enter power save
- if (heartbeat_counter >= (check_started() ? EON_HEARTBEAT_IGNITION_CNT_ON : EON_HEARTBEAT_IGNITION_CNT_OFF)) {
- puts("EON hasn't sent a heartbeat for 0x");
- puth(heartbeat_counter);
- puts(" seconds. Safety is set to SILENT mode.\n");
- if (current_safety_mode != SAFETY_SILENT) {
- set_safety_mode(SAFETY_SILENT, 0U);
+ // reset this every 16th pass
+ if ((uptime_cnt & 0xFU) == 0U) {
+ pending_can_live = 0;
}
- if (power_save_status != POWER_SAVE_STATUS_ENABLED) {
- set_power_save_state(POWER_SAVE_STATUS_ENABLED);
+ #ifdef DEBUG
+ puts("** blink ");
+ puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts(" ");
+ puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts(" ");
+ puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
+ #endif
+
+ // Tick drivers
+ fan_tick();
+
+ // set green LED to be controls allowed
+ current_board->set_led(LED_GREEN, controls_allowed);
+
+ // turn off the blue LED, turned on by CAN
+ // unless we are in power saving mode
+ current_board->set_led(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
+
+ // increase heartbeat counter and cap it at the uint32 limit
+ if (heartbeat_counter < __UINT32_MAX__) {
+ heartbeat_counter += 1U;
}
- // Also disable IR when the heartbeat goes missing
- current_board->set_ir_power(0U);
+ #ifdef EON
+ // check heartbeat counter if we are running EON code.
+ // if the heartbeat has been gone for a while, go to SILENT safety mode and enter power save
+ if (heartbeat_counter >= (check_started() ? EON_HEARTBEAT_IGNITION_CNT_ON : EON_HEARTBEAT_IGNITION_CNT_OFF)) {
+ puts("EON hasn't sent a heartbeat for 0x");
+ puth(heartbeat_counter);
+ puts(" seconds. Safety is set to SILENT mode.\n");
+ if (current_safety_mode != SAFETY_SILENT) {
+ set_safety_mode(SAFETY_SILENT, 0U);
+ }
+ if (power_save_status != POWER_SAVE_STATUS_ENABLED) {
+ set_power_save_state(POWER_SAVE_STATUS_ENABLED);
+ }
- // If enumerated but no heartbeat (phone up, boardd not running), turn the fan on to cool the device
- if(usb_enumerated()){
- current_board->set_fan_power(50U);
- } else {
- current_board->set_fan_power(0U);
+ // Also disable IR when the heartbeat goes missing
+ current_board->set_ir_power(0U);
+
+ // If enumerated but no heartbeat (phone up, boardd not running), turn the fan on to cool the device
+ if(usb_enumerated()){
+ current_board->set_fan_power(50U);
+ } else {
+ current_board->set_fan_power(0U);
+ }
}
+
+ // enter CDP mode when car starts to ensure we are charging a turned off EON
+ if (check_started() && (usb_power_mode != USB_POWER_CDP)) {
+ current_board->set_usb_power_mode(USB_POWER_CDP);
+ }
+ #endif
+
+ // check registers
+ check_registers();
+
+ // set ignition_can to false after 2s of no CAN seen
+ if (ignition_can_cnt > 2U) {
+ ignition_can = false;
+ };
+
+ // on to the next one
+ uptime_cnt += 1U;
+ safety_mode_cnt += 1U;
+ ignition_can_cnt += 1U;
+
+ // synchronous safety check
+ safety_tick(current_hooks);
}
- // enter CDP mode when car starts to ensure we are charging a turned off EON
- if (check_started() && (usb_power_mode != USB_POWER_CDP)) {
- current_board->set_usb_power_mode(USB_POWER_CDP);
- }
- #endif
-
- // check registers
- check_registers();
-
- // set ignition_can to false after 2s of no CAN seen
- if (ignition_can_cnt > 2U) {
- ignition_can = false;
- };
-
- // on to the next one
- uptime_cnt += 1U;
- safety_mode_cnt += 1U;
- ignition_can_cnt += 1U;
-
- // synchronous safety check
- safety_tick(current_hooks);
+ loop_counter++;
+ loop_counter %= 8U;
}
TIM9->SR = 0;
}
@@ -753,8 +771,8 @@ int main(void) {
// Init interrupt table
init_interrupts(true);
- // 1s timer
- REGISTER_INTERRUPT(TIM1_BRK_TIM9_IRQn, TIM1_BRK_TIM9_IRQ_Handler, 2U, FAULT_INTERRUPT_RATE_TIM1)
+ // 8Hz timer
+ REGISTER_INTERRUPT(TIM1_BRK_TIM9_IRQn, TIM1_BRK_TIM9_IRQ_Handler, 10U, FAULT_INTERRUPT_RATE_TIM9)
// shouldn't have interrupts here, but just in case
disable_interrupts();
@@ -778,7 +796,6 @@ int main(void) {
puts("Config:\n");
puts(" Board type: "); puts(current_board->board_type); puts("\n");
puts(has_external_debug_serial ? " Real serial\n" : " USB serial\n");
- puts(is_entering_bootmode ? " ESP wants bootmode\n" : " No bootmode\n");
// init board
current_board->init();
@@ -794,10 +811,10 @@ int main(void) {
}
if (board_has_gps()) {
- uart_init(&uart_ring_esp_gps, 9600);
+ uart_init(&uart_ring_gps, 9600);
} else {
// enable ESP uart
- uart_init(&uart_ring_esp_gps, 115200);
+ uart_init(&uart_ring_gps, 115200);
}
if(board_has_lin()){
@@ -820,14 +837,14 @@ int main(void) {
set_safety_mode(SAFETY_SILENT, 0);
// enable CAN TXs
- current_board->enable_can_transcievers(true);
+ current_board->enable_can_transceivers(true);
#ifndef EON
spi_init();
#endif
- // 1hz
- timer_init(TIM9, 1464);
+ // 8hz
+ timer_init(TIM9, 183);
NVIC_EnableIRQ(TIM1_BRK_TIM9_IRQn);
#ifdef DEBUG
diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h
index 363a992db..7fa2c4bbb 100644
--- a/panda/board/main_declarations.h
+++ b/panda/board/main_declarations.h
@@ -13,3 +13,4 @@ const board *current_board;
bool is_enumerated = 0;
uint32_t heartbeat_counter = 0;
uint32_t uptime_cnt = 0;
+bool siren_enabled = false;
diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h
index 3df750c10..2cb79cb61 100644
--- a/panda/board/power_saving.h
+++ b/panda/board/power_saving.h
@@ -28,13 +28,13 @@ void set_power_save_state(int state) {
enable = true;
}
- current_board->enable_can_transcievers(enable);
+ current_board->enable_can_transceivers(enable);
// Switch EPS/GPS
if (enable) {
- current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+ current_board->set_gps_mode(GPS_ENABLED);
} else {
- current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
+ current_board->set_gps_mode(GPS_DISABLED);
}
if(board_has_gmlan()){
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
index 93a437523..a4a6d4c2a 100644
--- a/panda/board/safety/safety_hyundai.h
+++ b/panda/board/safety/safety_hyundai.h
@@ -1,4 +1,4 @@
-const int HYUNDAI_MAX_STEER = 255; // like stock
+const int HYUNDAI_MAX_STEER = 384; // like stock
const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
const int HYUNDAI_MAX_RATE_UP = 3;
diff --git a/panda/board/spi_flasher.h b/panda/board/spi_flasher.h
index b0511c14e..2636bf535 100644
--- a/panda/board/spi_flasher.h
+++ b/panda/board/spi_flasher.h
@@ -290,7 +290,7 @@ void soft_flasher_start(void) {
// B8,B9: CAN 1
set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
- current_board->enable_can_transciever(1, true);
+ current_board->enable_can_transceiver(1, true);
// init can
llcan_set_speed(CAN1, 5000, false, false);
diff --git a/panda/certs/debugesp b/panda/certs/debugesp
deleted file mode 100644
index 789beaac1..000000000
--- a/panda/certs/debugesp
+++ /dev/null
@@ -1,15 +0,0 @@
------BEGIN RSA PRIVATE KEY-----
-MIICXAIBAAKBgQCjIHvrSCWN0Nec6ozbImYik30PIF7JSWgdwDKTxSJ05RM3pj5E
-LQEGt3qcaVrTokO68tpt5Gu1p6ZsNqWg7iVTW9M7Qj7IH45YDzQP/PSRjgSosQA6
-6f5Gokba5QrW38myqimvj+0p+YH+CNGCBRlTUQGCO8uLCspMZneRSLPW9QIDAQAB
-AoGADaUn+HRef9BaWMvd4G6uMHI54cwJYbj8NpDfKjExQqnuw5bqWnWRQmiSnwbJ
-DC7kj3zE/LBAuj890ot3q1CAWqh47ZICZfoX9Qbi5TpvIHFCGy6YkOliF6iIQhR2
-4+zNKTAA0zNKskOM25PdI+grK1Ni/bEofSA6TrqvEwsmxnkCQQDVp9FUUor2Bo/h
-/3oAIP51LTw7vfpztYbJr+BDV63czV2DLXzSwzeNrwH4sA3oy1mjUgMBBgAarNGE
-DYlc4H5jAkEAw3UCHzzXPlxkw2QGp7nBly5y3p80Uqc31NuYz8rdX/U8KTngi2No
-Ft/SGCEXNpeYbToj+WK3RJJ2Ey0mK8+IxwJAcpGd/5CPsaQNLcw4WK9Yo+8Q2Jxk
-G/4gfDCSmqn+smNxnLEcuUwzkwdgkEGgA9BfjeOhdsAH+EXpx90WZrZ/LwJBAK0k
-jq+rTqUQZbZsejTEKYjJ/bnV4BzDwoKN0Q1pkLc7X4LJoW74rTFuLgdv8MdMfRtt
-IIb/eoeFEpGkMicnHesCQHgR7BTUGBM6Uxam7RCdsgVsxoHBma21E/44ivWUMZzN
-3oVt0mPnjS4speOlqwED5pCJ7yw7jwLPFMs8kNxuIKU=
------END RSA PRIVATE KEY-----
diff --git a/panda/certs/debugesp.pub b/panda/certs/debugesp.pub
deleted file mode 100644
index 3afcf3988..000000000
--- a/panda/certs/debugesp.pub
+++ /dev/null
@@ -1 +0,0 @@
-ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAAAgQCjIHvrSCWN0Nec6ozbImYik30PIF7JSWgdwDKTxSJ05RM3pj5ELQEGt3qcaVrTokO68tpt5Gu1p6ZsNqWg7iVTW9M7Qj7IH45YDzQP/PSRjgSosQA66f5Gokba5QrW38myqimvj+0p+YH+CNGCBRlTUQGCO8uLCspMZneRSLPW9Q== batman@y840
diff --git a/panda/certs/releaseesp.pub b/panda/certs/releaseesp.pub
deleted file mode 100644
index 1d1d54bb7..000000000
--- a/panda/certs/releaseesp.pub
+++ /dev/null
@@ -1 +0,0 @@
-ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAAAgQDN4pVyGuJJSde1l3Fjay8qPxog09DsAJZtYPk+armoYO1L6YKReUTcMNyHQYZZMZFmhCdgjCgTIF2QYWMoP4KSe8l6JF04YPP51dIgefc6UXjtlSI8Pyutr0v9xXjSfsVm3RAJxDSHgzs9AoMsluKCL+LhAR1nd7cuHXITJ80O4w== batman@y840
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index 605977db0..936c975f0 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -10,7 +10,6 @@ import traceback
import subprocess
import sys
from .dfu import PandaDFU # pylint: disable=import-error
-from .esptool import ESPROM, CesantaFlasher # noqa pylint: disable=import-error
from .flash_release import flash_release # noqa pylint: disable=import-error
from .update import ensure_st_up_to_date # noqa pylint: disable=import-error
from .serial import PandaSerial # noqa pylint: disable=import-error
@@ -148,6 +147,10 @@ class Panda(object):
HW_TYPE_PEDAL = b'\x04'
HW_TYPE_UNO = b'\x05'
+ CLOCK_SOURCE_MODE_DISABLED = 0
+ CLOCK_SOURCE_MODE_FREE_RUNNING = 1
+ CLOCK_SOURCE_MODE_EXTERNAL_SYNC = 2
+
def __init__(self, serial=None, claim=True):
self._serial = serial
self._handle = None
@@ -458,7 +461,7 @@ class Panda(object):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe5, int(enable), 0, b'')
def set_can_enable(self, bus_num, enable):
- # sets the can transciever enable pin
+ # sets the can transceiver enable pin
self._handle.controlWrite(Panda.REQUEST_OUT, 0xf4, int(bus_num), int(enable), b'')
def set_can_speed_kbps(self, bus, speed):
@@ -665,3 +668,11 @@ class Panda(object):
# ****************** Phone *****************
def set_phone_power(self, enabled):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xb3, int(enabled), 0, b'')
+
+ # ************** Clock Source **************
+ def set_clock_source_mode(self, mode):
+ self._handle.controlWrite(Panda.REQUEST_OUT, 0xf5, int(mode), 0, b'')
+
+ # ****************** Siren *****************
+ def set_siren(self, enabled):
+ self._handle.controlWrite(Panda.REQUEST_OUT, 0xf6, int(enabled), 0, b'')
diff --git a/panda/python/dfu.py b/panda/python/dfu.py
index 658b6a9e8..a7f51ecda 100644
--- a/panda/python/dfu.py
+++ b/panda/python/dfu.py
@@ -24,7 +24,7 @@ class PandaDFU(object):
self._handle = device.open()
self.legacy = "07*128Kg" in self._handle.getASCIIStringDescriptor(4)
return
- raise Exception("failed to open " + dfu_serial)
+ raise Exception("failed to open " + dfu_serial if dfu_serial is not None else "DFU device")
@staticmethod
def list():
diff --git a/panda/python/esptool.py b/panda/python/esptool.py
deleted file mode 100755
index 4a7dabfa2..000000000
--- a/panda/python/esptool.py
+++ /dev/null
@@ -1,1317 +0,0 @@
-#!/usr/bin/env python
-# NB: Before sending a PR to change the above line to '#!/usr/bin/env python3', please read https://github.com/themadinventor/esptool/issues/21
-#
-# ESP8266 ROM Bootloader Utility
-# https://github.com/themadinventor/esptool
-#
-# Copyright (C) 2014-2016 Fredrik Ahlberg, Angus Gratton, other contributors as noted.
-#
-# This program is free software; you can redistribute it and/or modify it under
-# the terms of the GNU General Public License as published by the Free Software
-# Foundation; either version 2 of the License, or (at your option) any later version.
-#
-# This program is distributed in the hope that it will be useful, but WITHOUT
-# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-# FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License along with
-# this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
-# Street, Fifth Floor, Boston, MA 02110-1301 USA.
-
-# pylint: skip-file
-# flake8: noqa
-
-import argparse
-import hashlib
-import inspect
-import json
-import os
-#import serial
-import struct
-import subprocess
-import sys
-import tempfile
-import time
-#import traceback
-import usb1
-
-__version__ = "1.2"
-
-class FakePort(object):
- def __init__(self, serial=None):
- from panda import Panda
- self.panda = Panda(serial)
-
- # will only work on new st, old ones will stay @ 921600
- self.baudrate = 230400
-
- @property
- def baudrate(self):
- return self.baudrate
-
- @baudrate.setter
- def baudrate(self, x):
- print("set baud to", x)
- self.panda.set_uart_baud(1, x)
-
- def write(self, buf):
- SEND_STEP = 0x20
- for i in range(0, len(buf), SEND_STEP):
- self.panda.serial_write(1, buf[i:i+SEND_STEP])
-
- def flushInput(self):
- self.panda.serial_clear(1)
-
- def flushOutput(self):
- self.panda.serial_clear(1)
-
- def read(self, llen):
- ret = self.panda._handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xe0, 1, 0, 1)
- if ret == '':
- time.sleep(0.1)
- ret = self.panda._handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xe0, 1, 0, 1)
- return str(ret)
-
- def reset(self):
- self.panda.esp_reset(1)
-
- def inWaiting(self):
- return False
-
-class ESPROM(object):
- # These are the currently known commands supported by the ROM
- ESP_FLASH_BEGIN = 0x02
- ESP_FLASH_DATA = 0x03
- ESP_FLASH_END = 0x04
- ESP_MEM_BEGIN = 0x05
- ESP_MEM_END = 0x06
- ESP_MEM_DATA = 0x07
- ESP_SYNC = 0x08
- ESP_WRITE_REG = 0x09
- ESP_READ_REG = 0x0a
-
- # Maximum block sized for RAM and Flash writes, respectively.
- ESP_RAM_BLOCK = 0x1800
- ESP_FLASH_BLOCK = 0x400
-
- # Default baudrate. The ROM auto-bauds, so we can use more or less whatever we want.
- ESP_ROM_BAUD = 115200
-
- # First byte of the application image
- ESP_IMAGE_MAGIC = 0xe9
-
- # Initial state for the checksum routine
- ESP_CHECKSUM_MAGIC = 0xef
-
- # OTP ROM addresses
- ESP_OTP_MAC0 = 0x3ff00050
- ESP_OTP_MAC1 = 0x3ff00054
- ESP_OTP_MAC3 = 0x3ff0005c
-
- # Flash sector size, minimum unit of erase.
- ESP_FLASH_SECTOR = 0x1000
-
- def __init__(self, port=None, baud=ESP_ROM_BAUD):
- self._port = FakePort(port)
- self._slip_reader = slip_reader(self._port)
-
- """ Read a SLIP packet from the serial port """
- def read(self):
- return next(self._slip_reader)
-
- """ Write bytes to the serial port while performing SLIP escaping """
- def write(self, packet):
- buf = '\xc0' \
- + (packet.replace('\xdb','\xdb\xdd').replace('\xc0','\xdb\xdc')) \
- + '\xc0'
- self._port.write(buf)
-
- """ Calculate checksum of a blob, as it is defined by the ROM """
- @staticmethod
- def checksum(data, state=ESP_CHECKSUM_MAGIC):
- for b in data:
- state ^= ord(b)
- return state
-
- """ Send a request and read the response """
- def command(self, op=None, data=None, chk=0):
- if op is not None:
- pkt = struct.pack('> 16) & 0xff, (mac3 >> 8) & 0xff, mac3 & 0xff)
- elif ((mac1 >> 16) & 0xff) == 0:
- oui = (0x18, 0xfe, 0x34)
- elif ((mac1 >> 16) & 0xff) == 1:
- oui = (0xac, 0xd0, 0x74)
- else:
- raise FatalError("Unknown OUI")
- return oui + ((mac1 >> 8) & 0xff, mac1 & 0xff, (mac0 >> 24) & 0xff)
-
- """ Read Chip ID from OTP ROM - see http://esp8266-re.foogod.com/wiki/System_get_chip_id_%28IoT_RTOS_SDK_0.9.9%29 """
- def chip_id(self):
- id0 = self.read_reg(self.ESP_OTP_MAC0)
- id1 = self.read_reg(self.ESP_OTP_MAC1)
- return (id0 >> 24) | ((id1 & 0xffffff) << 8)
-
- """ Read SPI flash manufacturer and device id """
- def flash_id(self):
- self.flash_begin(0, 0)
- self.write_reg(0x60000240, 0x0, 0xffffffff)
- self.write_reg(0x60000200, 0x10000000, 0xffffffff)
- flash_id = self.read_reg(0x60000240)
- return flash_id
-
- """ Abuse the loader protocol to force flash to be left in write mode """
- def flash_unlock_dio(self):
- # Enable flash write mode
- self.flash_begin(0, 0)
- # Reset the chip rather than call flash_finish(), which would have
- # write protected the chip again (why oh why does it do that?!)
- self.mem_begin(0,0,0,0x40100000)
- self.mem_finish(0x40000080)
-
- """ Perform a chip erase of SPI flash """
- def flash_erase(self):
- # Trick ROM to initialize SFlash
- self.flash_begin(0, 0)
-
- # This is hacky: we don't have a custom stub, instead we trick
- # the bootloader to jump to the SPIEraseChip() routine and then halt/crash
- # when it tries to boot an unconfigured system.
- self.mem_begin(0,0,0,0x40100000)
- self.mem_finish(0x40004984)
-
- # Yup - there's no good way to detect if we succeeded.
- # It it on the other hand unlikely to fail.
-
- def run_stub(self, stub, params, read_output=True):
- stub = dict(stub)
- stub['code'] = unhexify(stub['code'])
- if 'data' in stub:
- stub['data'] = unhexify(stub['data'])
-
- if stub['num_params'] != len(params):
- raise FatalError('Stub requires %d params, %d provided'
- % (stub['num_params'], len(params)))
-
- params = struct.pack('<' + ('I' * stub['num_params']), *params)
- pc = params + stub['code']
-
- # Upload
- self.mem_begin(len(pc), 1, len(pc), stub['params_start'])
- self.mem_block(pc, 0)
- if 'data' in stub:
- self.mem_begin(len(stub['data']), 1, len(stub['data']), stub['data_start'])
- self.mem_block(stub['data'], 0)
- self.mem_finish(stub['entry'])
-
- if read_output:
- print('Stub executed, reading response:')
- while True:
- p = self.read()
- print(hexify(p))
- if p == '':
- return
-
-
-class ESPBOOTLOADER(object):
- """ These are constants related to software ESP bootloader, working with 'v2' image files """
-
- # First byte of the "v2" application image
- IMAGE_V2_MAGIC = 0xea
-
- # First 'segment' value in a "v2" application image, appears to be a constant version value?
- IMAGE_V2_SEGMENT = 4
-
-
-def LoadFirmwareImage(filename):
- """ Load a firmware image, without knowing what kind of file (v1 or v2) it is.
-
- Returns a BaseFirmwareImage subclass, either ESPFirmwareImage (v1) or OTAFirmwareImage (v2).
- """
- with open(filename, 'rb') as f:
- magic = ord(f.read(1))
- f.seek(0)
- if magic == ESPROM.ESP_IMAGE_MAGIC:
- return ESPFirmwareImage(f)
- elif magic == ESPBOOTLOADER.IMAGE_V2_MAGIC:
- return OTAFirmwareImage(f)
- else:
- raise FatalError("Invalid image magic number: %d" % magic)
-
-
-class BaseFirmwareImage(object):
- """ Base class with common firmware image functions """
- def __init__(self):
- self.segments = []
- self.entrypoint = 0
-
- def add_segment(self, addr, data, pad_to=4):
- """ Add a segment to the image, with specified address & data
- (padded to a boundary of pad_to size) """
- # Data should be aligned on word boundary
- l = len(data)
- if l % pad_to:
- data += b"\x00" * (pad_to - l % pad_to)
- if l > 0:
- self.segments.append((addr, len(data), data))
-
- def load_segment(self, f, is_irom_segment=False):
- """ Load the next segment from the image file """
- (offset, size) = struct.unpack(' 0x40200000 or offset < 0x3ffe0000 or size > 65536:
- raise FatalError('Suspicious segment 0x%x, length %d' % (offset, size))
- segment_data = f.read(size)
- if len(segment_data) < size:
- raise FatalError('End of file reading segment 0x%x, length %d (actual length %d)' % (offset, size, len(segment_data)))
- segment = (offset, size, segment_data)
- self.segments.append(segment)
- return segment
-
- def save_segment(self, f, segment, checksum=None):
- """ Save the next segment to the image file, return next checksum value if provided """
- (offset, size, data) = segment
- f.write(struct.pack(' 16:
- raise FatalError('Invalid firmware image magic=%d segments=%d' % (magic, segments))
-
- for i in range(segments):
- self.load_segment(load_file)
- self.checksum = self.read_checksum(load_file)
-
- def save(self, filename):
- with open(filename, 'wb') as f:
- self.write_v1_header(f, self.segments)
- checksum = ESPROM.ESP_CHECKSUM_MAGIC
- for segment in self.segments:
- checksum = self.save_segment(f, segment, checksum)
- self.append_checksum(f, checksum)
-
-
-class OTAFirmwareImage(BaseFirmwareImage):
- """ 'Version 2' firmware image, segments loaded by software bootloader stub
- (ie Espressif bootloader or rboot)
- """
- def __init__(self, load_file=None):
- super(OTAFirmwareImage, self).__init__()
- self.version = 2
- if load_file is not None:
- (magic, segments, first_flash_mode, first_flash_size_freq, first_entrypoint) = struct.unpack(' 16:
- raise FatalError('Invalid V2 second header magic=%d segments=%d' % (magic, segments))
-
- # load all the usual segments
- for _ in range(segments):
- self.load_segment(load_file)
- self.checksum = self.read_checksum(load_file)
-
- def save(self, filename):
- with open(filename, 'wb') as f:
- # Save first header for irom0 segment
- f.write(struct.pack(' 0:
- esp._port.baudrate = baud_rate
- # Read the greeting.
- p = esp.read()
- if p != 'OHAI':
- raise FatalError('Failed to connect to the flasher (got %s)' % hexify(p))
-
- def flash_write(self, addr, data, show_progress=False):
- assert addr % self._esp.ESP_FLASH_SECTOR == 0, 'Address must be sector-aligned'
- assert len(data) % self._esp.ESP_FLASH_SECTOR == 0, 'Length must be sector-aligned'
- sys.stdout.write('Writing %d @ 0x%x... ' % (len(data), addr))
- sys.stdout.flush()
- self._esp.write(struct.pack(' length:
- raise FatalError('Read more than expected')
- p = self._esp.read()
- if len(p) != 16:
- raise FatalError('Expected digest, got: %s' % hexify(p))
- expected_digest = hexify(p).upper()
- digest = hashlib.md5(data).hexdigest().upper()
- print()
- if digest != expected_digest:
- raise FatalError('Digest mismatch: expected %s, got %s' % (expected_digest, digest))
- p = self._esp.read()
- if len(p) != 1:
- raise FatalError('Expected status, got: %s' % hexify(p))
- status_code = struct.unpack(', ) or a single
-# argument.
-
-def load_ram(esp, args):
- image = LoadFirmwareImage(args.filename)
-
- print('RAM boot...')
- for (offset, size, data) in image.segments:
- print('Downloading %d bytes at %08x...' % (size, offset), end=' ')
- sys.stdout.flush()
- esp.mem_begin(size, div_roundup(size, esp.ESP_RAM_BLOCK), esp.ESP_RAM_BLOCK, offset)
-
- seq = 0
- while len(data) > 0:
- esp.mem_block(data[0:esp.ESP_RAM_BLOCK], seq)
- data = data[esp.ESP_RAM_BLOCK:]
- seq += 1
- print('done!')
-
- print('All segments done, executing at %08x' % image.entrypoint)
- esp.mem_finish(image.entrypoint)
-
-
-def read_mem(esp, args):
- print('0x%08x = 0x%08x' % (args.address, esp.read_reg(args.address)))
-
-
-def write_mem(esp, args):
- esp.write_reg(args.address, args.value, args.mask, 0)
- print('Wrote %08x, mask %08x to %08x' % (args.value, args.mask, args.address))
-
-
-def dump_mem(esp, args):
- f = open(args.filename, 'wb')
- for i in range(args.size / 4):
- d = esp.read_reg(args.address + (i * 4))
- f.write(struct.pack('> 16
- args.flash_size = {18: '2m', 19: '4m', 20: '8m', 21: '16m', 22: '32m'}.get(size_id)
- if args.flash_size is None:
- print('Warning: Could not auto-detect Flash size (FlashID=0x%x, SizeID=0x%x), defaulting to 4m' % (flash_id, size_id))
- args.flash_size = '4m'
- else:
- print('Auto-detected Flash size:', args.flash_size)
-
-
-def write_flash(esp, args):
- detect_flash_size(esp, args)
- flash_mode = {'qio':0, 'qout':1, 'dio':2, 'dout': 3}[args.flash_mode]
- flash_size_freq = {'4m':0x00, '2m':0x10, '8m':0x20, '16m':0x30, '32m':0x40, '16m-c1': 0x50, '32m-c1':0x60, '32m-c2':0x70}[args.flash_size]
- flash_size_freq += {'40m':0, '26m':1, '20m':2, '80m': 0xf}[args.flash_freq]
- flash_params = struct.pack('BB', flash_mode, flash_size_freq)
-
- flasher = CesantaFlasher(esp, args.baud)
-
- for address, argfile in args.addr_filename:
- image = argfile.read()
- argfile.seek(0) # rewind in case we need it again
- if address + len(image) > int(args.flash_size.split('m')[0]) * (1 << 17):
- print('WARNING: Unlikely to work as data goes beyond end of flash. Hint: Use --flash_size')
- # Fix sflash config data.
- if address == 0 and image[0] == '\xe9':
- print('Flash params set to 0x%02x%02x' % (flash_mode, flash_size_freq))
- image = image[0:2] + flash_params + image[4:]
- # Pad to sector size, which is the minimum unit of writing (erasing really).
- if len(image) % esp.ESP_FLASH_SECTOR != 0:
- image += '\xff' * (esp.ESP_FLASH_SECTOR - (len(image) % esp.ESP_FLASH_SECTOR))
- t = time.time()
- flasher.flash_write(address, image, not args.no_progress)
- t = time.time() - t
- print('\rWrote %d bytes at 0x%x in %.1f seconds (%.1f kbit/s)...'
- % (len(image), address, t, len(image) / t * 8 / 1000))
- print('Leaving...')
- if args.verify:
- print('Verifying just-written flash...')
- _verify_flash(flasher, args, flash_params)
- flasher.boot_fw()
-
-
-def image_info(args):
- image = LoadFirmwareImage(args.filename)
- print('Image version: %d' % image.version)
- print(('Entry point: %08x' % image.entrypoint) if image.entrypoint != 0 else 'Entry point not set')
- print('%d segments' % len(image.segments))
- print()
- checksum = ESPROM.ESP_CHECKSUM_MAGIC
- for (idx, (offset, size, data)) in enumerate(image.segments):
- if image.version == 2 and idx == 0:
- print('Segment 1: %d bytes IROM0 (no load address)' % size)
- else:
- print('Segment %d: %5d bytes at %08x' % (idx + 1, size, offset))
- checksum = ESPROM.checksum(data, checksum)
- print()
- print('Checksum: %02x (%s)' % (image.checksum, 'valid' if image.checksum == checksum else 'invalid!'))
-
-
-def make_image(args):
- image = ESPFirmwareImage()
- if len(args.segfile) == 0:
- raise FatalError('No segments specified')
- if len(args.segfile) != len(args.segaddr):
- raise FatalError('Number of specified files does not match number of specified addresses')
- for (seg, addr) in zip(args.segfile, args.segaddr):
- data = open(seg, 'rb').read()
- image.add_segment(addr, data)
- image.entrypoint = args.entrypoint
- image.save(args.output)
-
-
-def elf2image(args):
- e = ELFFile(args.input)
- if args.version == '1':
- image = ESPFirmwareImage()
- else:
- image = OTAFirmwareImage()
- irom_data = e.load_section('.irom0.text')
- if len(irom_data) == 0:
- raise FatalError(".irom0.text section not found in ELF file - can't create V2 image.")
- image.add_segment(0, irom_data, 16)
- image.entrypoint = e.get_entry_point()
- for section, start in ((".text", "_text_start"), (".data", "_data_start"), (".rodata", "_rodata_start")):
- data = e.load_section(section)
- image.add_segment(e.get_symbol_addr(start), data)
-
- image.flash_mode = {'qio':0, 'qout':1, 'dio':2, 'dout': 3}[args.flash_mode]
- image.flash_size_freq = {'4m':0x00, '2m':0x10, '8m':0x20, '16m':0x30, '32m':0x40, '16m-c1': 0x50, '32m-c1':0x60, '32m-c2':0x70}[args.flash_size]
- image.flash_size_freq += {'40m':0, '26m':1, '20m':2, '80m': 0xf}[args.flash_freq]
-
- irom_offs = e.get_symbol_addr("_irom0_text_start") - 0x40200000
-
- if args.version == '1':
- if args.output is None:
- args.output = args.input + '-'
- image.save(args.output + "0x00000.bin")
- data = e.load_section(".irom0.text")
- if irom_offs < 0:
- raise FatalError('Address of symbol _irom0_text_start in ELF is located before flash mapping address. Bad linker script?')
- if (irom_offs & 0xFFF) != 0: # irom0 isn't flash sector aligned
- print("WARNING: irom0 section offset is 0x%08x. ELF is probably linked for 'elf2image --version=2'" % irom_offs)
- with open(args.output + "0x%05x.bin" % irom_offs, "wb") as f:
- f.write(data)
- f.close()
- else: # V2 OTA image
- if args.output is None:
- args.output = "%s-0x%05x.bin" % (os.path.splitext(args.input)[0], irom_offs & ~(ESPROM.ESP_FLASH_SECTOR - 1))
- image.save(args.output)
-
-
-def read_mac(esp, args):
- mac = esp.read_mac()
- print('MAC: %s' % ':'.join(['%02x' % x for x in mac]))
-
-
-def chip_id(esp, args):
- chipid = esp.chip_id()
- print('Chip ID: 0x%08x' % chipid)
-
-
-def erase_flash(esp, args):
- flasher = CesantaFlasher(esp, args.baud)
- print('Erasing flash (this may take a while)...')
- t = time.time()
- flasher.flash_erase_chip()
- t = time.time() - t
- print('Erase took %.1f seconds' % t)
-
-
-def run(esp, args):
- esp.run()
-
-
-def flash_id(esp, args):
- flash_id = esp.flash_id()
- esp.flash_finish(False)
- print('Manufacturer: %02x' % (flash_id & 0xff))
- print('Device: %02x%02x' % ((flash_id >> 8) & 0xff, (flash_id >> 16) & 0xff))
-
-
-def read_flash(esp, args):
- flasher = CesantaFlasher(esp, args.baud)
- t = time.time()
- data = flasher.flash_read(args.address, args.size, not args.no_progress)
- t = time.time() - t
- print('\rRead %d bytes at 0x%x in %.1f seconds (%.1f kbit/s)...'
- % (len(data), args.address, t, len(data) / t * 8 / 1000))
- open(args.filename, 'wb').write(data)
-
-
-def _verify_flash(flasher, args, flash_params=None):
- differences = False
- for address, argfile in args.addr_filename:
- image = argfile.read()
- argfile.seek(0) # rewind in case we need it again
- if address == 0 and image[0] == '\xe9' and flash_params is not None:
- image = image[0:2] + flash_params + image[4:]
- image_size = len(image)
- print('Verifying 0x%x (%d) bytes @ 0x%08x in flash against %s...' % (image_size, image_size, address, argfile.name))
- # Try digest first, only read if there are differences.
- digest, _ = flasher.flash_digest(address, image_size)
- digest = hexify(digest).upper()
- expected_digest = hashlib.md5(image).hexdigest().upper()
- if digest == expected_digest:
- print('-- verify OK (digest matched)')
- continue
- else:
- differences = True
- if getattr(args, 'diff', 'no') != 'yes':
- print('-- verify FAILED (digest mismatch)')
- continue
-
- flash = flasher.flash_read(address, image_size)
- assert flash != image
- diff = [i for i in range(image_size) if flash[i] != image[i]]
- print('-- verify FAILED: %d differences, first @ 0x%08x' % (len(diff), address + diff[0]))
- for d in diff:
- print(' %08x %02x %02x' % (address + d, ord(flash[d]), ord(image[d])))
- if differences:
- raise FatalError("Verify failed.")
-
-
-def verify_flash(esp, args, flash_params=None):
- flasher = CesantaFlasher(esp)
- _verify_flash(flasher, args, flash_params)
-
-
-def version(args):
- print(__version__)
-
-#
-# End of operations functions
-#
-
-
-def main():
- parser = argparse.ArgumentParser(description='esptool.py v%s - ESP8266 ROM Bootloader Utility' % __version__, prog='esptool')
-
- parser.add_argument(
- '--port', '-p',
- help='Serial port device',
- default=os.environ.get('ESPTOOL_PORT', None))
-
- parser.add_argument(
- '--baud', '-b',
- help='Serial port baud rate used when flashing/reading',
- type=arg_auto_int,
- default=os.environ.get('ESPTOOL_BAUD', ESPROM.ESP_ROM_BAUD))
-
- subparsers = parser.add_subparsers(
- dest='operation',
- help='Run esptool {command} -h for additional help')
-
- parser_load_ram = subparsers.add_parser(
- 'load_ram',
- help='Download an image to RAM and execute')
- parser_load_ram.add_argument('filename', help='Firmware image')
-
- parser_dump_mem = subparsers.add_parser(
- 'dump_mem',
- help='Dump arbitrary memory to disk')
- parser_dump_mem.add_argument('address', help='Base address', type=arg_auto_int)
- parser_dump_mem.add_argument('size', help='Size of region to dump', type=arg_auto_int)
- parser_dump_mem.add_argument('filename', help='Name of binary dump')
-
- parser_read_mem = subparsers.add_parser(
- 'read_mem',
- help='Read arbitrary memory location')
- parser_read_mem.add_argument('address', help='Address to read', type=arg_auto_int)
-
- parser_write_mem = subparsers.add_parser(
- 'write_mem',
- help='Read-modify-write to arbitrary memory location')
- parser_write_mem.add_argument('address', help='Address to write', type=arg_auto_int)
- parser_write_mem.add_argument('value', help='Value', type=arg_auto_int)
- parser_write_mem.add_argument('mask', help='Mask of bits to write', type=arg_auto_int)
-
- def add_spi_flash_subparsers(parent, auto_detect=False):
- """ Add common parser arguments for SPI flash properties """
- parent.add_argument('--flash_freq', '-ff', help='SPI Flash frequency',
- choices=['40m', '26m', '20m', '80m'],
- default=os.environ.get('ESPTOOL_FF', '40m'))
- parent.add_argument('--flash_mode', '-fm', help='SPI Flash mode',
- choices=['qio', 'qout', 'dio', 'dout'],
- default=os.environ.get('ESPTOOL_FM', 'qio'))
- choices = ['4m', '2m', '8m', '16m', '32m', '16m-c1', '32m-c1', '32m-c2']
- default = '4m'
- if auto_detect:
- default = 'detect'
- choices.insert(0, 'detect')
- parent.add_argument('--flash_size', '-fs', help='SPI Flash size in Mbit', type=str.lower,
- choices=choices,
- default=os.environ.get('ESPTOOL_FS', default))
-
- parser_write_flash = subparsers.add_parser(
- 'write_flash',
- help='Write a binary blob to flash')
- parser_write_flash.add_argument('addr_filename', metavar=' ', help='Address followed by binary filename, separated by space',
- action=AddrFilenamePairAction)
- add_spi_flash_subparsers(parser_write_flash, auto_detect=True)
- parser_write_flash.add_argument('--no-progress', '-p', help='Suppress progress output', action="store_true")
- parser_write_flash.add_argument('--verify', help='Verify just-written data (only necessary if very cautious, data is already CRCed', action='store_true')
-
- subparsers.add_parser(
- 'run',
- help='Run application code in flash')
-
- parser_image_info = subparsers.add_parser(
- 'image_info',
- help='Dump headers from an application image')
- parser_image_info.add_argument('filename', help='Image file to parse')
-
- parser_make_image = subparsers.add_parser(
- 'make_image',
- help='Create an application image from binary files')
- parser_make_image.add_argument('output', help='Output image file')
- parser_make_image.add_argument('--segfile', '-f', action='append', help='Segment input file')
- parser_make_image.add_argument('--segaddr', '-a', action='append', help='Segment base address', type=arg_auto_int)
- parser_make_image.add_argument('--entrypoint', '-e', help='Address of entry point', type=arg_auto_int, default=0)
-
- parser_elf2image = subparsers.add_parser(
- 'elf2image',
- help='Create an application image from ELF file')
- parser_elf2image.add_argument('input', help='Input ELF file')
- parser_elf2image.add_argument('--output', '-o', help='Output filename prefix (for version 1 image), or filename (for version 2 single image)', type=str)
- parser_elf2image.add_argument('--version', '-e', help='Output image version', choices=['1','2'], default='1')
- add_spi_flash_subparsers(parser_elf2image)
-
- subparsers.add_parser(
- 'read_mac',
- help='Read MAC address from OTP ROM')
-
- subparsers.add_parser(
- 'chip_id',
- help='Read Chip ID from OTP ROM')
-
- subparsers.add_parser(
- 'flash_id',
- help='Read SPI flash manufacturer and device ID')
-
- parser_read_flash = subparsers.add_parser(
- 'read_flash',
- help='Read SPI flash content')
- parser_read_flash.add_argument('address', help='Start address', type=arg_auto_int)
- parser_read_flash.add_argument('size', help='Size of region to dump', type=arg_auto_int)
- parser_read_flash.add_argument('filename', help='Name of binary dump')
- parser_read_flash.add_argument('--no-progress', '-p', help='Suppress progress output', action="store_true")
-
- parser_verify_flash = subparsers.add_parser(
- 'verify_flash',
- help='Verify a binary blob against flash')
- parser_verify_flash.add_argument('addr_filename', help='Address and binary file to verify there, separated by space',
- action=AddrFilenamePairAction)
- parser_verify_flash.add_argument('--diff', '-d', help='Show differences',
- choices=['no', 'yes'], default='no')
-
- subparsers.add_parser(
- 'erase_flash',
- help='Perform Chip Erase on SPI flash')
-
- subparsers.add_parser(
- 'version', help='Print esptool version')
-
- # internal sanity check - every operation matches a module function of the same name
- for operation in list(subparsers.choices.keys()):
- assert operation in globals(), "%s should be a module function" % operation
-
- args = parser.parse_args()
-
- print('esptool.py v%s' % __version__)
-
- # operation function can take 1 arg (args), 2 args (esp, arg)
- # or be a member function of the ESPROM class.
-
- operation_func = globals()[args.operation]
- operation_args,_,_,_ = inspect.getargspec(operation_func)
- if operation_args[0] == 'esp': # operation function takes an ESPROM connection object
- initial_baud = min(ESPROM.ESP_ROM_BAUD, args.baud) # don't sync faster than the default baud rate
- esp = ESPROM(args.port, initial_baud)
- esp.connect()
- operation_func(esp, args)
- else:
- operation_func(args)
-
-
-class AddrFilenamePairAction(argparse.Action):
- """ Custom parser class for the address/filename pairs passed as arguments """
- def __init__(self, option_strings, dest, nargs='+', **kwargs):
- super(AddrFilenamePairAction, self).__init__(option_strings, dest, nargs, **kwargs)
-
- def __call__(self, parser, namespace, values, option_string=None):
- # validate pair arguments
- pairs = []
- for i in range(0,len(values),2):
- try:
- address = int(values[i],0)
- except ValueError:
- raise argparse.ArgumentError(self,'Address "%s" must be a number' % values[i])
- try:
- argfile = open(values[i + 1], 'rb')
- except IOError as e:
- raise argparse.ArgumentError(self, e)
- except IndexError:
- raise argparse.ArgumentError(self,'Must be pairs of an address and the binary filename to write there')
- pairs.append((address, argfile))
- setattr(namespace, self.dest, pairs)
-
-# This is "wrapped" stub_flasher.c, to be loaded using run_stub.
-_CESANTA_FLASHER_STUB = """\
-{"code_start": 1074790404, "code": "080000601C000060000000601000006031FCFF71FCFF\
-81FCFFC02000680332D218C020004807404074DCC48608005823C0200098081BA5A9239245005803\
-1B555903582337350129230B446604DFC6F3FF21EEFFC0200069020DF0000000010078480040004A\
-0040B449004012C1F0C921D911E901DD0209312020B4ED033C2C56C2073020B43C3C56420701F5FF\
-C000003C4C569206CD0EEADD860300202C4101F1FFC0000056A204C2DCF0C02DC0CC6CCAE2D1EAFF\
-0606002030F456D3FD86FBFF00002020F501E8FFC00000EC82D0CCC0C02EC0C73DEB2ADC46030020\
-2C4101E1FFC00000DC42C2DCF0C02DC056BCFEC602003C5C8601003C6C4600003C7C08312D0CD811\
-C821E80112C1100DF0000C180000140010400C0000607418000064180000801800008C1800008418\
-0000881800009018000018980040880F0040A80F0040349800404C4A0040740F0040800F0040980F\
-00400099004012C1E091F5FFC961CD0221EFFFE941F9310971D9519011C01A223902E2D1180C0222\
-6E1D21E4FF31E9FF2AF11A332D0F42630001EAFFC00000C030B43C2256A31621E1FF1A2228022030\
-B43C3256B31501ADFFC00000DD023C4256ED1431D6FF4D010C52D90E192E126E0101DDFFC0000021\
-D2FF32A101C020004802303420C0200039022C0201D7FFC00000463300000031CDFF1A333803D023\
-C03199FF27B31ADC7F31CBFF1A3328030198FFC0000056C20E2193FF2ADD060E000031C6FF1A3328\
-030191FFC0000056820DD2DD10460800000021BEFF1A2228029CE231BCFFC020F51A33290331BBFF\
-C02C411A332903C0F0F4222E1D22D204273D9332A3FFC02000280E27B3F721ABFF381E1A2242A400\
-01B5FFC00000381E2D0C42A40001B3FFC0000056120801B2FFC00000C02000280EC2DC0422D2FCC0\
-2000290E01ADFFC00000222E1D22D204226E1D281E22D204E7B204291E860000126E012198FF32A0\
-042A21C54C003198FF222E1D1A33380337B202C6D6FF2C02019FFFC000002191FF318CFF1A223A31\
-019CFFC00000218DFF1C031A22C549000C02060300003C528601003C624600003C72918BFF9A1108\
-71C861D851E841F83112C1200DF00010000068100000581000007010000074100000781000007C10\
-0000801000001C4B0040803C004091FDFF12C1E061F7FFC961E941F9310971D9519011C01A662906\
-21F3FFC2D1101A22390231F2FF0C0F1A33590331EAFFF26C1AED045C2247B3028636002D0C016DFF\
-C0000021E5FF41EAFF2A611A4469040622000021E4FF1A222802F0D2C0D7BE01DD0E31E0FF4D0D1A\
-3328033D0101E2FFC00000561209D03D2010212001DFFFC000004D0D2D0C3D01015DFFC0000041D5\
-FFDAFF1A444804D0648041D2FF1A4462640061D1FF106680622600673F1331D0FF10338028030C43\
-853A002642164613000041CAFF222C1A1A444804202FC047328006F6FF222C1A273F3861C2FF222C\
-1A1A6668066732B921BDFF3D0C1022800148FFC0000021BAFF1C031A2201BFFFC000000C02460300\
-5C3206020000005C424600005C5291B7FF9A110871C861D851E841F83112C1200DF0B0100000C010\
-0000D010000012C1E091FEFFC961D951E9410971F931CD039011C0ED02DD0431A1FF9C1422A06247\
-B302062D0021F4FF1A22490286010021F1FF1A223902219CFF2AF12D0F011FFFC00000461C0022D1\
-10011CFFC0000021E9FFFD0C1A222802C7B20621E6FF1A22F8022D0E3D014D0F0195FFC000008C52\
-22A063C6180000218BFF3D01102280F04F200111FFC00000AC7D22D1103D014D0F010DFFC0000021\
-D6FF32D110102280010EFFC0000021D3FF1C031A220185FFC00000FAEEF0CCC056ACF821CDFF317A\
-FF1A223A310105FFC0000021C9FF1C031A22017CFFC000002D0C91C8FF9A110871C861D851E841F8\
-3112C1200DF0000200600000001040020060FFFFFF0012C1E00C02290131FAFF21FAFF026107C961\
-C02000226300C02000C80320CC10564CFF21F5FFC02000380221F4FF20231029010C432D010163FF\
-C0000008712D0CC86112C1200DF00080FE3F8449004012C1D0C9A109B17CFC22C1110C13C51C0026\
-1202463000220111C24110B68202462B0031F5FF3022A02802A002002D011C03851A0066820A2801\
-32210105A6FF0607003C12C60500000010212032A01085180066A20F2221003811482105B3FF2241\
-10861A004C1206FDFF2D011C03C5160066B20E280138114821583185CFFF06F7FF005C1286F5FF00\
-10212032A01085140066A20D2221003811482105E1FF06EFFF0022A06146EDFF45F0FFC6EBFF0000\
-01D2FFC0000006E9FF000C022241100C1322C110C50F00220111060600000022C1100C13C50E0022\
-011132C2FA303074B6230206C8FF08B1C8A112C1300DF0000000000010404F484149007519031027\
-000000110040A8100040BC0F0040583F0040CC2E00401CE20040D83900408000004021F4FF12C1E0\
-C961C80221F2FF097129010C02D951C91101F4FFC0000001F3FFC00000AC2C22A3E801F2FFC00000\
-21EAFFC031412A233D0C01EFFFC000003D0222A00001EDFFC00000C1E4FF2D0C01E8FFC000002D01\
-32A004450400C5E7FFDD022D0C01E3FFC00000666D1F4B2131DCFF4600004B22C0200048023794F5\
-31D9FFC0200039023DF08601000001DCFFC000000871C861D85112C1200DF000000012C1F0026103\
-01EAFEC00000083112C1100DF000643B004012C1D0E98109B1C9A1D991F97129013911E2A0C001FA\
-FFC00000CD02E792F40C0DE2A0C0F2A0DB860D00000001F4FFC00000204220E71240F7921C226102\
-01EFFFC0000052A0DC482157120952A0DD571205460500004D0C3801DA234242001BDD3811379DC5\
-C6000000000C0DC2A0C001E3FFC00000C792F608B12D0DC8A1D891E881F87112C1300DF00000", "\
-entry": 1074792180, "num_params": 1, "params_start": 1074790400, "data": "FE0510\
-401A0610403B0610405A0610407A061040820610408C0610408C061040", "data_start": 10736\
-43520}
-"""
-
-if __name__ == '__main__':
- try:
- main()
- except FatalError as e:
- print('\nA fatal error occurred: %s' % e)
- sys.exit(2)
diff --git a/panda/python/flash_release.py b/panda/python/flash_release.py
index 961a83a0d..6e77d6a7f 100755
--- a/panda/python/flash_release.py
+++ b/panda/python/flash_release.py
@@ -7,7 +7,7 @@ import json
import io
def flash_release(path=None, st_serial=None):
- from panda import Panda, PandaDFU, ESPROM, CesantaFlasher
+ from panda import Panda, PandaDFU
from zipfile import ZipFile
def status(x):
@@ -23,33 +23,28 @@ def flash_release(path=None, st_serial=None):
st_serial = panda_list[0]
print("Using panda with serial %s" % st_serial)
- if path is not None:
+ if path is None:
print("Fetching latest firmware from github.com/commaai/panda-artifacts")
r = requests.get("https://raw.githubusercontent.com/commaai/panda-artifacts/master/latest.json")
url = json.loads(r.text)['url']
r = requests.get(url)
print("Fetching firmware from %s" % url)
- path = io.StringIO(r.content)
+ path = io.BytesIO(r.content)
zf = ZipFile(path)
zf.printdir()
- version = zf.read("version")
- status("0. Preparing to flash " + version)
+ version = zf.read("version").decode()
+ status("0. Preparing to flash " + str(version))
code_bootstub = zf.read("bootstub.panda.bin")
code_panda = zf.read("panda.bin")
- code_boot_15 = zf.read("boot_v1.5.bin")
- code_boot_15 = code_boot_15[0:2] + "\x00\x30" + code_boot_15[4:]
-
- code_user1 = zf.read("user1.bin")
- code_user2 = zf.read("user2.bin")
-
# enter DFU mode
status("1. Entering DFU mode")
panda = Panda(st_serial)
- panda.enter_bootloader()
+ panda.reset(enter_bootstub=True)
+ panda.reset(enter_bootloader=True)
time.sleep(1)
# program bootstub
@@ -64,29 +59,8 @@ def flash_release(path=None, st_serial=None):
panda.flash(code=code_panda)
panda.close()
- # flashing ESP
- if panda.is_white():
- status("4. Flashing ESP (slow!)")
-
- def align(x, sz=0x1000):
- x + "\xFF" * ((sz - len(x)) % sz)
-
- esp = ESPROM(st_serial)
- esp.connect()
- flasher = CesantaFlasher(esp, 230400)
- flasher.flash_write(0x0, align(code_boot_15), True)
- flasher.flash_write(0x1000, align(code_user1), True)
- flasher.flash_write(0x81000, align(code_user2), True)
- flasher.flash_write(0x3FE000, "\xFF" * 0x1000)
- flasher.boot_fw()
- del flasher
- del esp
- time.sleep(1)
- else:
- status("4. No ESP in non-white panda")
-
# check for connection
- status("5. Verifying version")
+ status("4. Verifying version")
panda = Panda(st_serial)
my_version = panda.get_version()
print("dongle id: %s" % panda.get_serial()[0])
diff --git a/rednose/helpers/__init__.py b/rednose/helpers/__init__.py
index 1588a4338..aba6b41fe 100644
--- a/rednose/helpers/__init__.py
+++ b/rednose/helpers/__init__.py
@@ -1,4 +1,5 @@
import os
+import platform
from cffi import FFI
TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates'))
@@ -13,7 +14,8 @@ def write_code(folder, name, code, header):
def load_code(folder, name):
- shared_fn = os.path.join(folder, f"lib{name}.so")
+ shared_ext = "dylib" if platform.system() == "Darwin" else "so"
+ shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}")
header_fn = os.path.join(folder, f"{name}.h")
with open(header_fn) as f:
diff --git a/selfdrive/assets/offroad/circled-checkmark.png b/selfdrive/assets/offroad/circled-checkmark.png
new file mode 100644
index 000000000..bc6b49585
Binary files /dev/null and b/selfdrive/assets/offroad/circled-checkmark.png differ
diff --git a/selfdrive/assets/offroad/icon_app_store.png b/selfdrive/assets/offroad/icon_app_store.png
new file mode 100644
index 000000000..ae0dd95ce
Binary files /dev/null and b/selfdrive/assets/offroad/icon_app_store.png differ
diff --git a/selfdrive/assets/offroad/icon_calibration.png b/selfdrive/assets/offroad/icon_calibration.png
new file mode 100644
index 000000000..c4ee0d63d
Binary files /dev/null and b/selfdrive/assets/offroad/icon_calibration.png differ
diff --git a/selfdrive/assets/offroad/icon_checkmark.png b/selfdrive/assets/offroad/icon_checkmark.png
new file mode 100644
index 000000000..06efdfb0c
Binary files /dev/null and b/selfdrive/assets/offroad/icon_checkmark.png differ
diff --git a/selfdrive/assets/offroad/icon_chevron_right.png b/selfdrive/assets/offroad/icon_chevron_right.png
new file mode 100644
index 000000000..a3aaa7648
Binary files /dev/null and b/selfdrive/assets/offroad/icon_chevron_right.png differ
diff --git a/selfdrive/assets/offroad/icon_connect_app.png b/selfdrive/assets/offroad/icon_connect_app.png
new file mode 100644
index 000000000..cd216d3e6
Binary files /dev/null and b/selfdrive/assets/offroad/icon_connect_app.png differ
diff --git a/selfdrive/assets/offroad/icon_eon.png b/selfdrive/assets/offroad/icon_eon.png
new file mode 100644
index 000000000..72856c4e6
Binary files /dev/null and b/selfdrive/assets/offroad/icon_eon.png differ
diff --git a/selfdrive/assets/offroad/icon_map.png b/selfdrive/assets/offroad/icon_map.png
new file mode 100644
index 000000000..21dd0bacc
Binary files /dev/null and b/selfdrive/assets/offroad/icon_map.png differ
diff --git a/selfdrive/assets/offroad/icon_map_speed.png b/selfdrive/assets/offroad/icon_map_speed.png
new file mode 100644
index 000000000..1eeab8460
Binary files /dev/null and b/selfdrive/assets/offroad/icon_map_speed.png differ
diff --git a/selfdrive/assets/offroad/icon_menu.png b/selfdrive/assets/offroad/icon_menu.png
new file mode 100644
index 000000000..837cf5831
Binary files /dev/null and b/selfdrive/assets/offroad/icon_menu.png differ
diff --git a/selfdrive/assets/offroad/icon_metric.png b/selfdrive/assets/offroad/icon_metric.png
new file mode 100644
index 000000000..eaa2438fa
Binary files /dev/null and b/selfdrive/assets/offroad/icon_metric.png differ
diff --git a/selfdrive/assets/offroad/icon_minus.png b/selfdrive/assets/offroad/icon_minus.png
new file mode 100644
index 000000000..e5327c0d3
Binary files /dev/null and b/selfdrive/assets/offroad/icon_minus.png differ
diff --git a/selfdrive/assets/offroad/icon_monitoring.png b/selfdrive/assets/offroad/icon_monitoring.png
new file mode 100644
index 000000000..05f78811e
Binary files /dev/null and b/selfdrive/assets/offroad/icon_monitoring.png differ
diff --git a/selfdrive/assets/offroad/icon_network.png b/selfdrive/assets/offroad/icon_network.png
new file mode 100644
index 000000000..3236924f4
Binary files /dev/null and b/selfdrive/assets/offroad/icon_network.png differ
diff --git a/selfdrive/assets/offroad/icon_openpilot.png b/selfdrive/assets/offroad/icon_openpilot.png
new file mode 100644
index 000000000..0a90a8791
Binary files /dev/null and b/selfdrive/assets/offroad/icon_openpilot.png differ
diff --git a/selfdrive/assets/offroad/icon_openpilot_mirrored.png b/selfdrive/assets/offroad/icon_openpilot_mirrored.png
new file mode 100644
index 000000000..23a7d5a55
Binary files /dev/null and b/selfdrive/assets/offroad/icon_openpilot_mirrored.png differ
diff --git a/selfdrive/assets/offroad/icon_play_store.png b/selfdrive/assets/offroad/icon_play_store.png
new file mode 100644
index 000000000..1eca9d589
Binary files /dev/null and b/selfdrive/assets/offroad/icon_play_store.png differ
diff --git a/selfdrive/assets/offroad/icon_plus.png b/selfdrive/assets/offroad/icon_plus.png
new file mode 100644
index 000000000..92b448b0b
Binary files /dev/null and b/selfdrive/assets/offroad/icon_plus.png differ
diff --git a/selfdrive/assets/offroad/icon_road.png b/selfdrive/assets/offroad/icon_road.png
new file mode 100644
index 000000000..5868ed1cc
Binary files /dev/null and b/selfdrive/assets/offroad/icon_road.png differ
diff --git a/selfdrive/assets/offroad/icon_settings.png b/selfdrive/assets/offroad/icon_settings.png
new file mode 100644
index 000000000..d0c90a620
Binary files /dev/null and b/selfdrive/assets/offroad/icon_settings.png differ
diff --git a/selfdrive/assets/offroad/icon_shell.png b/selfdrive/assets/offroad/icon_shell.png
new file mode 100644
index 000000000..f1d655416
Binary files /dev/null and b/selfdrive/assets/offroad/icon_shell.png differ
diff --git a/selfdrive/assets/offroad/icon_speed_limit.png b/selfdrive/assets/offroad/icon_speed_limit.png
new file mode 100644
index 000000000..0aa7038f9
Binary files /dev/null and b/selfdrive/assets/offroad/icon_speed_limit.png differ
diff --git a/selfdrive/assets/offroad/icon_user.png b/selfdrive/assets/offroad/icon_user.png
new file mode 100644
index 000000000..9b653cc4b
Binary files /dev/null and b/selfdrive/assets/offroad/icon_user.png differ
diff --git a/selfdrive/assets/offroad/icon_warning.png b/selfdrive/assets/offroad/icon_warning.png
new file mode 100644
index 000000000..50fe82112
Binary files /dev/null and b/selfdrive/assets/offroad/icon_warning.png differ
diff --git a/selfdrive/assets/offroad/illustration_arrow.png b/selfdrive/assets/offroad/illustration_arrow.png
new file mode 100644
index 000000000..221908664
Binary files /dev/null and b/selfdrive/assets/offroad/illustration_arrow.png differ
diff --git a/selfdrive/assets/offroad/illustration_sim_absent.png b/selfdrive/assets/offroad/illustration_sim_absent.png
new file mode 100644
index 000000000..554097409
Binary files /dev/null and b/selfdrive/assets/offroad/illustration_sim_absent.png differ
diff --git a/selfdrive/assets/offroad/illustration_sim_present.png b/selfdrive/assets/offroad/illustration_sim_present.png
new file mode 100644
index 000000000..0856795f0
Binary files /dev/null and b/selfdrive/assets/offroad/illustration_sim_present.png differ
diff --git a/selfdrive/assets/offroad/illustration_training_lane_01.png b/selfdrive/assets/offroad/illustration_training_lane_01.png
new file mode 100644
index 000000000..27d9bcee3
Binary files /dev/null and b/selfdrive/assets/offroad/illustration_training_lane_01.png differ
diff --git a/selfdrive/assets/offroad/illustration_training_lane_02.png b/selfdrive/assets/offroad/illustration_training_lane_02.png
new file mode 100644
index 000000000..4f3e2ef44
Binary files /dev/null and b/selfdrive/assets/offroad/illustration_training_lane_02.png differ
diff --git a/selfdrive/assets/offroad/illustration_training_lead_01.png b/selfdrive/assets/offroad/illustration_training_lead_01.png
new file mode 100644
index 000000000..12f3f6bae
Binary files /dev/null and b/selfdrive/assets/offroad/illustration_training_lead_01.png differ
diff --git a/selfdrive/assets/offroad/illustration_training_lead_02.png b/selfdrive/assets/offroad/illustration_training_lead_02.png
new file mode 100644
index 000000000..26c9ffe71
Binary files /dev/null and b/selfdrive/assets/offroad/illustration_training_lead_02.png differ
diff --git a/selfdrive/assets/offroad/indicator_wifi_0.png b/selfdrive/assets/offroad/indicator_wifi_0.png
new file mode 100644
index 000000000..9cf9762ad
Binary files /dev/null and b/selfdrive/assets/offroad/indicator_wifi_0.png differ
diff --git a/selfdrive/assets/offroad/indicator_wifi_100.png b/selfdrive/assets/offroad/indicator_wifi_100.png
new file mode 100644
index 000000000..dc9f28fab
Binary files /dev/null and b/selfdrive/assets/offroad/indicator_wifi_100.png differ
diff --git a/selfdrive/assets/offroad/indicator_wifi_25.png b/selfdrive/assets/offroad/indicator_wifi_25.png
new file mode 100644
index 000000000..cbf9bc89e
Binary files /dev/null and b/selfdrive/assets/offroad/indicator_wifi_25.png differ
diff --git a/selfdrive/assets/offroad/indicator_wifi_50.png b/selfdrive/assets/offroad/indicator_wifi_50.png
new file mode 100644
index 000000000..8ee118a41
Binary files /dev/null and b/selfdrive/assets/offroad/indicator_wifi_50.png differ
diff --git a/selfdrive/assets/offroad/indicator_wifi_75.png b/selfdrive/assets/offroad/indicator_wifi_75.png
new file mode 100644
index 000000000..bcbebce85
Binary files /dev/null and b/selfdrive/assets/offroad/indicator_wifi_75.png differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 72e81a58b..6f34cbed7 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -20,7 +20,7 @@ from websocket import ABNF, WebSocketTimeoutException, create_connection
import cereal.messaging as messaging
from cereal.services import service_list
-from common import android
+from common.hardware import HARDWARE
from common.api import Api
from common.basedir import PERSIST
from common.params import Params
@@ -132,7 +132,7 @@ def reboot():
def do_reboot():
time.sleep(2)
- android.reboot()
+ HARDWARE.reboot()
threading.Thread(target=do_reboot).start()
@@ -218,21 +218,7 @@ def getSshAuthorizedKeys():
@dispatcher.add_method
def getSimInfo():
- sim_state = android.getprop("gsm.sim.state").split(",")
- network_type = android.getprop("gsm.network.type").split(',')
- mcc_mnc = android.getprop("gsm.sim.operator.numeric") or None
-
- sim_id = android.parse_service_call_string(android.service_call(['iphonesubinfo', '11']))
- cell_data_state = android.parse_service_call_unpack(android.service_call(['phone', '46']), ">q")
- cell_data_connected = (cell_data_state == 2)
-
- return {
- 'sim_id': sim_id,
- 'mcc_mnc': mcc_mnc,
- 'network_type': network_type,
- 'sim_state': sim_state,
- 'data_connected': cell_data_connected
- }
+ return HARDWARE.get_sim_info()
@dispatcher.add_method
diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript
index 0ccbe6bd2..94e630a64 100644
--- a/selfdrive/boardd/SConscript
+++ b/selfdrive/boardd/SConscript
@@ -1,6 +1,6 @@
Import('env', 'common', 'cereal', 'messaging', 'cython_dependencies')
-env.Program('boardd', ['boardd.cc', 'panda.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
+env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
env.Command(['boardd_api_impl.so', 'boardd_api_impl.cpp'],
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index a3bb2b0b4..9823ad985 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -28,6 +28,7 @@
#include "messaging.hpp"
#include "panda.h"
+#include "pigeon.h"
#define MAX_IR_POWER 0.5f
@@ -35,15 +36,6 @@
#define CUTOFF_IL 200
#define SATURATE_IL 1600
#define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a')
-#define VOLTAGE_K 0.091 // LPF gain for 5s tau (dt/tau / (dt/tau + 1))
-
-#ifdef QCOM
-const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30; // turn off charge after 30 hrs
-const float VBATT_START_CHARGING = 11.5;
-const float VBATT_PAUSE_CHARGING = 11.0;
-#endif
-
-namespace {
Panda * panda = NULL;
std::atomic safety_setter_thread_running(false);
@@ -195,13 +187,9 @@ void usb_retry_connect() {
}
void can_recv(PubMaster &pm) {
- uint64_t start_time = nanos_since_boot();
-
// create message
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(start_time);
-
+ MessageBuilder msg;
+ auto event = msg.initEvent();
int recv = panda->can_receive(event);
if (recv){
pm.send("can", msg);
@@ -266,7 +254,7 @@ void can_recv_thread() {
useconds_t sleep = remaining / 1000;
usleep(sleep);
} else {
- LOGW("missed cycle");
+ LOGW("missed cycles (%d) %lld", (int)-1*remaining/dt, remaining);
next_frame_time = cur_time;
}
@@ -280,14 +268,11 @@ void can_health_thread() {
uint32_t no_ignition_cnt = 0;
bool ignition_last = false;
- float voltage_f = 12.5; // filtered voltage
// Broadcast empty health message when panda is not yet connected
while (!panda){
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto healthData = event.initHealth();
+ MessageBuilder msg;
+ auto healthData = msg.initEvent().initHealth();
healthData.setHwType(cereal::HealthData::HwType::UNKNOWN);
pm.send("health", msg);
@@ -296,10 +281,8 @@ void can_health_thread() {
// run at 2hz
while (!do_exit && panda->connected) {
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto healthData = event.initHealth();
+ MessageBuilder msg;
+ auto healthData = msg.initEvent().initHealth();
health_t health = panda->get_health();
@@ -307,8 +290,6 @@ void can_health_thread() {
health.ignition_line = 1;
}
- voltage_f = VOLTAGE_K * (health.voltage / 1000.0) + (1.0 - VOLTAGE_K) * voltage_f; // LPF
-
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
@@ -322,24 +303,6 @@ void can_health_thread() {
no_ignition_cnt += 1;
}
-#ifdef QCOM
- bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
- bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX;
- if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
- std::vector disable_power_down = read_db_bytes("DisablePowerDown");
- if (disable_power_down.size() != 1 || disable_power_down[0] != '1') {
- LOGW("TURN OFF CHARGING!\n");
- panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CLIENT);
- LOGW("POWER DOWN DEVICE\n");
- system("service call power 17 i32 0 i32 1");
- }
- }
- if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) {
- LOGW("TURN ON CHARGING!\n");
- panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP);
- }
-#endif
-
#ifndef __x86_64__
bool power_save_desired = !ignition;
if (health.power_save_enabled != power_save_desired){
@@ -405,7 +368,7 @@ void can_health_thread() {
size_t i = 0;
for (size_t f = size_t(cereal::HealthData::FaultType::RELAY_MALFUNCTION);
- f <= size_t(cereal::HealthData::FaultType::INTERRUPT_RATE_KLINE_INIT); f++){
+ f <= size_t(cereal::HealthData::FaultType::INTERRUPT_RATE_TIM9); f++){
if (fault_bits.test(f)) {
faults.set(i, cereal::HealthData::FaultType(f));
i++;
@@ -421,13 +384,16 @@ void hardware_control_thread() {
LOGD("start hardware control thread");
SubMaster sm({"thermal", "frontFrame"});
- // Only control fan speed on UNO
- if (panda->hw_type != cereal::HealthData::HwType::UNO) return;
+ // Other pandas don't have hardware to control
+ if (panda->hw_type != cereal::HealthData::HwType::UNO && panda->hw_type != cereal::HealthData::HwType::DOS) return;
uint64_t last_front_frame_t = 0;
uint16_t prev_fan_speed = 999;
uint16_t ir_pwr = 0;
uint16_t prev_ir_pwr = 999;
+#ifdef QCOM
+ bool prev_charging_disabled = false;
+#endif
unsigned int cnt = 0;
while (!do_exit && panda->connected) {
@@ -435,11 +401,27 @@ void hardware_control_thread() {
sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update?
if (sm.updated("thermal")){
+ // Fan speed
uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeed();
if (fan_speed != prev_fan_speed || cnt % 100 == 0){
panda->set_fan_speed(fan_speed);
prev_fan_speed = fan_speed;
}
+
+#ifdef QCOM
+ // Charging mode
+ bool charging_disabled = sm["thermal"].getThermal().getChargingDisabled();
+ if (charging_disabled != prev_charging_disabled){
+ if (charging_disabled){
+ panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CLIENT);
+ LOGW("TURN OFF CHARGING!\n");
+ } else {
+ panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP);
+ LOGW("TURN ON CHARGING!\n");
+ }
+ prev_charging_disabled = charging_disabled;
+ }
+#endif
}
if (sm.updated("frontFrame")){
auto event = sm["frontFrame"];
@@ -468,82 +450,11 @@ void hardware_control_thread() {
}
}
-#define pigeon_send(x) _pigeon_send(x, sizeof(x)-1)
-void _pigeon_send(const char *dat, int len) {
- unsigned char a[0x20+1];
- a[0] = 1;
- for (int i=0; iusb_bulk_write(2, a, ll+1);
- }
-}
-
-void pigeon_set_power(int power) {
- panda->usb_write(0xd9, power, 0);
-}
-
-void pigeon_set_baud(int baud) {
- panda->usb_write(0xe2, 1, 0);
- panda->usb_write(0xe4, 1, baud/300);
-}
-
-void pigeon_init() {
- usleep(1000*1000);
- LOGW("panda GPS start");
-
- // power off pigeon
- pigeon_set_power(0);
- usleep(100*1000);
-
- // 9600 baud at init
- pigeon_set_baud(9600);
-
- // power on pigeon
- pigeon_set_power(1);
- usleep(500*1000);
-
- // baud rate upping
- pigeon_send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A");
- usleep(100*1000);
-
- // set baud rate to 460800
- pigeon_set_baud(460800);
- usleep(100*1000);
-
- // init from ubloxd
- // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py
- pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F");
- pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2");
- pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35");
- pigeon_send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80");
- pigeon_send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85");
- pigeon_send("\xB5\x62\x06\x00\x00\x00\x06\x18");
- pigeon_send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22");
- pigeon_send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23");
- pigeon_send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24");
- pigeon_send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10");
- pigeon_send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63");
- pigeon_send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37");
- pigeon_send("\xB5\x62\x06\x24\x00\x00\x2A\x84");
- pigeon_send("\xB5\x62\x06\x23\x00\x00\x29\x81");
- pigeon_send("\xB5\x62\x06\x1E\x00\x00\x24\x72");
- pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51");
- pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
- pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
- pigeon_send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70");
-
- LOGW("panda GPS on");
-}
-
-static void pigeon_publish_raw(PubMaster &pm, unsigned char *dat, int alen) {
+static void pigeon_publish_raw(PubMaster &pm, std::string dat) {
// create message
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto ublox_raw = event.initUbloxRaw(alen);
- memcpy(ublox_raw.begin(), dat, alen);
+ MessageBuilder msg;
+ auto ublox_raw = msg.initEvent().initUbloxRaw(dat.length());
+ memcpy(ublox_raw.begin(), dat.data(), dat.length());
pm.send("ubloxRaw", msg);
}
@@ -555,37 +466,32 @@ void pigeon_thread() {
// ubloxRaw = 8042
PubMaster pm({"ubloxRaw"});
- // run at ~100hz
- unsigned char dat[0x1000];
- uint64_t cnt = 0;
+#ifdef QCOM2
+ Pigeon * pigeon = Pigeon::connect("/dev/ttyHS0");
+#else
+ Pigeon * pigeon = Pigeon::connect(panda);
+#endif
- pigeon_init();
+ pigeon->init();
while (!do_exit && panda->connected) {
- int alen = 0;
- while (alen < 0xfc0) {
- int len = panda->usb_read(0xe0, 1, 0, dat+alen, 0x40);
- if (len <= 0) break;
-
- //printf("got %d\n", len);
- alen += len;
- }
- if (alen > 0) {
- if (dat[0] == (char)0x00){
+ std::string recv = pigeon->receive();
+ if (recv.length() > 0) {
+ if (recv[0] == (char)0x00){
LOGW("received invalid ublox message, resetting panda GPS");
- pigeon_init();
+ pigeon->init();
} else {
- pigeon_publish_raw(pm, dat, alen);
+ pigeon_publish_raw(pm, recv);
}
}
- // 10ms
+ // 10ms - 100 Hz
usleep(10*1000);
- cnt++;
}
+
+ delete pigeon;
}
-}
int main() {
int err;
@@ -606,6 +512,8 @@ int main() {
fake_send = true;
}
+ panda_set_power(true);
+
while (!do_exit){
std::vector threads;
threads.push_back(std::thread(can_health_thread));
diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc
index 8ba9d3d56..2fa3f5b9b 100644
--- a/selfdrive/boardd/can_list_to_can_capnp.cc
+++ b/selfdrive/boardd/can_list_to_can_capnp.cc
@@ -1,10 +1,4 @@
-#include
-#include
-#include
-#include "common/timing.h"
-#include
-#include "cereal/gen/cpp/log.capnp.h"
-#include "cereal/gen/cpp/car.capnp.h"
+#include "messaging.hpp"
typedef struct {
long address;
@@ -16,21 +10,19 @@ typedef struct {
extern "C" {
void can_list_to_can_capnp_cpp(const std::vector &can_list, std::string &out, bool sendCan, bool valid) {
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- event.setValid(valid);
+ MessageBuilder msg;
+ auto event = msg.initEvent(valid);
auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size());
int j = 0;
for (auto it = can_list.begin(); it != can_list.end(); it++, j++) {
- canData[j].setAddress(it->address);
- canData[j].setBusTime(it->busTime);
- canData[j].setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size()));
- canData[j].setSrc(it->src);
+ auto c = canData[j];
+ c.setAddress(it->address);
+ c.setBusTime(it->busTime);
+ c.setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size()));
+ c.setSrc(it->src);
}
- auto words = capnp::messageToFlatArray(msg);
- auto bytes = words.asBytes();
+ auto bytes = msg.toBytes();
out.append((const char *)bytes.begin(), bytes.size());
}
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 5820fa400..21ce0c301 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -2,10 +2,29 @@
#include
#include
+#include
+
#include "common/swaglog.h"
+#include "common/gpio.h"
#include "panda.h"
+void panda_set_power(bool power){
+#ifdef QCOM2
+ int err = 0;
+ err += gpio_init(GPIO_STM_RST_N, true);
+ err += gpio_init(GPIO_STM_BOOT0, true);
+
+ err += gpio_set(GPIO_STM_RST_N, false);
+ err += gpio_set(GPIO_STM_BOOT0, false);
+
+ usleep(100*1000); // 100 ms
+
+ err += gpio_set(GPIO_STM_RST_N, power);
+ assert(err == 0);
+#endif
+}
+
Panda::Panda(){
int err;
@@ -39,8 +58,10 @@ Panda::Panda(){
is_pigeon =
(hw_type == cereal::HealthData::HwType::GREY_PANDA) ||
(hw_type == cereal::HealthData::HwType::BLACK_PANDA) ||
- (hw_type == cereal::HealthData::HwType::UNO);
- has_rtc = (hw_type == cereal::HealthData::HwType::UNO);
+ (hw_type == cereal::HealthData::HwType::UNO) ||
+ (hw_type == cereal::HealthData::HwType::DOS);
+ has_rtc = (hw_type == cereal::HealthData::HwType::UNO) ||
+ (hw_type == cereal::HealthData::HwType::DOS);
return;
@@ -80,6 +101,10 @@ int Panda::usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigne
int err;
const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
+ if (!connected){
+ return LIBUSB_ERROR_NO_DEVICE;
+ }
+
pthread_mutex_lock(&usb_lock);
do {
err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout);
@@ -109,6 +134,10 @@ int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int lengt
int err;
int transferred = 0;
+ if (!connected){
+ return 0;
+ }
+
pthread_mutex_lock(&usb_lock);
do {
// Try sending can messages. If the receive buffer on the panda is full it will NAK
@@ -131,6 +160,10 @@ int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length
int err;
int transferred = 0;
+ if (!connected){
+ return 0;
+ }
+
pthread_mutex_lock(&usb_lock);
do {
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 3586cda74..c3e2dc981 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -34,6 +34,9 @@ struct __attribute__((packed)) health_t {
uint8_t power_save_enabled;
};
+
+void panda_set_power(bool power);
+
class Panda {
private:
libusb_context *ctx = NULL;
diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc
new file mode 100644
index 000000000..4ec7ebf8c
--- /dev/null
+++ b/selfdrive/boardd/pigeon.cc
@@ -0,0 +1,226 @@
+#include
+#include
+#include
+#include
+#include
+
+#include "common/swaglog.h"
+#include "common/gpio.h"
+
+#include "pigeon.h"
+
+// Termios on macos doesn't define all baud rate constants
+#ifndef B460800
+#define B460800 0010004
+#endif
+
+using namespace std::string_literals;
+
+
+Pigeon * Pigeon::connect(Panda * p){
+ PandaPigeon * pigeon = new PandaPigeon();
+ pigeon->connect(p);
+
+ return pigeon;
+}
+
+Pigeon * Pigeon::connect(const char * tty){
+ TTYPigeon * pigeon = new TTYPigeon();
+ pigeon->connect(tty);
+
+ return pigeon;
+}
+
+void Pigeon::init() {
+ usleep(1000*1000);
+ LOGW("panda GPS start");
+
+ // power off pigeon
+ set_power(0);
+ usleep(100*1000);
+
+ // 9600 baud at init
+ set_baud(9600);
+
+ // power on pigeon
+ set_power(1);
+ usleep(500*1000);
+
+ // baud rate upping
+ send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s);
+ usleep(100*1000);
+
+ // set baud rate to 460800
+ set_baud(460800);
+ usleep(100*1000);
+
+ // init from ubloxd
+ // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py
+ send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s);
+ send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"s);
+ send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s);
+ send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s);
+ send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s);
+ send("\xB5\x62\x06\x00\x00\x00\x06\x18"s);
+ send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s);
+ send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23"s);
+ send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s);
+ send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s);
+ send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s);
+ send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s);
+ send("\xB5\x62\x06\x24\x00\x00\x2A\x84"s);
+ send("\xB5\x62\x06\x23\x00\x00\x29\x81"s);
+ send("\xB5\x62\x06\x1E\x00\x00\x24\x72"s);
+ send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s);
+ send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s);
+ send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s);
+ send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s);
+
+ LOGW("panda GPS on");
+}
+
+void PandaPigeon::connect(Panda * p) {
+ panda = p;
+}
+
+void PandaPigeon::set_baud(int baud) {
+ panda->usb_write(0xe2, 1, 0);
+ panda->usb_write(0xe4, 1, baud/300);
+}
+
+void PandaPigeon::send(std::string s) {
+ int len = s.length();
+ const char * dat = s.data();
+
+ unsigned char a[0x20+1];
+ a[0] = 1;
+ for (int i=0; iusb_bulk_write(2, a, ll+1);
+ }
+}
+
+std::string PandaPigeon::receive() {
+ std::string r;
+
+ while (true){
+ unsigned char dat[0x40];
+ int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat));
+ if (len <= 0 || r.length() > 0x1000) break;
+ r.append((char*)dat, len);
+ }
+
+ return r;
+}
+
+void PandaPigeon::set_power(bool power) {
+ panda->usb_write(0xd9, power, 0);
+}
+
+PandaPigeon::~PandaPigeon(){
+}
+
+void handle_tty_issue(int err, const char func[]) {
+ LOGE_100("tty error %d \"%s\" in %s", err, strerror(err), func);
+}
+
+void TTYPigeon::connect(const char * tty) {
+ pigeon_tty_fd = open(tty, O_RDWR);
+ if (pigeon_tty_fd < 0){
+ handle_tty_issue(errno, __func__);
+ assert(pigeon_tty_fd >= 0);
+ }
+ assert(tcgetattr(pigeon_tty_fd, &pigeon_tty) == 0);
+
+ // configure tty
+ pigeon_tty.c_cflag &= ~PARENB; // disable parity
+ pigeon_tty.c_cflag &= ~CSTOPB; // single stop bit
+ pigeon_tty.c_cflag |= CS8; // 8 bits per byte
+ pigeon_tty.c_cflag &= ~CRTSCTS; // no RTS/CTS flow control
+ pigeon_tty.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines
+ pigeon_tty.c_lflag &= ~ICANON; // disable canonical mode
+ pigeon_tty.c_lflag &= ~ISIG; // disable interpretation of INTR, QUIT and SUSP
+ pigeon_tty.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off software flow ctrl
+ pigeon_tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // disable any special handling of received bytes
+ pigeon_tty.c_oflag &= ~OPOST; // prevent special interpretation of output bytes
+ pigeon_tty.c_oflag &= ~ONLCR; // prevent conversion of newline to carriage return/line feed
+
+ // configure blocking behavior
+ pigeon_tty.c_cc[VMIN] = 0; // min amount of characters returned
+ pigeon_tty.c_cc[VTIME] = 0; // max blocking time in s/10 (0=inf)
+
+ assert(tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty) == 0);
+}
+
+void TTYPigeon::set_baud(int baud){
+ speed_t baud_const = 0;
+ switch(baud){
+ case 9600:
+ baud_const = B9600;
+ break;
+ case 460800:
+ baud_const = B460800;
+ break;
+ default:
+ assert(false);
+ }
+
+ // make sure everything is tx'ed before changing baud
+ assert(tcdrain(pigeon_tty_fd) == 0);
+
+ // change baud
+ assert(tcgetattr(pigeon_tty_fd, &pigeon_tty) == 0);
+ assert(cfsetspeed(&pigeon_tty, baud_const) == 0);
+ assert(tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty) == 0);
+
+ // flush
+ assert(tcflush(pigeon_tty_fd, TCIOFLUSH) == 0);
+}
+
+void TTYPigeon::send(std::string s) {
+ int len = s.length();
+ const char * dat = s.data();
+
+ int err = write(pigeon_tty_fd, dat, len);
+ if(err < 0) { handle_tty_issue(err, __func__); }
+ err = tcdrain(pigeon_tty_fd);
+ if(err < 0) { handle_tty_issue(err, __func__); }
+}
+
+std::string TTYPigeon::receive() {
+ std::string r;
+
+ while (true){
+ char dat[0x40];
+ int len = read(pigeon_tty_fd, dat, sizeof(dat));
+ if(len < 0) {
+ handle_tty_issue(len, __func__);
+ } else if (len == 0 || r.length() > 0x1000){
+ break;
+ } else {
+ r.append(dat, len);
+ }
+
+ }
+ return r;
+}
+
+void TTYPigeon::set_power(bool power){
+#ifdef QCOM2
+ int err = 0;
+ err += gpio_init(GPIO_UBLOX_RST_N, true);
+ err += gpio_init(GPIO_UBLOX_SAFEBOOT_N, true);
+ err += gpio_init(GPIO_UBLOX_PWR_EN, true);
+
+ err += gpio_set(GPIO_UBLOX_RST_N, power);
+ err += gpio_set(GPIO_UBLOX_SAFEBOOT_N, power);
+ err += gpio_set(GPIO_UBLOX_PWR_EN, power);
+ assert(err == 0);
+#endif
+}
+
+TTYPigeon::~TTYPigeon(){
+ close(pigeon_tty_fd);
+}
diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h
new file mode 100644
index 000000000..667ac7061
--- /dev/null
+++ b/selfdrive/boardd/pigeon.h
@@ -0,0 +1,43 @@
+#pragma once
+#include
+#include
+
+
+#include "panda.h"
+
+class Pigeon {
+ public:
+ static Pigeon* connect(Panda * p);
+ static Pigeon* connect(const char * tty);
+ virtual ~Pigeon(){};
+
+ void init();
+ virtual void set_baud(int baud) = 0;
+ virtual void send(std::string s) = 0;
+ virtual std::string receive() = 0;
+ virtual void set_power(bool power) = 0;
+};
+
+class PandaPigeon : public Pigeon {
+ Panda * panda = NULL;
+public:
+ ~PandaPigeon();
+ void connect(Panda * p);
+ void set_baud(int baud);
+ void send(std::string s);
+ std::string receive();
+ void set_power(bool power);
+};
+
+
+class TTYPigeon : public Pigeon {
+ int pigeon_tty_fd = -1;
+ struct termios pigeon_tty;
+public:
+ ~TTYPigeon();
+ void connect(const char* tty);
+ void set_baud(int baud);
+ void send(std::string s);
+ std::string receive();
+ void set_power(bool power);
+};
diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript
index 1b205198c..7a81b76c8 100644
--- a/selfdrive/camerad/SConscript
+++ b/selfdrive/camerad/SConscript
@@ -11,6 +11,10 @@ if arch == "aarch64":
elif arch == "larch64":
libs += []
cameras = ['cameras/camera_qcom2.c']
+ # no screen
+ # env = env.Clone()
+ # env.Append(CXXFLAGS = '-DNOSCREEN')
+ # env.Append(CFLAGS = '-DNOSCREEN')
else:
if webcam:
libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio']
@@ -20,8 +24,8 @@ else:
env.Append(CFLAGS = '-DWEBCAM')
env.Append(CPPPATH = '/usr/local/include/opencv4')
else:
- libs += []
cameras = ['cameras/camera_frame_stream.cc']
+
if arch == "Darwin":
del libs[libs.index('OpenCL')]
env = env.Clone()
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc
index aecca564a..9a4263103 100644
--- a/selfdrive/camerad/cameras/camera_frame_stream.cc
+++ b/selfdrive/camerad/cameras/camera_frame_stream.cc
@@ -45,7 +45,7 @@ void camera_init(CameraState *s, int camera_id, unsigned int fps) {
tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s);
}
-void run_frame_stream(DualCameraState *s) {
+void run_frame_stream(MultiCameraState *s) {
SubMaster sm({"frame"});
CameraState *const rear_camera = &s->rear;
@@ -93,7 +93,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
},
};
-void cameras_init(DualCameraState *s) {
+void cameras_init(MultiCameraState *s) {
camera_init(&s->rear, CAMERA_ID_IMX298, 20);
s->rear.transform = (mat3){{
1.0, 0.0, 0.0,
@@ -111,7 +111,7 @@ void cameras_init(DualCameraState *s) {
void camera_autoexposure(CameraState *s, float grey_frac) {}
-void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear,
+void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear,
VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats,
VisionBuf *camera_bufs_front) {
assert(camera_bufs_rear);
@@ -124,11 +124,11 @@ void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear,
camera_open(&s->rear, camera_bufs_rear, true);
}
-void cameras_close(DualCameraState *s) {
+void cameras_close(MultiCameraState *s) {
camera_close(&s->rear);
}
-void cameras_run(DualCameraState *s) {
+void cameras_run(MultiCameraState *s) {
set_thread_name("frame_streaming");
run_frame_stream(s);
cameras_close(s);
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h
index 80ff54b5e..a29e40304 100644
--- a/selfdrive/camerad/cameras/camera_frame_stream.h
+++ b/selfdrive/camerad/cameras/camera_frame_stream.h
@@ -40,17 +40,17 @@ typedef struct CameraState {
} CameraState;
-typedef struct DualCameraState {
+typedef struct MultiCameraState {
int ispif_fd;
CameraState rear;
CameraState front;
-} DualCameraState;
+} MultiCameraState;
-void cameras_init(DualCameraState *s);
-void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
-void cameras_run(DualCameraState *s);
-void cameras_close(DualCameraState *s);
+void cameras_init(MultiCameraState *s);
+void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
+void cameras_run(MultiCameraState *s);
+void cameras_close(MultiCameraState *s);
void camera_autoexposure(CameraState *s, float grey_frac);
#ifdef __cplusplus
} // extern "C"
diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc
index dfed65238..27709ef22 100644
--- a/selfdrive/camerad/cameras/camera_qcom.cc
+++ b/selfdrive/camerad/cameras/camera_qcom.cc
@@ -258,7 +258,7 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li
return err;
}
-void cameras_init(DualCameraState *s) {
+void cameras_init(MultiCameraState *s) {
char project_name[1024] = {0};
property_get("ro.boot.project_name", project_name, "");
@@ -545,10 +545,10 @@ static void imx298_ois_calibration(int ois_fd, uint8_t* eeprom) {
-static void sensors_init(DualCameraState *s) {
+static void sensors_init(MultiCameraState *s) {
int err;
- int sensorinit_fd = -1;
+ unique_fd sensorinit_fd;
if (s->device == DEVICE_LP3) {
sensorinit_fd = open("/dev/v4l-subdev11", O_RDWR | O_NONBLOCK);
} else {
@@ -1829,7 +1829,7 @@ static void front_start(CameraState *s) {
-void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) {
+void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) {
int err;
struct ispif_cfg_data ispif_cfg_data;
@@ -1866,13 +1866,13 @@ void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *ca
assert(camera_bufs_rear);
assert(camera_bufs_front);
- int msmcfg_fd = open("/dev/media0", O_RDWR | O_NONBLOCK);
- assert(msmcfg_fd >= 0);
+ s->msmcfg_fd = open("/dev/media0", O_RDWR | O_NONBLOCK);
+ assert(s->msmcfg_fd >= 0);
sensors_init(s);
- int v4l_fd = open("/dev/video0", O_RDWR | O_NONBLOCK);
- assert(v4l_fd >= 0);
+ 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);
@@ -2018,7 +2018,7 @@ static void ops_term() {
static void* ops_thread(void* arg) {
int err;
- DualCameraState *s = (DualCameraState*)arg;
+ MultiCameraState *s = (MultiCameraState*)arg;
set_thread_name("camera_settings");
@@ -2109,7 +2109,7 @@ static void* ops_thread(void* arg) {
return NULL;
}
-void cameras_run(DualCameraState *s) {
+void cameras_run(MultiCameraState *s) {
int err;
pthread_t ops_thread_handle;
@@ -2221,7 +2221,7 @@ void cameras_run(DualCameraState *s) {
cameras_close(s);
}
-void cameras_close(DualCameraState *s) {
+void cameras_close(MultiCameraState *s) {
camera_close(&s->rear);
camera_close(&s->front);
}
diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h
index 812f1a596..02e139d7c 100644
--- a/selfdrive/camerad/cameras/camera_qcom.h
+++ b/selfdrive/camerad/cameras/camera_qcom.h
@@ -15,6 +15,7 @@
#include "common/mat.h"
#include "common/visionbuf.h"
#include "common/buffering.h"
+#include "common/utilpp.h"
#include "camera_common.h"
@@ -69,13 +70,13 @@ typedef struct CameraState {
uint32_t line_length_pclk;
unsigned int max_gain;
- int csid_fd;
- int csiphy_fd;
- int sensor_fd;
- int isp_fd;
- int eeprom_fd;
+ unique_fd csid_fd;
+ unique_fd csiphy_fd;
+ unique_fd sensor_fd;
+ unique_fd isp_fd;
+ unique_fd eeprom_fd;
// rear only
- int ois_fd, actuator_fd;
+ unique_fd ois_fd, actuator_fd;
uint16_t infinity_dac;
struct msm_vfe_axi_stream_cfg_cmd stream_cfg;
@@ -123,19 +124,21 @@ typedef struct CameraState {
} CameraState;
-typedef struct DualCameraState {
+typedef struct MultiCameraState {
int device;
- int ispif_fd;
+ unique_fd ispif_fd;
+ unique_fd msmcfg_fd;
+ unique_fd v4l_fd;
CameraState rear;
CameraState front;
-} DualCameraState;
+} MultiCameraState;
-void cameras_init(DualCameraState *s);
-void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
-void cameras_run(DualCameraState *s);
-void cameras_close(DualCameraState *s);
+void cameras_init(MultiCameraState *s);
+void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
+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);
diff --git a/selfdrive/camerad/cameras/debayer.cl b/selfdrive/camerad/cameras/debayer.cl
index c9e3716df..5188dc88c 100644
--- a/selfdrive/camerad/cameras/debayer.cl
+++ b/selfdrive/camerad/cameras/debayer.cl
@@ -81,28 +81,18 @@ __kernel void debayer10(__global uchar const * const in,
float4 p = convert_float4(pint);
-#if NEW == 1
- // now it's 0x2a
- const float black_level = 42.0f;
- // max on black level
- p = max(0.0, p - black_level);
-#else
// 64 is the black level of the sensor, remove
// (changed to 56 for HDR)
const float black_level = 56.0f;
// TODO: switch to max here?
p = (p - black_level);
-#endif
-
-#if NEW == 0
// correct vignetting (no pow function?)
// see https://www.eecis.udel.edu/~jye/lab_research/09/JiUp.pdf the A (4th order)
const float r = ((oy - RGB_HEIGHT/2)*(oy - RGB_HEIGHT/2) + (ox - RGB_WIDTH/2)*(ox - RGB_WIDTH/2));
const float fake_f = 700.0f; // should be 910, but this fits...
const float lil_a = (1.0f + r/(fake_f*fake_f));
p = p * lil_a * lil_a;
-#endif
// rescale to 1.0
#if HDR
@@ -125,12 +115,8 @@ __kernel void debayer10(__global uchar const * const in,
float3 c1 = (float3)(p.s0, (p.s1+p.s2)/2.0f, p.s3);
#endif
-#if NEW
- // TODO: new color correction
-#else
// color correction
c1 = color_correct(c1);
-#endif
#if HDR
// srgb gamma isn't right for YUV, so it's disabled for now
diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc
index 658ab55ba..c28d6edf5 100644
--- a/selfdrive/camerad/main.cc
+++ b/selfdrive/camerad/main.cc
@@ -1,6 +1,7 @@
#include
#include
#include
+#include
#if defined(QCOM) && !defined(QCOM_REPLAY)
#include "cameras/camera_qcom.h"
@@ -13,6 +14,7 @@
#endif
#include "common/util.h"
+#include "common/params.h"
#include "common/swaglog.h"
#include "common/ipc.h"
@@ -33,8 +35,9 @@
#include
#define UI_BUF_COUNT 4
+#define DEBAYER_LOCAL_WORKSIZE 16
#define YUV_COUNT 40
-#define MAX_CLIENTS 5
+#define MAX_CLIENTS 6
extern "C" {
volatile sig_atomic_t do_exit = 0;
@@ -44,6 +47,10 @@ void set_do_exit(int sig) {
do_exit = 1;
}
+/*
+TODO: refactor out camera specific things from here
+*/
+
struct VisionState;
struct VisionClientState {
@@ -74,8 +81,13 @@ struct VisionState {
cl_program prg_debayer_rear;
cl_program prg_debayer_front;
+ cl_program prg_debayer_wide;
cl_kernel krnl_debayer_rear;
cl_kernel krnl_debayer_front;
+ cl_kernel krnl_debayer_wide;
+ int debayer_cl_localMemSize;
+ size_t debayer_cl_globalWorkSize[2];
+ size_t debayer_cl_localWorkSize[2];
cl_program prg_rgb_laplacian;
cl_kernel krnl_rgb_laplacian;
@@ -88,10 +100,12 @@ struct VisionState {
// processing
TBuffer ui_tb;
TBuffer ui_front_tb;
+ TBuffer ui_wide_tb;
mat3 yuv_transform;
TBuffer *yuv_tb;
TBuffer *yuv_front_tb;
+ TBuffer *yuv_wide_tb;
// TODO: refactor for both cameras?
Pool yuv_pool;
@@ -113,6 +127,15 @@ struct VisionState {
int yuv_front_width, yuv_front_height;
RGBToYUVState front_rgb_to_yuv_state;
+ Pool yuv_wide_pool;
+ VisionBuf yuv_wide_ion[YUV_COUNT];
+ cl_mem yuv_wide_cl[YUV_COUNT];
+ YUVBuf yuv_wide_bufs[YUV_COUNT];
+ FrameMetadata yuv_wide_metas[YUV_COUNT];
+ size_t yuv_wide_buf_size;
+ int yuv_wide_width, yuv_wide_height;
+ RGBToYUVState wide_rgb_to_yuv_state;
+
size_t rgb_buf_size;
int rgb_width, rgb_height, rgb_stride;
VisionBuf rgb_bufs[UI_BUF_COUNT];
@@ -124,11 +147,15 @@ struct VisionState {
int rgb_front_width, rgb_front_height, rgb_front_stride;
VisionBuf rgb_front_bufs[UI_BUF_COUNT];
cl_mem rgb_front_bufs_cl[UI_BUF_COUNT];
+
bool rhd_front;
- bool rhd_front_checked;
int front_meteringbox_xmin, front_meteringbox_xmax;
int front_meteringbox_ymin, front_meteringbox_ymax;
+ size_t rgb_wide_buf_size;
+ int rgb_wide_width, rgb_wide_height, rgb_wide_stride;
+ VisionBuf rgb_wide_bufs[UI_BUF_COUNT];
+ cl_mem rgb_wide_bufs_cl[UI_BUF_COUNT];
cl_mem camera_bufs_cl[FRAME_BUF_COUNT];
VisionBuf camera_bufs[FRAME_BUF_COUNT];
@@ -138,7 +165,11 @@ struct VisionState {
cl_mem front_camera_bufs_cl[FRAME_BUF_COUNT];
VisionBuf front_camera_bufs[FRAME_BUF_COUNT];
- DualCameraState cameras;
+ cl_mem wide_camera_bufs_cl[FRAME_BUF_COUNT];
+ VisionBuf wide_camera_bufs[FRAME_BUF_COUNT];
+
+
+ MultiCameraState cameras;
zsock_t *terminate_pub;
@@ -152,11 +183,13 @@ struct VisionState {
void* frontview_thread(void *arg) {
int err;
VisionState *s = (VisionState*)arg;
+ s->rhd_front = read_db_bool("IsRHD");
set_thread_name("frontview");
+ err = set_realtime_priority(51);
// we subscribe to this for placement of the AE metering box
// TODO: the loop is bad, ideally models shouldn't affect sensors
- SubMaster sm({"driverState", "dMonitoringState"});
+ SubMaster sm({"driverState"});
cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err);
assert(err == 0);
@@ -179,14 +212,20 @@ void* frontview_thread(void *arg) {
assert(err == 0);
err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[rgb_idx]);
assert(err == 0);
+#ifdef QCOM2
+ err = clSetKernelArg(s->krnl_debayer_front, 2, s->debayer_cl_localMemSize, 0);
+ assert(err == 0);
+ err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 2, NULL,
+ s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event);
+#else
float digital_gain = 1.0;
err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain);
assert(err == 0);
-
const size_t debayer_work_size = s->rgb_front_height; // doesn't divide evenly, is this okay?
const size_t debayer_local_work_size = 128;
err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL,
&debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event);
+#endif
assert(err == 0);
} else {
assert(s->rgb_front_buf_size >= s->cameras.front.frame_size);
@@ -197,16 +236,17 @@ void* frontview_thread(void *arg) {
}
clWaitForEvents(1, &debayer_event);
clReleaseEvent(debayer_event);
+
tbuffer_release(&s->cameras.front.camera_tb, buf_idx);
visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE);
sm.update(0);
- // no more check after gps check
- if (!s->rhd_front_checked && sm.updated("dMonitoringState")) {
- auto state = sm["dMonitoringState"].getDMonitoringState();
- s->rhd_front = state.getIsRHD();
- s->rhd_front_checked = state.getRhdChecked();
+
+#ifdef NOSCREEN
+ if (frame_data.frame_id % 4 == 2) {
+ sendrgb(&s->cameras, (uint8_t*) s->rgb_front_bufs[rgb_idx].addr, s->rgb_front_bufs[rgb_idx].len, 2);
}
+#endif
if (sm.updated("driverState")) {
auto state = sm["driverState"].getDriverState();
@@ -241,6 +281,7 @@ void* frontview_thread(void *arg) {
int x_end;
int y_start;
int y_end;
+ int skip = 1;
if (s->front_meteringbox_xmax > 0)
{
@@ -256,9 +297,15 @@ void* frontview_thread(void *arg) {
x_start = s->rhd_front ? 0:s->rgb_front_width * 3 / 5;
x_end = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
}
-
+#ifdef QCOM2
+ x_start = 0.15*s->rgb_front_width;
+ x_end = 0.85*s->rgb_front_width;
+ y_start = 0.5*s->rgb_front_height;
+ y_end = 0.75*s->rgb_front_height;
+ skip = 2;
+#endif
uint32_t lum_binning[256] = {0,};
- for (int y = y_start; y < y_end; ++y) {
+ for (int y = y_start; y < y_end; y += skip) {
for (int x = x_start; x < x_end; x += 2) { // every 2nd col
const uint8_t *pix = &bgr_front_ptr[y * s->rgb_front_stride + x * 3];
unsigned int lum = (unsigned int)pix[0] + pix[1] + pix[2];
@@ -272,7 +319,7 @@ void* frontview_thread(void *arg) {
lum_binning[std::min(lum / 3, 255u)]++;
}
}
- const unsigned int lum_total = (y_end - y_start) * (x_end - x_start)/2;
+ const unsigned int lum_total = (y_end - y_start) * (x_end - x_start) / 2 / skip;
unsigned int lum_cur = 0;
int lum_med = 0;
for (lum_med=0; lum_med<256; lum_med++) {
@@ -300,11 +347,8 @@ void* frontview_thread(void *arg) {
// send frame event
{
if (s->pm != NULL) {
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
-
- auto framed = event.initFrontFrame();
+ MessageBuilder msg;
+ auto framed = msg.initEvent().initFrontFrame();
framed.setFrameId(frame_data.frame_id);
framed.setEncodeId(cnt);
framed.setTimestampEof(frame_data.timestamp_eof);
@@ -336,6 +380,159 @@ void* frontview_thread(void *arg) {
return NULL;
}
+
+#ifdef QCOM2
+// wide
+void* wideview_thread(void *arg) {
+ int err;
+ VisionState *s = (VisionState*)arg;
+
+ set_thread_name("wideview");
+
+ err = set_realtime_priority(51);
+ LOG("setpriority returns %d", err);
+
+ // init cl stuff
+ const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0};
+ cl_command_queue q = clCreateCommandQueueWithProperties(s->context, s->device_id, props, &err);
+ assert(err == 0);
+
+ // init the net
+ LOG("wideview start!");
+
+ for (int cnt = 0; !do_exit; cnt++) {
+ int buf_idx = tbuffer_acquire(&s->cameras.wide.camera_tb);
+ // int buf_idx = camera_acquire_buffer(s);
+ if (buf_idx < 0) {
+ break;
+ }
+
+ double t1 = millis_since_boot();
+
+ FrameMetadata frame_data = s->cameras.wide.camera_bufs_metadata[buf_idx];
+ uint32_t frame_id = frame_data.frame_id;
+
+ if (frame_id == -1) {
+ LOGE("no frame data? wtf");
+ tbuffer_release(&s->cameras.wide.camera_tb, buf_idx);
+ continue;
+ }
+
+ int ui_idx = tbuffer_select(&s->ui_wide_tb);
+ int rgb_idx = ui_idx;
+
+ cl_event debayer_event;
+ if (s->cameras.wide.ci.bayer) {
+ err = clSetKernelArg(s->krnl_debayer_wide, 0, sizeof(cl_mem), &s->wide_camera_bufs_cl[buf_idx]);
+ assert(err == 0);
+ err = clSetKernelArg(s->krnl_debayer_wide, 1, sizeof(cl_mem), &s->rgb_wide_bufs_cl[rgb_idx]);
+ assert(err == 0);
+ err = clSetKernelArg(s->krnl_debayer_wide, 2, s->debayer_cl_localMemSize, 0);
+ assert(err == 0);
+ err = clEnqueueNDRangeKernel(q, s->krnl_debayer_wide, 2, NULL,
+ s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event);
+ assert(err == 0);
+ } else {
+ assert(s->rgb_wide_buf_size >= s->frame_size);
+ assert(s->rgb_stride == s->frame_stride);
+ err = clEnqueueCopyBuffer(q, s->wide_camera_bufs_cl[buf_idx], s->rgb_wide_bufs_cl[rgb_idx],
+ 0, 0, s->rgb_wide_buf_size, 0, 0, &debayer_event);
+ assert(err == 0);
+ }
+
+ clWaitForEvents(1, &debayer_event);
+ clReleaseEvent(debayer_event);
+
+ tbuffer_release(&s->cameras.wide.camera_tb, buf_idx);
+
+ visionbuf_sync(&s->rgb_wide_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE);
+
+#ifdef NOSCREEN
+ if (frame_data.frame_id % 4 == 0) {
+ sendrgb(&s->cameras, (uint8_t*) s->rgb_wide_bufs[rgb_idx].addr, s->rgb_wide_bufs[rgb_idx].len, 1);
+ }
+#endif
+
+ double t2 = millis_since_boot();
+
+ double yt1 = millis_since_boot();
+
+ int yuv_idx = pool_select(&s->yuv_wide_pool);
+
+ s->yuv_wide_metas[yuv_idx] = frame_data;
+
+ uint8_t* yuv_ptr_y = s->yuv_wide_bufs[yuv_idx].y;
+ cl_mem yuv_wide_cl = s->yuv_wide_cl[yuv_idx];
+ rgb_to_yuv_queue(&s->wide_rgb_to_yuv_state, q, s->rgb_wide_bufs_cl[rgb_idx], yuv_wide_cl);
+ visionbuf_sync(&s->yuv_wide_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE);
+
+ double yt2 = millis_since_boot();
+
+ // keep another reference around till were done processing
+ pool_acquire(&s->yuv_wide_pool, yuv_idx);
+ pool_push(&s->yuv_wide_pool, yuv_idx);
+
+ // send frame event
+ {
+ if (s->pm != NULL) {
+ MessageBuilder msg;
+ auto framed = msg.initEvent().initWideFrame();
+ framed.setFrameId(frame_data.frame_id);
+ framed.setEncodeId(cnt);
+ framed.setTimestampEof(frame_data.timestamp_eof);
+ framed.setFrameLength(frame_data.frame_length);
+ framed.setIntegLines(frame_data.integ_lines);
+ framed.setGlobalGain(frame_data.global_gain);
+ framed.setLensPos(frame_data.lens_pos);
+ framed.setLensSag(frame_data.lens_sag);
+ framed.setLensErr(frame_data.lens_err);
+ framed.setLensTruePos(frame_data.lens_true_pos);
+ framed.setGainFrac(frame_data.gain_frac);
+
+ s->pm->send("wideFrame", msg);
+ }
+ }
+
+ tbuffer_dispatch(&s->ui_wide_tb, ui_idx);
+
+ // auto exposure over big box
+ // TODO: fix this? should not use med imo
+ const int exposure_x = 384;
+ const int exposure_y = 300;
+ const int exposure_height = 400;
+ const int exposure_width = 1152;
+ if (cnt % 3 == 0) {
+ // find median box luminance for AE
+ uint32_t lum_binning[256] = {0,};
+ for (int y=0; yyuv_wide_width) + exposure_x + x];
+ lum_binning[lum]++;
+ }
+ }
+ const unsigned int lum_total = exposure_height * exposure_width / 4;
+ unsigned int lum_cur = 0;
+ int lum_med = 0;
+ for (lum_med=0; lum_med<256; lum_med++) {
+ // shouldn't be any values less than 16 - yuv footroom
+ lum_cur += lum_binning[lum_med];
+ if (lum_cur >= lum_total / 2) {
+ break;
+ }
+ }
+
+ camera_autoexposure(&s->cameras.wide, lum_med / 256.0);
+ }
+
+ pool_release(&s->yuv_wide_pool, yuv_idx);
+ double t5 = millis_since_boot();
+ LOGD("queued: %.2fms, yuv: %.2f, | processing: %.3fms", (t2-t1), (yt2-yt1), (t5-t1));
+ }
+
+ return NULL;
+}
+#endif
+
// processing
void* processing_thread(void *arg) {
int err;
@@ -390,13 +587,19 @@ void* processing_thread(void *arg) {
assert(err == 0);
err = clSetKernelArg(s->krnl_debayer_rear, 1, sizeof(cl_mem), &s->rgb_bufs_cl[rgb_idx]);
assert(err == 0);
+#ifdef QCOM2
+ err = clSetKernelArg(s->krnl_debayer_rear, 2, s->debayer_cl_localMemSize, 0);
+ assert(err == 0);
+ err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 2, NULL,
+ s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event);
+#else
err = clSetKernelArg(s->krnl_debayer_rear, 2, sizeof(float), &s->cameras.rear.digital_gain);
assert(err == 0);
-
const size_t debayer_work_size = s->rgb_height; // doesn't divide evenly, is this okay?
const size_t debayer_local_work_size = 128;
err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 1, NULL,
&debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event);
+#endif
assert(err == 0);
} else {
assert(s->rgb_buf_size >= s->frame_size);
@@ -405,7 +608,6 @@ void* processing_thread(void *arg) {
0, 0, s->rgb_buf_size, 0, 0, &debayer_event);
assert(err == 0);
}
-
clWaitForEvents(1, &debayer_event);
clReleaseEvent(debayer_event);
@@ -413,6 +615,12 @@ void* processing_thread(void *arg) {
visionbuf_sync(&s->rgb_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE);
+#ifdef NOSCREEN
+ if (frame_data.frame_id % 4 == 1) {
+ sendrgb(&s->cameras, (uint8_t*) s->rgb_bufs[rgb_idx].addr, s->rgb_bufs[rgb_idx].len, 0);
+ }
+#endif
+
#if defined(QCOM) && !defined(QCOM_REPLAY)
/*FILE *dump_rgb_file = fopen("/tmp/process_dump.rgb", "wb");
fwrite(s->rgb_bufs[rgb_idx].addr, s->rgb_bufs[rgb_idx].len, sizeof(uint8_t), dump_rgb_file);
@@ -485,7 +693,7 @@ void* processing_thread(void *arg) {
// truly stuck, needs help
s->cameras.rear.self_recover -= 1;
if (s->cameras.rear.self_recover < -FOCUS_RECOVER_PATIENCE) {
- LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d",
+ LOGD("rear camera bad state detected. attempting recovery from %.1f, recover state is %d",
lens_true_pos, s->cameras.rear.self_recover.load());
s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); // parity determined by which end is stuck at
}
@@ -495,7 +703,7 @@ void* processing_thread(void *arg) {
// in suboptimal position with high prob, but may still recover by itself
s->cameras.rear.self_recover -= 1;
if (s->cameras.rear.self_recover < -(FOCUS_RECOVER_PATIENCE*3)) {
- LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, s->cameras.rear.self_recover.load());
+ LOGD("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, s->cameras.rear.self_recover.load());
s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0);
}
} else if (s->cameras.rear.self_recover < 0) {
@@ -530,11 +738,8 @@ void* processing_thread(void *arg) {
// send frame event
{
if (s->pm != NULL) {
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
-
- auto framed = event.initFrame();
+ MessageBuilder msg;
+ auto framed = msg.initEvent().initFrame();
framed.setFrameId(frame_data.frame_id);
framed.setEncodeId(cnt);
framed.setTimestampEof(frame_data.timestamp_eof);
@@ -618,11 +823,8 @@ void* processing_thread(void *arg) {
free(row);
jpeg_finish_compress(&cinfo);
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
-
- auto thumbnaild = event.initThumbnail();
+ MessageBuilder msg;
+ auto thumbnaild = msg.initEvent().initThumbnail();
thumbnaild.setFrameId(frame_data.frame_id);
thumbnaild.setTimestampEof(frame_data.timestamp_eof);
thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len));
@@ -638,20 +840,29 @@ void* processing_thread(void *arg) {
tbuffer_dispatch(&s->ui_tb, ui_idx);
// auto exposure over big box
+#ifdef QCOM2
+ const int exposure_x = 384;
+ const int exposure_y = 300;
+ const int exposure_height = 400;
+ const int exposure_width = 1152;
+ const int skip = 2;
+#else
const int exposure_x = 290;
- const int exposure_y = 282 + 40;
+ const int exposure_y = 322;
const int exposure_height = 314;
const int exposure_width = 560;
+ const int skip = 1;
+#endif
if (cnt % 3 == 0) {
// find median box luminance for AE
uint32_t lum_binning[256] = {0,};
- for (int y=0; yyuv_width) + exposure_x + x];
lum_binning[lum]++;
}
}
- const unsigned int lum_total = exposure_height * exposure_width;
+ const unsigned int lum_total = exposure_height * exposure_width / skip / skip;
unsigned int lum_cur = 0;
int lum_med = 0;
for (lum_med=0; lum_med<256; lum_med++) {
@@ -661,8 +872,6 @@ void* processing_thread(void *arg) {
break;
}
}
- // double avg = (double)acc / (big_box_width * big_box_height) - 16;
- // printf("avg %d\n", lum_med);
camera_autoexposure(&s->cameras.rear, lum_med / 256.0);
}
@@ -769,6 +978,20 @@ void* visionserver_client_thread(void* arg) {
} else {
assert(false);
}
+ } else if (stream_type == VISION_STREAM_RGB_WIDE) {
+ stream_bufs->width = s->rgb_wide_width;
+ stream_bufs->height = s->rgb_wide_height;
+ stream_bufs->stride = s->rgb_wide_stride;
+ stream_bufs->buf_len = s->rgb_wide_bufs[0].len;
+ rep.num_fds = UI_BUF_COUNT;
+ for (int i=0; irgb_wide_bufs[i].fd;
+ }
+ if (stream->tb) {
+ stream->tbuffer = &s->ui_wide_tb;
+ } else {
+ assert(false);
+ }
} else if (stream_type == VISION_STREAM_YUV) {
stream_bufs->width = s->yuv_width;
stream_bufs->height = s->yuv_height;
@@ -797,6 +1020,21 @@ void* visionserver_client_thread(void* arg) {
} else {
stream->queue = pool_get_queue(&s->yuv_front_pool);
}
+ } else if (stream_type == VISION_STREAM_YUV_WIDE) {
+ stream_bufs->width = s->yuv_wide_width;
+ stream_bufs->height = s->yuv_wide_height;
+ stream_bufs->stride = s->yuv_wide_width;
+ stream_bufs->buf_len = s->yuv_wide_buf_size;
+ rep.num_fds = YUV_COUNT;
+ for (int i=0; iyuv_wide_ion[i].fd;
+ }
+ if (stream->tb) {
+ stream->tbuffer = s->yuv_wide_tb;
+ } else {
+ stream->queue = pool_get_queue(&s->yuv_wide_pool);
+ }
+
} else {
assert(false);
}
@@ -849,6 +1087,9 @@ void* visionserver_client_thread(void* arg) {
} else if (stream_i == VISION_STREAM_YUV_FRONT) {
rep.d.stream_acq.extra.frame_id = s->yuv_front_metas[idx].frame_id;
rep.d.stream_acq.extra.timestamp_eof = s->yuv_front_metas[idx].timestamp_eof;
+ } else if (stream_i == VISION_STREAM_YUV_WIDE) {
+ rep.d.stream_acq.extra.frame_id = s->yuv_wide_metas[idx].frame_id;
+ rep.d.stream_acq.extra.timestamp_eof = s->yuv_wide_metas[idx].timestamp_eof;
}
vipc_send(fd, &rep);
}
@@ -959,25 +1200,28 @@ cl_program build_debayer_program(VisionState *s,
int frame_width, int frame_height, int frame_stride,
int rgb_width, int rgb_height, int rgb_stride,
int bayer_flip, int hdr) {
+#ifdef QCOM2
+ assert(rgb_width == frame_width);
+ assert(rgb_height == frame_height);
+#else
assert(rgb_width == frame_width/2);
assert(rgb_height == frame_height/2);
-
- #ifdef QCOM2
- int dnew = 1;
- #else
- int dnew = 0;
- #endif
+#endif
char args[4096];
snprintf(args, sizeof(args),
"-cl-fast-relaxed-math -cl-denorms-are-zero "
"-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d "
"-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d "
- "-DBAYER_FLIP=%d -DHDR=%d -DNEW=%d",
+ "-DBAYER_FLIP=%d -DHDR=%d",
frame_width, frame_height, frame_stride,
rgb_width, rgb_height, rgb_stride,
- bayer_flip, hdr, dnew);
+ bayer_flip, hdr);
+#ifdef QCOM2
+ return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/real_debayer.cl", args);
+#else
return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args);
+#endif
}
cl_program build_conv_program(VisionState *s,
@@ -1010,19 +1254,7 @@ cl_program build_pool_program(VisionState *s,
void cl_init(VisionState *s) {
int err;
- cl_platform_id platform_id = NULL;
- cl_uint num_devices;
- cl_uint num_platforms;
-
- err = clGetPlatformIDs(1, &platform_id, &num_platforms);
- assert(err == 0);
- err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1,
- &s->device_id, &num_devices);
- assert(err == 0);
-
- cl_print_info(platform_id, s->device_id);
- printf("\n");
-
+ s->device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
s->context = clCreateContext(NULL, 1, &s->device_id, NULL, NULL, &err);
assert(err == 0);
}
@@ -1042,7 +1274,7 @@ void init_buffers(VisionState *s) {
for (int i=0; icamera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context,
&s->camera_bufs_cl[i]);
- #ifndef QCOM2
+ #ifdef QCOM
// TODO: make lengths correct
s->focus_bufs[i] = visionbuf_allocate(0xb80);
s->stats_bufs[i] = visionbuf_allocate(0xb80);
@@ -1054,11 +1286,22 @@ void init_buffers(VisionState *s) {
s->device_id, s->context,
&s->front_camera_bufs_cl[i]);
}
-
+#ifdef QCOM2
+ for (int i=0; iwide_camera_bufs[i] = visionbuf_allocate_cl(s->cameras.wide.frame_size,
+ s->device_id, s->context,
+ &s->wide_camera_bufs_cl[i]);
+ }
+#endif
// processing buffers
if (s->cameras.rear.ci.bayer) {
- s->rgb_width = s->frame_width/2;
- s->rgb_height = s->frame_height/2;
+#ifdef QCOM2
+ s->rgb_width = s->frame_width;
+ s->rgb_height = s->frame_height;
+#else
+ s->rgb_width = s->frame_width / 2;
+ s->rgb_height = s->frame_height / 2;
+#endif
} else {
s->rgb_width = s->frame_width;
s->rgb_height = s->frame_height;
@@ -1074,15 +1317,22 @@ void init_buffers(VisionState *s) {
}
tbuffer_init(&s->ui_tb, UI_BUF_COUNT, "rgb");
- //assert(s->cameras.front.ci.bayer);
if (s->cameras.front.ci.bayer) {
- s->rgb_front_width = s->cameras.front.ci.frame_width/2;
- s->rgb_front_height = s->cameras.front.ci.frame_height/2;
+#ifdef QCOM2
+ s->rgb_front_width = s->cameras.front.ci.frame_width;
+ s->rgb_front_height = s->cameras.front.ci.frame_height;
+#else
+ s->rgb_front_width = s->cameras.front.ci.frame_width / 2;
+ s->rgb_front_height = s->cameras.front.ci.frame_height / 2;
+#endif
} else {
s->rgb_front_width = s->cameras.front.ci.frame_width;
s->rgb_front_height = s->cameras.front.ci.frame_height;
}
-
+#ifdef QCOM2
+ s->rgb_wide_width = s->cameras.wide.ci.frame_width;
+ s->rgb_wide_height = s->cameras.wide.ci.frame_height;
+#endif
for (int i=0; irgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]);
@@ -1093,6 +1343,17 @@ void init_buffers(VisionState *s) {
}
}
tbuffer_init(&s->ui_front_tb, UI_BUF_COUNT, "frontrgb");
+#ifdef QCOM2
+ for (int i=0; irgb_wide_width, s->rgb_wide_height, &s->rgb_wide_bufs[i]);
+ s->rgb_wide_bufs_cl[i] = visionbuf_to_cl(&s->rgb_wide_bufs[i], s->device_id, s->context);
+ if (i == 0){
+ s->rgb_wide_stride = img.stride;
+ s->rgb_wide_buf_size = img.size;
+ }
+ }
+ tbuffer_init(&s->ui_wide_tb, UI_BUF_COUNT, "widergb");
+#endif
// yuv back for recording and orbd
pool_init(&s->yuv_pool, YUV_COUNT);
@@ -1124,6 +1385,23 @@ void init_buffers(VisionState *s) {
s->yuv_front_bufs[i].v = s->yuv_front_bufs[i].u + (s->yuv_front_width/2 * s->yuv_front_height/2);
}
+ // yuv wide for recording
+#ifdef QCOM2
+ pool_init(&s->yuv_wide_pool, YUV_COUNT);
+ s->yuv_wide_tb = pool_get_tbuffer(&s->yuv_wide_pool);
+
+ s->yuv_wide_width = s->rgb_wide_width;
+ s->yuv_wide_height = s->rgb_wide_height;
+ s->yuv_wide_buf_size = s->rgb_wide_width * s->rgb_wide_height * 3 / 2;
+
+ for (int i=0; iyuv_wide_ion[i] = visionbuf_allocate_cl(s->yuv_wide_buf_size, s->device_id, s->context, &s->yuv_wide_cl[i]);
+ s->yuv_wide_bufs[i].y = (uint8_t*)s->yuv_wide_ion[i].addr;
+ s->yuv_wide_bufs[i].u = s->yuv_wide_bufs[i].y + (s->yuv_wide_width * s->yuv_wide_height);
+ s->yuv_wide_bufs[i].v = s->yuv_wide_bufs[i].u + (s->yuv_wide_width/2 * s->yuv_wide_height/2);
+ }
+#endif
+
if (s->cameras.rear.ci.bayer) {
// debayering does a 2x downscale
s->yuv_transform = transform_scale_buffer(s->cameras.rear.transform, 0.5);
@@ -1149,7 +1427,24 @@ void init_buffers(VisionState *s) {
s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err);
assert(err == 0);
}
+#ifdef QCOM2
+ if (s->cameras.wide.ci.bayer) {
+ s->prg_debayer_wide = build_debayer_program(s, s->cameras.wide.ci.frame_width, s->cameras.wide.ci.frame_height,
+ s->cameras.wide.ci.frame_stride,
+ s->rgb_wide_width, s->rgb_wide_height, s->rgb_wide_stride,
+ s->cameras.wide.ci.bayer_flip, s->cameras.wide.ci.hdr);
+ s->krnl_debayer_wide = clCreateKernel(s->prg_debayer_wide, "debayer10", &err);
+ assert(err == 0);
+ }
+#endif
+ s->debayer_cl_localMemSize = (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * sizeof(float);
+ s->debayer_cl_globalWorkSize[0] = s->rgb_width;
+ s->debayer_cl_globalWorkSize[1] = s->rgb_height;
+ s->debayer_cl_localWorkSize[0] = DEBAYER_LOCAL_WORKSIZE;
+ s->debayer_cl_localWorkSize[1] = DEBAYER_LOCAL_WORKSIZE;
+
+#ifdef QCOM
s->prg_rgb_laplacian = build_conv_program(s, s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y,
3);
s->krnl_rgb_laplacian = clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err);
@@ -1169,9 +1464,13 @@ void init_buffers(VisionState *s) {
s->conv_cl_localWorkSize[1] = CONV_LOCAL_WORKSIZE;
for (int i=0; i<(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1); i++) {s->lapres[i] = 16160;}
+#endif
rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride);
rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride);
+#ifdef QCOM2
+ rgb_to_yuv_init(&s->wide_rgb_to_yuv_state, s->context, s->device_id, s->yuv_wide_width, s->yuv_wide_height, s->rgb_wide_stride);
+#endif
}
void free_buffers(VisionState *s) {
@@ -1179,18 +1478,28 @@ void free_buffers(VisionState *s) {
for (int i=0; icamera_bufs[i]);
visionbuf_free(&s->front_camera_bufs[i]);
+#ifdef QCOM
visionbuf_free(&s->focus_bufs[i]);
visionbuf_free(&s->stats_bufs[i]);
+#elif defined(QCOM2)
+ visionbuf_free(&s->wide_camera_bufs[i]);
+#endif
}
for (int i=0; irgb_bufs[i]);
visionbuf_free(&s->rgb_front_bufs[i]);
+#ifdef QCOM2
+ visionbuf_free(&s->rgb_wide_bufs[i]);
+#endif
}
for (int i=0; iyuv_ion[i]);
visionbuf_free(&s->yuv_front_ion[i]);
+#ifdef QCOM2
+ visionbuf_free(&s->yuv_wide_ion[i]);
+#endif
}
clReleaseMemObject(s->rgb_conv_roi_cl);
@@ -1201,7 +1510,11 @@ void free_buffers(VisionState *s) {
clReleaseProgram(s->prg_debayer_front);
clReleaseKernel(s->krnl_debayer_rear);
clReleaseKernel(s->krnl_debayer_front);
-
+#ifdef QCOM2
+ clReleaseProgram(s->prg_debayer_wide);
+ clReleaseKernel(s->krnl_debayer_wide);
+#endif
+
clReleaseProgram(s->prg_rgb_laplacian);
clReleaseKernel(s->krnl_rgb_laplacian);
@@ -1223,13 +1536,18 @@ void party(VisionState *s) {
processing_thread, s);
assert(err == 0);
-#if !defined(QCOM2) && !defined(__APPLE__)
- // TODO: fix front camera on qcom2
+#ifndef __APPLE__
pthread_t frontview_thread_handle;
err = pthread_create(&frontview_thread_handle, NULL,
frontview_thread, s);
assert(err == 0);
#endif
+#ifdef QCOM2
+ pthread_t wideview_thread_handle;
+ err = pthread_create(&wideview_thread_handle, NULL,
+ wideview_thread, s);
+ assert(err == 0);
+#endif
// priority for cameras
err = set_realtime_priority(51);
@@ -1241,15 +1559,23 @@ void party(VisionState *s) {
tbuffer_stop(&s->ui_front_tb);
pool_stop(&s->yuv_pool);
pool_stop(&s->yuv_front_pool);
+#ifdef QCOM2
+ tbuffer_stop(&s->ui_wide_tb);
+ pool_stop(&s->yuv_wide_pool);
+#endif
zsock_signal(s->terminate_pub, 0);
-#if !defined(QCOM2) && !defined(QCOM_REPLAY) && !defined(__APPLE__)
+#if (defined(QCOM) && !defined(QCOM_REPLAY)) || defined(WEBCAM) || defined(QCOM2)
LOG("joining frontview_thread");
err = pthread_join(frontview_thread_handle, NULL);
assert(err == 0);
#endif
-
+#ifdef QCOM2
+ LOG("joining wideview_thread");
+ err = pthread_join(wideview_thread_handle, NULL);
+ assert(err == 0);
+#endif
LOG("joining visionserver_thread");
err = pthread_join(visionserver_thread_handle, NULL);
assert(err == 0);
@@ -1263,6 +1589,9 @@ void party(VisionState *s) {
int main(int argc, char *argv[]) {
set_realtime_priority(51);
+#ifdef QCOM
+ set_core_affinity(2);
+#endif
zsys_handler_set(NULL);
signal(SIGINT, (sighandler_t)set_do_exit);
@@ -1283,11 +1612,17 @@ int main(int argc, char *argv[]) {
init_buffers(s);
-#if (defined(QCOM) && !defined(QCOM_REPLAY)) || defined(QCOM2)
+#if defined(QCOM) && !defined(QCOM_REPLAY)
s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"});
+#elif defined(QCOM2)
+ s->pm = new PubMaster({"frame", "frontFrame", "wideFrame", "thumbnail"});
#endif
+#ifndef QCOM2
cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]);
+#else
+ cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0], &s->wide_camera_bufs[0]);
+#endif
party(s);
diff --git a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc
index c8b875105..9d68e5b9e 100644
--- a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc
+++ b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc
@@ -42,14 +42,7 @@ static inline double millis_since_boot() {
void cl_init(cl_device_id &device_id, cl_context &context) {
int err;
- cl_platform_id platform_id = NULL;
- cl_uint num_devices;
- cl_uint num_platforms;
-
- err = clGetPlatformIDs(1, &platform_id, &num_platforms);
- err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1,
- &device_id, &num_devices);
- cl_print_info(platform_id, device_id);
+ device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err);
}
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 0438dd76a..60f3a94e4 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -5,7 +5,7 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip, interp
from selfdrive.car import create_gas_command
from selfdrive.car.honda import hondacan
-from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD
+from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -135,8 +135,6 @@ class CarController():
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
- apply_gas = clip(actuators.gas, 0., 1.)
- apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed
@@ -147,14 +145,14 @@ class CarController():
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
- lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
+ lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
- can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud))
+ can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
- if CS.CP.radarOffCan:
+ if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
@@ -169,14 +167,19 @@ class CarController():
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
- pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
- can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
- pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake))
- self.apply_brake_last = apply_brake
+ if CS.CP.carFingerprint in HONDA_BOSCH:
+ pass # TODO: implement
+ else:
+ apply_gas = clip(actuators.gas, 0., 1.)
+ apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
+ pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
+ can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
+ pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake))
+ self.apply_brake_last = apply_brake
- if CS.CP.enableGasInterceptor:
- # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
- # This prevents unexpected pedal range rescaling
- can_sends.append(create_gas_command(self.packer, apply_gas, idx))
+ if CS.CP.enableGasInterceptor:
+ # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
+ # This prevents unexpected pedal range rescaling
+ can_sends.append(create_gas_command(self.packer, apply_gas, idx))
return can_sends
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 4aaa98aa6..77f74681b 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -79,7 +79,7 @@ def get_can_signals(CP):
("GEARBOX", 100),
]
- if CP.radarOffCan:
+ if CP.carFingerprint in HONDA_BOSCH:
# Civic is only bosch to use the same brake message as other hondas.
if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT):
signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)]
@@ -90,6 +90,10 @@ def get_can_signals(CP):
("EPB_STATE", "EPB_STATUS", 0),
("CRUISE_SPEED", "ACC_HUD", 0)]
checks += [("GAS_PEDAL_2", 100)]
+ if CP.openpilotLongitudinalControl:
+ signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
+ ("BRAKE_ERROR_2", "STANDSTILL", 1)]
+ checks += [("STANDSTILL", 50)]
else:
# Nidec signals.
signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
@@ -206,7 +210,7 @@ class CarState(CarStateBase):
# LOW_SPEED_LOCKOUT is not worth a warning
ret.steerWarning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2']
- if self.CP.radarOffCan:
+ if not self.CP.openpilotLongitudinalControl:
self.brake_error = 0
else:
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2']
@@ -270,7 +274,7 @@ class CarState(CarStateBase):
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0
- if self.CP.radarOffCan:
+ if self.CP.carFingerprint in HONDA_BOSCH:
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 178a452ec..45aea3fc1 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -1,12 +1,26 @@
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import HONDA_BOSCH
+# CAN bus layout with relay
+# 0 = ACC-CAN - radar side
+# 1 = F-CAN B - powertrain
+# 2 = ACC-CAN - camera side
+# 3 = F-CAN A - OBDII port
+
+# CAN bus layout with giraffe
+# 0 = F-CAN B - powertrain
+# 1 = ACC-CAN - camera side
+# 2 = ACC-CAN - radar side
def get_pt_bus(car_fingerprint, has_relay):
return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0
-def get_lkas_cmd_bus(car_fingerprint, has_relay):
+def get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled=False):
+ if radar_disabled:
+ # when radar is disabled, steering commands are sent directly to powertrain bus
+ return get_pt_bus(car_fingerprint, has_relay)
+ # normally steering commands are sent to radar, which forwards them to powertrain bus
return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0
@@ -35,12 +49,47 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
-def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay):
+def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, car_fingerprint, has_relay):
+ commands = []
+ bus = get_pt_bus(car_fingerprint, has_relay)
+
+ control_on = 5 if enabled else 0
+ # no gas = -30000
+ gas_command = gas if enabled and gas > 0 else -30000
+ accel_command = accel if enabled else 0
+ braking = 1 if enabled and accel < 0 else 0
+ standstill = 1 if enabled and stopping else 0
+ standstill_release = 1 if enabled and starting else 0
+
+ acc_control_values = {
+ # setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
+ "CONTROL_ON": control_on,
+ "GAS_COMMAND": gas_command, # used for gas
+ "ACCEL_COMMAND": accel_command, # used for brakes
+ "BRAKE_LIGHTS": braking,
+ "BRAKE_REQUEST": braking,
+ "STANDSTILL": standstill,
+ "STANDSTILL_RELEASE": standstill_release,
+ }
+ commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values, idx))
+
+ acc_control_on_values = {
+ "SET_TO_3": 0x03,
+ "CONTROL_ON": enabled,
+ "SET_TO_FF": 0xff,
+ "SET_TO_75": 0x75,
+ "SET_TO_30": 0x30,
+ }
+ commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values, idx))
+
+ return commands
+
+def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay, radar_disabled):
values = {
"STEER_TORQUE": apply_steer if lkas_active else 0,
"STEER_TORQUE_REQUEST": lkas_active,
}
- bus = get_lkas_cmd_bus(car_fingerprint, has_relay)
+ bus = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled)
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
@@ -55,27 +104,40 @@ def create_bosch_supplemental_1(packer, car_fingerprint, idx, has_relay):
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx)
-def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, stock_hud):
+def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, openpilot_longitudinal_control, stock_hud):
commands = []
bus_pt = get_pt_bus(car_fingerprint, has_relay)
- bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay)
+ radar_disabled = car_fingerprint in HONDA_BOSCH and openpilot_longitudinal_control
+ bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled)
- if car_fingerprint not in HONDA_BOSCH:
- acc_hud_values = {
- 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
- 'PCM_GAS': hud.pcm_accel,
- 'CRUISE_SPEED': hud.v_cruise,
- 'ENABLE_MINI_CAR': 1,
- 'HUD_LEAD': hud.car,
- 'HUD_DISTANCE': 3, # max distance setting on display
- 'IMPERIAL_UNIT': int(not is_metric),
- 'SET_ME_X01_2': 1,
- 'SET_ME_X01': 1,
- "FCM_OFF": stock_hud["FCM_OFF"],
- "FCM_OFF_2": stock_hud["FCM_OFF_2"],
- "FCM_PROBLEM": stock_hud["FCM_PROBLEM"],
- "ICONS": stock_hud["ICONS"],
- }
+ if openpilot_longitudinal_control:
+ if car_fingerprint in HONDA_BOSCH:
+ acc_hud_values = {
+ 'CRUISE_SPEED': hud.v_cruise,
+ 'ENABLE_MINI_CAR': 1,
+ 'SET_TO_1': 1,
+ 'HUD_LEAD': hud.car,
+ 'HUD_DISTANCE': 3,
+ 'ACC_ON': hud.car != 0,
+ 'SET_TO_X1': 1,
+ 'IMPERIAL_UNIT': int(not is_metric),
+ }
+ else:
+ acc_hud_values = {
+ 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
+ 'PCM_GAS': hud.pcm_accel,
+ 'CRUISE_SPEED': hud.v_cruise,
+ 'ENABLE_MINI_CAR': 1,
+ 'HUD_LEAD': hud.car,
+ 'HUD_DISTANCE': 3, # max distance setting on display
+ 'IMPERIAL_UNIT': int(not is_metric),
+ 'SET_ME_X01_2': 1,
+ 'SET_ME_X01': 1,
+ "FCM_OFF": stock_hud["FCM_OFF"],
+ "FCM_OFF_2": stock_hud["FCM_OFF_2"],
+ "FCM_PROBLEM": stock_hud["FCM_PROBLEM"],
+ "ICONS": stock_hud["ICONS"],
+ }
commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx))
lkas_hud_values = {
@@ -87,6 +149,12 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
}
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
+ if radar_disabled and car_fingerprint in HONDA_BOSCH:
+ radar_hud_values = {
+ 'SET_TO_1' : 0x01,
+ }
+ commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
+
return commands
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index ac8bfe3b9..d74db1b1f 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -478,7 +478,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, pcm_enable=False)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
- if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
+ if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
if self.CS.park_brake:
events.add(EventName.parkBrake)
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 713c38ae1..6a92ced81 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -645,10 +645,12 @@ FW_VERSIONS = {
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TPA-G020\x00\x00',
b'57114-TPG-A020\x00\x00',
+ b'57114-TMB-H030\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TPA-G030\x00\x00',
b'39990-TPG-A020\x00\x00',
+ b'39990-TMA-H020\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TMA-H110\x00\x00',
@@ -661,10 +663,12 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'36161-TPA-E050\x00\x00',
b'36161-TPG-A030\x00\x00',
+ b'36161-TMB-H040\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TPA-G520\x00\x00',
b'78109-TPG-A110\x00\x00',
+ b'78109-TMB-H220\x00\x00',
],
(Ecu.hud, 0x18da61f1, None): [
b'78209-TLA-X010\x00\x00',
@@ -672,10 +676,12 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36802-TPA-E040\x00\x00',
b'36802-TPG-A020\x00\x00',
+ b'36802-TMB-H040\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TLA-G220\x00\x00',
b'77959-TLA-C320\x00\x00',
+ b'77959-TLA-H240\x00\x00',
],
},
CAR.FIT: {
@@ -754,6 +760,7 @@ FW_VERSIONS = {
b'78109-THR-AB20\x00\x00',
b'78109-THR-AB30\x00\x00',
b'78109-THR-AB40\x00\x00',
+ b'78109-THR-AC20\x00\x00',
b'78109-THR-AC40\x00\x00',
b'78109-THR-AE20\x00\x00',
b'78109-THR-AE40\x00\x00',
@@ -858,6 +865,7 @@ FW_VERSIONS = {
b'36161-T6Z-A020\x00\x00',
b'36161-T6Z-A310\x00\x00',
b'36161-T6Z-A520\x00\x00',
+ b'36161-TJZ-A120\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-T6Z-A010\x00\x00',
@@ -866,6 +874,7 @@ FW_VERSIONS = {
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-T6Z-A420\x00\x00',
b'78109-T6Z-A510\x00\x00',
+ b'78109-TJZ-A510\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-T6Z-A020\x00\x00',
@@ -874,6 +883,7 @@ FW_VERSIONS = {
b'57114-T6Z-A120\x00\x00',
b'57114-T6Z-A130\x00\x00',
b'57114-T6Z-A520\x00\x00',
+ b'57114-TJZ-A520\x00\x00',
],
},
CAR.INSIGHT: {
@@ -891,6 +901,7 @@ FW_VERSIONS = {
b'77959-TXM-A230\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
+ b'57114-TXM-A030\x00\x00',
b'57114-TXM-A040\x00\x00',
],
(Ecu.shiftByWire, 0x18da0bf1, None): [
@@ -900,7 +911,9 @@ FW_VERSIONS = {
b'38897-TXM-A020\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
+ b'78109-TXM-A010\x00\x00',
b'78109-TXM-A020\x00\x00',
+ b'78109-TXM-A110\x00\x00',
],
},
CAR.HRV: {
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 787cc91b0..c47545ef2 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -43,11 +43,13 @@ class CarController():
self.steer_rate_limited = False
self.last_resume_frame = 0
+ self.p = SteerLimitParams(CP)
+
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart):
# Steering Torque
- new_steer = actuators.steer * SteerLimitParams.STEER_MAX
- apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams)
+ new_steer = 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
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
@@ -75,9 +77,8 @@ class CarController():
if pcm_cancel_cmd:
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
- # SCC won't resume anyway when the lead distace is less than 3.7m
# send resume at a max freq of 5Hz
- if CS.lead_distance > 3.7 and (frame - self.last_resume_frame)*DT_CTRL > 0.2:
+ if (frame - self.last_resume_frame)*DT_CTRL > 0.2:
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL))
self.last_resume_frame = frame
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 61d9de05b..fd53ffc8a 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -1,3 +1,4 @@
+import copy
from cereal import car
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_HYBRID
from selfdrive.car.interfaces import CarStateBase
@@ -124,8 +125,8 @@ class CarState(CarStateBase):
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11 and CLU11
- self.lkas11 = cp_cam.vl["LKAS11"]
- self.clu11 = cp.vl["CLU11"]
+ self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
+ self.clu11 = copy.copy(cp.vl["CLU11"])
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 553d48a2b..76569d091 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase):
ret.radarOffCan = True
# Most Hyundai car ports are community features for now
- ret.communityFeature = candidate not in [CAR.SONATA]
+ ret.communityFeature = candidate not in [CAR.SONATA, CAR.PALISADE]
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerRateCost = 0.5
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 9a39cb8f3..c36ecbd96 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -6,12 +6,16 @@ Ecu = car.CarParams.Ecu
# Steer torque limits
class SteerLimitParams:
- STEER_MAX = 255 # 409 is the max, 255 is stock
- STEER_DELTA_UP = 3
- STEER_DELTA_DOWN = 7
- STEER_DRIVER_ALLOWANCE = 50
- STEER_DRIVER_MULTIPLIER = 2
- STEER_DRIVER_FACTOR = 1
+ def __init__(self, CP):
+ if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE]:
+ self.STEER_MAX = 384
+ else:
+ self.STEER_MAX = 255
+ self.STEER_DELTA_UP = 3
+ self.STEER_DELTA_DOWN = 7
+ self.STEER_DRIVER_ALLOWANCE = 50
+ self.STEER_DRIVER_MULTIPLIER = 2
+ self.STEER_DRIVER_FACTOR = 1
class CAR:
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index 03f3a026b..bc6c6d0eb 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -3,7 +3,7 @@ from selfdrive.config import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, CAR
+from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1
class CarState(CarStateBase):
def __init__(self, CP):
@@ -121,7 +121,7 @@ class CarState(CarStateBase):
("WHEEL_SPEEDS", 100),
]
- if CP.carFingerprint == CAR.CX5:
+ if CP.carFingerprint in GEN1:
signals += [
("LKAS_BLOCK", "STEER_RATE", 0),
("LKAS_TRACK_STATE", "STEER_RATE", 0),
@@ -165,7 +165,7 @@ class CarState(CarStateBase):
signals = []
checks = []
- if CP.carFingerprint == CAR.CX5:
+ if CP.carFingerprint in GEN1:
signals += [
# sig_name, sig_address, default
("LKAS_REQUEST", "CAM_LKAS", 0),
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index dfa959da3..81dff12b9 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -25,26 +25,36 @@ class CarInterface(CarInterfaceBase):
ret.radarOffCan = True
- # Mazda port is a community feature for now
- ret.communityFeature = True
-
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1.0
ret.steerLimitTimer = 0.8
tire_stiffness_factor = 0.70 # not optimized yet
- if candidate in [CAR.CX5]:
+ if candidate == CAR.CX5:
ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 15.5
-
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.2]]
-
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]]
+ ret.lateralTuning.pid.kf = 0.00006
+ elif candidate == CAR.CX9:
+ ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG
+ ret.wheelbase = 3.1
+ ret.steerRatio = 17.6
+ ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]]
+ ret.lateralTuning.pid.kf = 0.00006
+ elif candidate == CAR.Mazda3:
+ ret.mass = 2875 * CV.LB_TO_KG + STD_CARGO_KG
+ ret.wheelbase = 2.7
+ ret.steerRatio = 14.0
+ ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]]
ret.lateralTuning.pid.kf = 0.00006
- # No steer below disable speed
- ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
+
+ # No steer below disable speed
+ ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
ret.centerToFront = ret.wheelbase * 0.41
diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py
index 55b9218db..dabbd2129 100644
--- a/selfdrive/car/mazda/mazdacan.py
+++ b/selfdrive/car/mazda/mazdacan.py
@@ -1,4 +1,4 @@
-from selfdrive.car.mazda.values import CAR, Buttons
+from selfdrive.car.mazda.values import GEN1, Buttons
def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
@@ -40,7 +40,7 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
csum = csum % 256
- if car_fingerprint == CAR.CX5:
+ if car_fingerprint in GEN1:
values = {
"LKAS_REQUEST" : apply_steer,
"CTR" : ctr,
@@ -69,7 +69,7 @@ def create_button_cmd(packer, car_fingerprint, button):
can = 0
res = 0
- if car_fingerprint == CAR.CX5:
+ if car_fingerprint in GEN1:
values = {
"CAN_OFF" : can,
"CAN_OFF_INV" : (can + 1) % 2,
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index 5759d431a..4e194d61d 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -18,6 +18,8 @@ class SteerLimitParams:
class CAR:
CX5 = "Mazda CX-5 2017"
+ CX9 = "Mazda CX-9 2017"
+ Mazda3 = "Mazda3 2017"
class LKAS_LIMITS:
STEER_THRESHOLD = 15
@@ -41,13 +43,10 @@ FINGERPRINTS = {
# CX-5 2019 GTR
{
64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 254: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 608: 8, 628: 8, 736: 8, 832: 8, 836: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1078: 8, 1080: 8, 1085: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1139: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1170: 8, 1171: 8, 1173: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1244: 8, 1260: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1446: 8, 1456: 8, 1479: 8, 1776: 8, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8
- },
-
- # Mazda 6 2017 GT
- {
- 64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 628: 8, 832: 8, 836: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1070: 8, 1078: 8, 1080: 8, 1085: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1182: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1275: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1456: 8, 1479: 8
- },
+ }
+ ],
+ CAR.CX9: [
# CX-9 2017 Australia - old CAM connector
{
64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 138: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 522: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 583: 8, 605: 8, 606: 8, 628: 8, 832: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1078: 8, 1085: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1139: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1170: 8, 1177: 8, 1180: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1247: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1271: 8, 1272: 8, 1274: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1446: 8, 1456: 8, 1479: 8
@@ -56,14 +55,20 @@ FINGERPRINTS = {
# CX-9 2016 - old CAM connector
{
64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 583: 8, 605: 8, 606: 8, 608: 8, 628: 8, 832: 8, 836: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1078: 8, 1080: 8, 1085: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1139: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1170: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1244: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1271: 8, 1272: 8, 1274: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1446: 8, 1456: 8, 1479: 8, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
- },
+ }
+ ],
+ CAR.Mazda3: [
# Mazda 3 2017
{
19: 5, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 628: 8, 832: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1070: 8, 1078: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1169: 8, 1170: 8, 1173: 8, 1177: 8, 1180: 8, 1182: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1275: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1456: 8, 1479: 8, 2015: 8, 2024: 8, 2025: 8
},
- ],
+ # Mazda 6 2017 GT
+ {
+ 64: 8, 70: 8, 80: 8, 117: 8, 118: 8, 120: 8, 121: 8, 130: 8, 134: 8, 145: 8, 154: 8, 155: 8, 157: 8, 158: 8, 159: 8, 253: 8, 304: 8, 305: 8, 357: 8, 358: 8, 359: 8, 512: 8, 514: 8, 515: 8, 529: 8, 533: 8, 535: 8, 539: 8, 540: 8, 541: 8, 542: 8, 543: 8, 552: 8, 576: 8, 577: 8, 578: 8, 579: 8, 580: 8, 581: 8, 582: 8, 605: 8, 606: 8, 607: 8, 628: 8, 832: 8, 836: 8, 863: 8, 865: 8, 866: 8, 867: 8, 868: 8, 869: 8, 870: 8, 976: 8, 977: 8, 978: 8, 1034: 8, 1045: 8, 1056: 8, 1061: 8, 1067: 8, 1070: 8, 1078: 8, 1080: 8, 1085: 8, 1086: 8, 1088: 8, 1093: 8, 1108: 8, 1114: 8, 1115: 8, 1116: 8, 1143: 8, 1147: 8, 1154: 8, 1157: 8, 1160: 8, 1163: 8, 1166: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1182: 8, 1183: 8, 1233: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1241: 8, 1242: 8, 1243: 8, 1244: 8, 1264: 8, 1266: 8, 1267: 8, 1269: 8, 1270: 8, 1271: 8, 1272: 8, 1274: 8, 1275: 8, 1277: 8, 1278: 8, 1409: 8, 1416: 8, 1425: 8, 1430: 8, 1435: 8, 1440: 8, 1456: 8, 1479: 8
+ }
+ ],
}
ECU_FINGERPRINT = {
@@ -73,4 +78,8 @@ ECU_FINGERPRINT = {
DBC = {
CAR.CX5: dbc_dict('mazda_2017', None),
+ CAR.CX9: dbc_dict('mazda_2017', None),
+ CAR.Mazda3: dbc_dict('mazda_2017', None),
}
+
+GEN1 = [ CAR.CX5, CAR.CX9, CAR.Mazda3 ]
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index d0298ec83..4b703d5e9 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -63,6 +63,7 @@ class CarInterface(CarInterfaceBase):
# create message
ret = car.CarState.new_message()
+ ret.canValid = True
# speeds
ret.vEgo = self.speed
@@ -82,9 +83,6 @@ class CarInterface(CarInterfaceBase):
curvature = self.yaw_rate / max(self.speed, 1.)
ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
- events = []
- ret.events = events
-
return ret.as_reader()
def apply(self, c):
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index e31eaee4f..e97cc23af 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -63,7 +63,7 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.COROLLA:
stop_and_go = False
- ret.safetyParam = 100
+ ret.safetyParam = 88
ret.wheelbase = 2.70
ret.steerRatio = 18.27
tire_stiffness_factor = 0.444 # not optimized yet
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index da18bfba9..f1d31fb4d 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -309,6 +309,7 @@ FW_VERSIONS = {
b'\x018966333P4700\x00\x00\x00\x00',
b'\x018966333Q6000\x00\x00\x00\x00',
b'\x018966333Q6200\x00\x00\x00\x00',
+ b'\x018966333W6000\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'8821F0601200 ',
@@ -316,17 +317,20 @@ FW_VERSIONS = {
b'8821F0603300 ',
b'8821F0607200 ',
b'8821F0608000 ',
+ b'8821F0609100 ',
],
(Ecu.esp, 0x7b0, None): [
b'F152606210\x00\x00\x00\x00\x00\x00',
b'F152606230\x00\x00\x00\x00\x00\x00',
b'F152606290\x00\x00\x00\x00\x00\x00',
b'F152633540\x00\x00\x00\x00\x00\x00',
+ b'F152633A20\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B33540\x00\x00\x00\x00\x00\x00',
b'8965B33542\x00\x00\x00\x00\x00\x00',
b'8965B33580\x00\x00\x00\x00\x00\x00',
+ b'8965B33621\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791
b'8821F0601200 ',
@@ -334,6 +338,7 @@ FW_VERSIONS = {
b'8821F0603300 ',
b'8821F0607200 ',
b'8821F0608000 ',
+ b'8821F0609100 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F0601200 ',
@@ -342,6 +347,7 @@ FW_VERSIONS = {
b'8646F0605000 ',
b'8646F0606000 ',
b'8646F0606100 ',
+ b'8646F0607100 ',
],
},
CAR.CAMRYH: {
@@ -425,6 +431,7 @@ FW_VERSIONS = {
(Ecu.dsu, 0x791, None): [
b'8821F0W01000 ',
b'8821FF401600 ',
+ b'8821FF404000 ',
b'8821FF404100 ',
b'8821FF405100 ',
b'8821FF406000 ',
@@ -436,6 +443,7 @@ FW_VERSIONS = {
b'F1526F4034\x00\x00\x00\x00\x00\x00',
b'F1526F4044\x00\x00\x00\x00\x00\x00',
b'F1526F4073\x00\x00\x00\x00\x00\x00',
+ b'F1526F4121\x00\x00\x00\x00\x00\x00',
b'F1526F4122\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
@@ -450,6 +458,7 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F0W01000 ',
b'8821FF401600 ',
+ b'8821FF404000 ',
b'8821FF404100 ',
b'8821FF405100 ',
b'8821FF406000 ',
@@ -540,6 +549,7 @@ FW_VERSIONS = {
b'\x018966312R3100\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
+ b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
@@ -548,6 +558,7 @@ FW_VERSIONS = {
(Ecu.eps, 0x7a1, None): [
b'8965B12361\x00\x00\x00\x00\x00\x00',
b'\x018965B12350\x00\x00\x00\x00\x00\x00',
+ b'\x018965B12470\x00\x00\x00\x00\x00\x00',
b'\x018965B12500\x00\x00\x00\x00\x00\x00',
b'\x018965B12520\x00\x00\x00\x00\x00\x00',
b'\x018965B12530\x00\x00\x00\x00\x00\x00',
@@ -556,6 +567,7 @@ FW_VERSIONS = {
b'F152602191\x00\x00\x00\x00\x00\x00',
b'\x01F152602280\x00\x00\x00\x00\x00\x00',
b'\x01F152602560\x00\x00\x00\x00\x00\x00',
+ b'\x01F152602650\x00\x00\x00\x00\x00\x00',
b'\x01F152612641\x00\x00\x00\x00\x00\x00',
b'\x01F152612651\x00\x00\x00\x00\x00\x00',
b'\x01F152612B10\x00\x00\x00\x00\x00\x00',
@@ -573,6 +585,7 @@ FW_VERSIONS = {
b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
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'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
@@ -593,6 +606,7 @@ FW_VERSIONS = {
b'\x018965B12350\x00\x00\x00\x00\x00\x00',
b'\x018965B12470\x00\x00\x00\x00\x00\x00',
b'\x018965B12500\x00\x00\x00\x00\x00\x00',
+ b'\x018965B12520\x00\x00\x00\x00\x00\x00',
b'\x018965B12530\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
@@ -604,6 +618,7 @@ FW_VERSIONS = {
b'F152612840\x00\x00\x00\x00\x00\x00',
b'F152612A10\x00\x00\x00\x00\x00\x00',
b'F152642540\x00\x00\x00\x00\x00\x00',
+ b'F152612A00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
@@ -683,7 +698,9 @@ FW_VERSIONS = {
b'\x01F15260E051\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
+ b'\x01896630E62200\x00\x00\x00\x00',
b'\x01896630E64100\x00\x00\x00\x00',
+ b'\x01896630E64200\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
@@ -698,6 +715,7 @@ FW_VERSIONS = {
],
(Ecu.esp, 0x7b0, None): [
b'\x01F15264872300\x00\x00\x00\x00',
+ b'\x01F15264872400\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@@ -747,6 +765,7 @@ FW_VERSIONS = {
b'\x02896634784000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347A5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966347A8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+ b'\x02896634765100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x03896634759100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00',
b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00',
b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00',
@@ -945,11 +964,13 @@ FW_VERSIONS = {
b'\x018966342X6000\x00\x00\x00\x00',
b'\x018966342W8000\x00\x00\x00\x00',
b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
+ b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152642291\x00\x00\x00\x00\x00\x00',
b'F152642330\x00\x00\x00\x00\x00\x00',
+ b'F152642331\x00\x00\x00\x00\x00\x00',
b'F152642531\x00\x00\x00\x00\x00\x00',
b'F152642532\x00\x00\x00\x00\x00\x00',
b'F152642521\x00\x00\x00\x00\x00\x00',
@@ -1075,9 +1096,12 @@ FW_VERSIONS = {
(Ecu.engine, 0x700, None): [
b'\x01896630E37200\x00\x00\x00\x00',
b'\x01896630E41000\x00\x00\x00\x00',
+ b'\x01896630E41100\x00\x00\x00\x00',
b'\x01896630E41200\x00\x00\x00\x00',
b'\x01896630E37300\x00\x00\x00\x00',
+ b'\x01896630E36300\x00\x00\x00\x00',
b'\x018966348R8500\x00\x00\x00\x00',
+ b'\x018966348W1300\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152648472\x00\x00\x00\x00\x00\x00',
@@ -1085,6 +1109,7 @@ FW_VERSIONS = {
b'F152648492\x00\x00\x00\x00\x00\x00',
b'F152648493\x00\x00\x00\x00\x00\x00',
b'F152648474\x00\x00\x00\x00\x00\x00',
+ b'F152648630\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'881514810300\x00\x00\x00\x00',
@@ -1095,10 +1120,12 @@ FW_VERSIONS = {
b'8965B0E011\x00\x00\x00\x00\x00\x00',
b'8965B0E012\x00\x00\x00\x00\x00\x00',
b'8965B48102\x00\x00\x00\x00\x00\x00',
+ b'8965B48112\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F4701000\x00\x00\x00\x00',
b'8821F4701100\x00\x00\x00\x00',
+ b'8821F4701200\x00\x00\x00\x00',
b'8821F4701300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
@@ -1106,6 +1133,7 @@ FW_VERSIONS = {
b'8646F4801200\x00\x00\x00\x00',
b'8646F4802001\x00\x00\x00\x00',
b'8646F4802100\x00\x00\x00\x00',
+ b'8646F4802200\x00\x00\x00\x00',
b'8646F4809000\x00\x00\x00\x00',
],
},
@@ -1154,6 +1182,7 @@ FW_VERSIONS = {
],
(Ecu.esp, 0x7b0, None): [
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
+ b'\x01F15260E041\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B48271\x00\x00\x00\x00\x00\x00',
diff --git a/selfdrive/clocksd/SConscript b/selfdrive/clocksd/SConscript
index 601e64bf1..d1cf13e9e 100644
--- a/selfdrive/clocksd/SConscript
+++ b/selfdrive/clocksd/SConscript
@@ -1,2 +1,2 @@
Import('env', 'common', 'cereal', 'messaging')
-env.Program('clocksd.cc', LIBS=['diag', 'time_genoff', common, cereal, messaging, 'capnp', 'zmq', 'kj'])
\ No newline at end of file
+env.Program('clocksd.cc', LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj'])
diff --git a/selfdrive/clocksd/clocksd.cc b/selfdrive/clocksd/clocksd.cc
index 2e17058e0..d9fa9eda3 100644
--- a/selfdrive/clocksd/clocksd.cc
+++ b/selfdrive/clocksd/clocksd.cc
@@ -1,12 +1,22 @@
+#include
+#include
+
#include
#include
+#include
#include
-#include
#include
-#include
+
+#include
#include "messaging.hpp"
#include "common/timing.h"
+// Apple doesn't have timerfd
+#ifndef __APPLE__
+#include
+#endif
+
+#ifdef QCOM
namespace {
int64_t arm_cntpct() {
int64_t v;
@@ -14,13 +24,14 @@ namespace {
return v;
}
}
+#endif
int main() {
setpriority(PRIO_PROCESS, 0, -13);
- int err = 0;
PubMaster pm({"clocks"});
+#ifndef __APPLE__
int timerfd = timerfd_create(CLOCK_BOOTTIME, 0);
assert(timerfd >= 0);
@@ -30,34 +41,43 @@ int main() {
spec.it_value.tv_sec = 1;
spec.it_value.tv_nsec = 0;
- err = timerfd_settime(timerfd, 0, &spec, 0);
+ int err = timerfd_settime(timerfd, 0, &spec, 0);
assert(err == 0);
uint64_t expirations = 0;
while ((err = read(timerfd, &expirations, sizeof(expirations)))) {
if (err < 0) break;
+#else
+ // Just run at 1Hz on apple
+ while (true){
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+#endif
uint64_t boottime = nanos_since_boot();
uint64_t monotonic = nanos_monotonic();
uint64_t monotonic_raw = nanos_monotonic_raw();
uint64_t wall_time = nanos_since_epoch();
+#ifdef QCOM
uint64_t modem_uptime_v = arm_cntpct() / 19200ULL; // 19.2 mhz clock
+#endif
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(boottime);
- auto clocks = event.initClocks();
+ MessageBuilder msg;
+ auto clocks = msg.initEvent().initClocks();
clocks.setBootTimeNanos(boottime);
clocks.setMonotonicNanos(monotonic);
clocks.setMonotonicRawNanos(monotonic_raw);
clocks.setWallTimeNanos(wall_time);
+#ifdef QCOM
clocks.setModemUptimeMillis(modem_uptime_v);
+#endif
pm.send("clocks", msg);
}
+#ifndef __APPLE__
close(timerfd);
+#endif
return 0;
-}
\ No newline at end of file
+}
diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript
index 8ecf786ff..f70abf58c 100644
--- a/selfdrive/common/SConscript
+++ b/selfdrive/common/SConscript
@@ -5,7 +5,9 @@ if SHARED:
else:
fxn = env.Library
-_common = fxn('common', ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c'], LIBS="json11")
+common_libs = ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c', 'gpio.cc', 'i2c.cc']
+
+_common = fxn('common', common_libs, LIBS="json11")
_visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c'])
files = [
@@ -42,4 +44,3 @@ else:
_gpucommon = fxn('gpucommon', files, CPPDEFINES=defines, LIBS=_gpu_libs)
Export('_common', '_visionipc', '_gpucommon', '_gpu_libs')
-
diff --git a/selfdrive/common/clutil.c b/selfdrive/common/clutil.c
index 9a80c9067..00d63cdca 100644
--- a/selfdrive/common/clutil.c
+++ b/selfdrive/common/clutil.c
@@ -36,6 +36,44 @@ void clu_init(void) {
#endif
}
+cl_device_id cl_get_device_id(cl_device_type device_type) {
+ bool opencl_platform_found = false;
+ cl_device_id device_id = NULL;
+
+ cl_uint num_platforms = 0;
+ int err = clGetPlatformIDs(0, NULL, &num_platforms);
+ assert(err == 0);
+ cl_platform_id* platform_ids = malloc(sizeof(cl_platform_id) * num_platforms);
+ err = clGetPlatformIDs(num_platforms, platform_ids, NULL);
+ assert(err == 0);
+
+ char cBuffer[1024];
+ for (size_t i = 0; i < num_platforms; i++) {
+ err = clGetPlatformInfo(platform_ids[i], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL);
+ assert(err == 0);
+ printf("platform[%zu] CL_PLATFORM_NAME: %s\n", i, cBuffer);
+
+ cl_uint num_devices;
+ err = clGetDeviceIDs(platform_ids[i], device_type, 0, NULL, &num_devices);
+ if (err != 0 || !num_devices) {
+ continue;
+ }
+ // Get first device
+ err = clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL);
+ assert(err == 0);
+ cl_print_info(platform_ids[i], device_id);
+ opencl_platform_found = true;
+ break;
+ }
+ free(platform_ids);
+
+ if (!opencl_platform_found) {
+ printf("No valid openCL platform found\n");
+ assert(opencl_platform_found);
+ }
+ return device_id;
+}
+
cl_program cl_create_program_from_file(cl_context ctx, const char* path) {
char* src_buf = read_file(path, NULL);
assert(src_buf);
diff --git a/selfdrive/common/clutil.h b/selfdrive/common/clutil.h
index b87961eac..abbda8c80 100644
--- a/selfdrive/common/clutil.h
+++ b/selfdrive/common/clutil.h
@@ -17,6 +17,7 @@ extern "C" {
void clu_init(void);
+cl_device_id cl_get_device_id(cl_device_type device_type);
cl_program cl_create_program_from_file(cl_context ctx, const char* path);
void cl_print_info(cl_platform_id platform, cl_device_id device);
void cl_print_build_errors(cl_program program, cl_device_id device);
diff --git a/selfdrive/common/framebuffer.cc b/selfdrive/common/framebuffer.cc
index 30604deb2..30ef7ae8e 100644
--- a/selfdrive/common/framebuffer.cc
+++ b/selfdrive/common/framebuffer.cc
@@ -1,3 +1,4 @@
+#include "util.h"
#include
#include
#include
@@ -37,13 +38,9 @@ extern "C" void framebuffer_swap(FramebufferState *s) {
}
extern "C" bool set_brightness(int brightness) {
- FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb");
- if (f != NULL) {
- fprintf(f, "%d", brightness);
- fclose(f);
- return true;
- }
- return false;
+ char bright[64];
+ snprintf(bright, sizeof(bright), "%d", brightness);
+ return 0 == write_file("/sys/class/leds/lcd-backlight/brightness", bright, strlen(bright));
}
extern "C" void framebuffer_set_power(FramebufferState *s, int mode) {
diff --git a/selfdrive/common/gpio.cc b/selfdrive/common/gpio.cc
new file mode 100644
index 000000000..8a72388d4
--- /dev/null
+++ b/selfdrive/common/gpio.cc
@@ -0,0 +1,29 @@
+#include "gpio.h"
+#include "util.h"
+#include
+#include
+#include
+
+// We assume that all pins have already been exported on boot,
+// and that we have permission to write to them.
+
+int gpio_init(int pin_nr, bool output){
+ char pin_dir_path[50];
+ int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path),
+ "/sys/class/gpio/gpio%d/direction", pin_nr);
+ if(pin_dir_path_len <= 0){
+ return -1;
+ }
+ const char *value = output ? "out" : "in";
+ return write_file(pin_dir_path, (void*)value, strlen(value));
+}
+
+int gpio_set(int pin_nr, bool high){
+ char pin_val_path[50];
+ int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path),
+ "/sys/class/gpio/gpio%d/value", pin_nr);
+ if(pin_val_path_len <= 0){
+ return -1;
+ }
+ return write_file(pin_val_path, (void*)(high ? "1" : "0"), 1);
+}
diff --git a/selfdrive/common/gpio.h b/selfdrive/common/gpio.h
new file mode 100644
index 000000000..479b4f7e0
--- /dev/null
+++ b/selfdrive/common/gpio.h
@@ -0,0 +1,35 @@
+#ifndef GPIO_H
+#define GPIO_H
+
+#include
+
+// Pin definitions
+#ifdef QCOM2
+ #define GPIO_HUB_RST_N 30
+ #define GPIO_UBLOX_RST_N 32
+ #define GPIO_UBLOX_SAFEBOOT_N 33
+ #define GPIO_UBLOX_PWR_EN 34
+ #define GPIO_STM_RST_N 124
+ #define GPIO_STM_BOOT0 134
+#else
+ #define GPIO_HUB_RST_N 0
+ #define GPIO_UBLOX_RST_N 0
+ #define GPIO_UBLOX_SAFEBOOT_N 0
+ #define GPIO_UBLOX_PWR_EN 0
+ #define GPIO_STM_RST_N 0
+ #define GPIO_STM_BOOT0 0
+#endif
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int gpio_init(int pin_nr, bool output);
+int gpio_set(int pin_nr, bool high);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+#endif
diff --git a/selfdrive/common/i2c.cc b/selfdrive/common/i2c.cc
new file mode 100644
index 000000000..a37b144dc
--- /dev/null
+++ b/selfdrive/common/i2c.cc
@@ -0,0 +1,80 @@
+#include "i2c.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include "common/swaglog.h"
+
+#define UNUSED(x) (void)(x)
+
+#ifdef QCOM2
+// TODO: decide if we want to isntall libi2c-dev everywhere
+#include
+
+I2CBus::I2CBus(uint8_t bus_id){
+ char bus_name[20];
+ snprintf(bus_name, 20, "/dev/i2c-%d", bus_id);
+
+ i2c_fd = open(bus_name, O_RDWR);
+ if(i2c_fd < 0){
+ throw std::runtime_error("Failed to open I2C bus");
+ }
+}
+
+I2CBus::~I2CBus(){
+ if(i2c_fd >= 0){ close(i2c_fd); }
+}
+
+int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){
+ int ret = 0;
+
+ ret = ioctl(i2c_fd, I2C_SLAVE, device_address);
+ if(ret < 0) { goto fail; }
+
+ ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer);
+ if((ret < 0) || (ret != len)) { goto fail; }
+
+fail:
+ return ret;
+}
+
+int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){
+ int ret = 0;
+
+ ret = ioctl(i2c_fd, I2C_SLAVE, device_address);
+ if(ret < 0) { goto fail; }
+
+ ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data);
+ if(ret < 0) { goto fail; }
+
+fail:
+ return ret;
+}
+
+#else
+
+I2CBus::I2CBus(uint8_t bus_id){
+ UNUSED(bus_id);
+ i2c_fd = -1;
+}
+
+I2CBus::~I2CBus(){}
+
+int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){
+ UNUSED(device_address);
+ UNUSED(register_address);
+ UNUSED(buffer);
+ UNUSED(len);
+ return -1;
+}
+
+int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){
+ UNUSED(device_address);
+ UNUSED(register_address);
+ UNUSED(data);
+ return -1;
+}
+#endif
diff --git a/selfdrive/common/i2c.h b/selfdrive/common/i2c.h
new file mode 100644
index 000000000..d5788510d
--- /dev/null
+++ b/selfdrive/common/i2c.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include
+#include
+
+class I2CBus {
+ private:
+ int i2c_fd;
+
+ public:
+ I2CBus(uint8_t bus_id);
+ ~I2CBus();
+
+ int read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len);
+ int set_register(uint8_t device_address, uint register_address, uint8_t data);
+};
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index 111e20a41..77f4a1b7a 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,41 +1,7 @@
-#ifndef MODELDATA_H
-#define MODELDATA_H
+#pragma once
#define MODEL_PATH_DISTANCE 192
#define POLYFIT_DEGREE 4
#define SPEED_PERCENTILES 10
#define DESIRE_PRED_SIZE 32
#define OTHER_META_SIZE 4
-
-typedef struct PathData {
- float points[MODEL_PATH_DISTANCE];
- float prob;
- float std;
- float stds[MODEL_PATH_DISTANCE];
- float poly[POLYFIT_DEGREE];
- float validLen;
-} PathData;
-
-typedef struct LeadData {
- float dist;
- float prob;
- float std;
- float rel_y;
- float rel_y_std;
- float rel_v;
- float rel_v_std;
- float rel_a;
- float rel_a_std;
-} LeadData;
-
-typedef struct ModelData {
- PathData path;
- PathData left_lane;
- PathData right_lane;
- LeadData lead;
- LeadData lead_future;
- float meta[OTHER_META_SIZE + DESIRE_PRED_SIZE];
- float speed[SPEED_PERCENTILES];
-} ModelData;
-
-#endif
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 4d9c214d3..d511ebf41 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -268,45 +268,19 @@ cleanup:
}
int read_db_value(const char* key, char** value, size_t* value_sz, bool persistent_param) {
- int lock_fd = -1;
- int result;
char path[1024];
const char* params_path = persistent_param ? persistent_params_path : default_params_path;
- result = snprintf(path, sizeof(path), "%s/.lock", params_path);
+ int result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
if (result < 0) {
- goto cleanup;
- }
- lock_fd = open(path, 0);
-
- result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
- if (result < 0) {
- goto cleanup;
+ return result;
}
- // Take lock.
- result = flock(lock_fd, LOCK_EX);
- if (result < 0) {
- goto cleanup;
- }
-
- // Read value.
- // TODO(mgraczyk): If there is a lot of contention, we can release the lock
- // after opening the file, before reading.
*value = static_cast(read_file(path, value_sz));
if (*value == NULL) {
- result = -22;
- goto cleanup;
+ return -22;
}
-
- result = 0;
-
-cleanup:
- // Release lock.
- if (lock_fd >= 0) {
- close(lock_fd);
- }
- return result;
+ return 0;
}
void read_db_value_blocking(const char* key, char** value, size_t* value_sz, bool persistent_param) {
@@ -330,8 +304,11 @@ int read_db_all(std::map *params, bool persistent_para
int lock_fd = open(lock_path.c_str(), 0);
if (lock_fd < 0) return -1;
- err = flock(lock_fd, LOCK_EX);
- if (err < 0) return err;
+ err = flock(lock_fd, LOCK_SH);
+ if (err < 0) {
+ close(lock_fd);
+ return err;
+ }
std::string key_path = util::string_format("%s/d", params_path);
DIR *d = opendir(key_path.c_str());
@@ -366,3 +343,8 @@ std::vector read_db_bytes(const char* param_name, bool persistent_param) {
}
return bytes;
}
+
+bool read_db_bool(const char* param_name, bool persistent_param) {
+ std::vector bytes = read_db_bytes(param_name, persistent_param);
+ return bytes.size() > 0 and bytes[0] == '1';
+}
diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h
index 49af04b4f..394c9b6ed 100644
--- a/selfdrive/common/params.h
+++ b/selfdrive/common/params.h
@@ -40,4 +40,5 @@ void read_db_value_blocking(const char* key, char** value, size_t* value_sz, boo
#include
int read_db_all(std::map *params, bool persistent_param = false);
std::vector read_db_bytes(const char* param_name, bool persistent_param = false);
+bool read_db_bool(const char* param_name, bool persistent_param = false);
#endif
diff --git a/selfdrive/common/spinner.c b/selfdrive/common/spinner.c
index 99e0ed422..2054229cc 100644
--- a/selfdrive/common/spinner.c
+++ b/selfdrive/common/spinner.c
@@ -61,6 +61,7 @@ int spin(int argc, char** argv) {
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);
diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc
index 41b6358d9..9d9034754 100644
--- a/selfdrive/common/swaglog.cc
+++ b/selfdrive/common/swaglog.cc
@@ -93,15 +93,13 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func
{"created", seconds_since_epoch()}
};
- char* log_s = strdup(log_j.dump().c_str());
- assert(log_s);
+ std::string log_s = log_j.dump();
free(msg_buf);
char levelnum_c = levelnum;
zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE);
- zmq_send(s.sock, log_s, strlen(log_s), ZMQ_NOBLOCK);
- free(log_s);
+ zmq_send(s.sock, log_s.c_str(), log_s.length(), ZMQ_NOBLOCK);
pthread_mutex_unlock(&s.lock);
}
diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c
index 2225ed215..0ab1e38c0 100644
--- a/selfdrive/common/touch.c
+++ b/selfdrive/common/touch.c
@@ -94,31 +94,3 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
}
return up;
}
-
-int touch_read(TouchState *s, int* out_x, int* out_y) {
- assert(out_x && out_y);
- struct input_event event;
- int err = read(s->fd, &event, sizeof(event));
- if (err < sizeof(event)) {
- return -1;
- }
- bool up = false;
- switch (event.type) {
- case EV_ABS:
- if (event.code == ABS_MT_POSITION_X) {
- s->last_x = event.value;
- } else if (event.code == ABS_MT_POSITION_Y) {
- s->last_y = event.value;
- }
- up = true;
- break;
- default:
- break;
- }
- if (up) {
- // adjust for flippening
- *out_x = s->last_y;
- *out_y = 1080 - s->last_x;
- }
- return up;
-}
diff --git a/selfdrive/common/touch.h b/selfdrive/common/touch.h
index c48f66b98..fa0faa271 100644
--- a/selfdrive/common/touch.h
+++ b/selfdrive/common/touch.h
@@ -1,5 +1,4 @@
-#ifndef TOUCH_H
-#define TOUCH_H
+#pragma once
#ifdef __cplusplus
extern "C" {
@@ -12,10 +11,7 @@ typedef struct TouchState {
void touch_init(TouchState *s);
int touch_poll(TouchState *s, int *out_x, int *out_y, int timeout);
-int touch_read(TouchState *s, int* out_x, int* out_y);
#ifdef __cplusplus
}
#endif
-
-#endif
diff --git a/selfdrive/common/util.c b/selfdrive/common/util.c
index 3728dabcd..33930f9ec 100644
--- a/selfdrive/common/util.c
+++ b/selfdrive/common/util.c
@@ -3,7 +3,8 @@
#include
#include
#include
-
+#include
+#include
#ifdef __linux__
#include
#include
@@ -41,6 +42,16 @@ 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);
+ if (fd == -1) {
+ return -1;
+ }
+ ssize_t n = write(fd, data, size);
+ close(fd);
+ return n == size ? 0 : -1;
+}
+
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
@@ -50,7 +61,6 @@ void set_thread_name(const char* name) {
int set_realtime_priority(int level) {
#ifdef __linux__
-
long tid = syscall(SYS_gettid);
// should match python using chrt
@@ -64,8 +74,7 @@ int set_realtime_priority(int level) {
}
int set_core_affinity(int core) {
-#ifdef QCOM
-
+#ifdef __linux__
long tid = syscall(SYS_gettid);
cpu_set_t rt_cpu;
diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h
index a9052146c..18d9619ce 100644
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@@ -41,6 +41,7 @@ extern "C" {
// 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);
void set_thread_name(const char* name);
diff --git a/selfdrive/common/utilpp.h b/selfdrive/common/utilpp.h
index 6a68b5fba..e0547e0c1 100644
--- a/selfdrive/common/utilpp.h
+++ b/selfdrive/common/utilpp.h
@@ -1,5 +1,4 @@
-#ifndef UTILPP_H
-#define UTILPP_H
+#pragma once
#include
#include
@@ -63,4 +62,16 @@ inline std::string readlink(std::string path) {
}
-#endif
+struct unique_fd {
+ unique_fd(int fd = -1) : fd_(fd) {}
+ unique_fd& operator=(unique_fd&& uf) {
+ fd_ = uf.fd_;
+ uf.fd_ = -1;
+ return *this;
+ }
+ ~unique_fd() {
+ if (fd_ != -1) close(fd_);
+ }
+ operator int() const { return fd_; }
+ int fd_;
+};
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index d95231d5c..ddbf69550 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.7.8-release"
+#define COMMA_VERSION "0.7.9-release"
diff --git a/selfdrive/common/visionbuf_cl.c b/selfdrive/common/visionbuf_cl.c
index c68950baa..21f100507 100644
--- a/selfdrive/common/visionbuf_cl.c
+++ b/selfdrive/common/visionbuf_cl.c
@@ -110,6 +110,7 @@ void visionbuf_free(const VisionBuf* buf) {
#if __OPENCL_VERSION__ >= 200
clSVMFree(buf->ctx, buf->addr);
#else
+ clReleaseCommandQueue(buf->copy_q);
munmap(buf->addr, buf->len);
close(buf->fd);
#endif
diff --git a/selfdrive/common/visionipc.c b/selfdrive/common/visionipc.c
index bae05ccc7..682049115 100644
--- a/selfdrive/common/visionipc.c
+++ b/selfdrive/common/visionipc.c
@@ -90,6 +90,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
err = vipc_send(s->ipc_fd, &p);
if (err < 0) {
close(s->ipc_fd);
+ s->ipc_fd = -1;
return -1;
}
@@ -97,6 +98,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
err = vipc_recv(s->ipc_fd, &rp);
if (err <= 0) {
close(s->ipc_fd);
+ s->ipc_fd = -1;
return -1;
}
assert(rp.type == VIPC_STREAM_BUFS);
@@ -190,5 +192,5 @@ void visionstream_destroy(VisionStream *s) {
}
}
if (s->bufs) free(s->bufs);
- close(s->ipc_fd);
+ if (s->ipc_fd >= 0) close(s->ipc_fd);
}
diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h
index 4844a71b1..1b216f525 100644
--- a/selfdrive/common/visionipc.h
+++ b/selfdrive/common/visionipc.h
@@ -23,8 +23,10 @@ typedef enum VisionIPCPacketType {
typedef enum VisionStreamType {
VISION_STREAM_RGB_BACK,
VISION_STREAM_RGB_FRONT,
+ VISION_STREAM_RGB_WIDE,
VISION_STREAM_YUV,
VISION_STREAM_YUV_FRONT,
+ VISION_STREAM_YUV_WIDE,
VISION_STREAM_MAX,
} VisionStreamType;
@@ -35,6 +37,10 @@ typedef struct VisionUIInfo {
int front_box_x, front_box_y;
int front_box_width, front_box_height;
+
+ int wide_box_x, wide_box_y;
+ int wide_box_width, wide_box_height;
+
} VisionUIInfo;
typedef struct VisionStreamBufs {
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 2546a598a..fe576192e 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -1,10 +1,9 @@
#!/usr/bin/env python3
import os
-import gc
from cereal import car, log
-from common.android import ANDROID, get_sound_card_online
+from common.hardware import HARDWARE
from common.numpy_fast import clip
-from common.realtime import sec_since_boot, set_realtime_priority, set_core_affinity, Ratekeeper, DT_CTRL
+from common.realtime import sec_since_boot, config_rt_process, Priority, Ratekeeper, DT_CTRL
from common.profiler import Profiler
from common.params import Params, put_nonblocking
import cereal.messaging as messaging
@@ -21,13 +20,16 @@ from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.planner import LON_MPC_STEP
-from selfdrive.locationd.calibration_helpers import Calibration
+from selfdrive.locationd.calibrationd import Calibration
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
+SIMULATION = "SIMULATION" in os.environ
+NOSENSOR = "NOSENSOR" in os.environ
+
ThermalStatus = log.ThermalData.ThermalStatus
State = log.ControlsState.OpenpilotState
HwType = log.HealthData.HwType
@@ -40,9 +42,7 @@ EventName = car.CarEvent.EventName
class Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
- gc.disable()
- set_realtime_priority(53)
- set_core_affinity(3)
+ config_rt_process(3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
@@ -79,11 +79,11 @@ class Controls:
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
- sounds_available = not ANDROID or get_sound_card_online()
+ sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
- controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
+ controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
@@ -94,7 +94,6 @@ class Controls:
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
- put_nonblocking("LongitudinalControl", "1" if self.CP.openpilotLongitudinalControl else "0")
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
@@ -139,7 +138,7 @@ class Controls:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
- if self.read_only and not passive:
+ if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if hw_type == HwType.whitePanda:
self.events.add(EventName.whitePandaUnsupported, static=True)
@@ -192,7 +191,7 @@ class Controls:
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting,
- LaneChangeState.laneChangeFinishing]:
+ LaneChangeState.laneChangeFinishing]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL):
@@ -206,12 +205,13 @@ class Controls:
self.events.add(EventName.commIssue)
if not self.sm['pathPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
- if not self.sm['liveLocationKalman'].sensorsOK and os.getenv("NOSENSOR") is None:
+ if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
- if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and os.getenv("NOSENSOR") is None:
+ if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
# 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 (SIMULATION or NOSENSOR): # TODO: send GPS in carla
+ self.events.add(EventName.noGps)
if not self.sm['pathPlan'].paramsValid:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
@@ -229,12 +229,12 @@ class Controls:
self.events.add(EventName.relayMalfunction)
if self.sm['plan'].fcw:
self.events.add(EventName.fcw)
- if self.sm['model'].frameDropPerc > 1:
- self.events.add(EventName.modeldLagging)
+ if self.sm['model'].frameDropPerc > 1 and (not SIMULATION):
+ self.events.add(EventName.modeldLagging)
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
- and not self.CP.radarOffCan and CS.vEgo < 0.3:
+ and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
self.events.add(EventName.noTarget)
def data_sample(self):
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index 870d8d4b7..6eaa86d3e 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -1,24 +1,21 @@
import os
import copy
import json
+from typing import List, Optional
from cereal import car, log
from common.basedir import BASEDIR
from common.params import Params
from common.realtime import DT_CTRL
+from selfdrive.controls.lib.events import Alert
from selfdrive.swaglog import cloudlog
-AlertSize = log.ControlsState.AlertSize
-AlertStatus = log.ControlsState.AlertStatus
-VisualAlert = car.CarControl.HUDControl.VisualAlert
-AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-
with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
OFFROAD_ALERTS = json.load(f)
-def set_offroad_alert(alert, show_alert, extra_text=None):
+def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] = None) -> None:
if show_alert:
a = OFFROAD_ALERTS[alert]
if extra_text is not None:
@@ -29,19 +26,30 @@ def set_offroad_alert(alert, show_alert, extra_text=None):
Params().delete(alert)
-class AlertManager():
+class AlertManager:
def __init__(self):
- self.activealerts = []
+ self.activealerts: List[Alert] = []
+ self.clear_current_alert()
- def alert_present(self):
+ def alert_present(self) -> bool:
return len(self.activealerts) > 0
- def add_many(self, frame, alerts, enabled=True):
+ def clear_current_alert(self) -> None:
+ self.alert_type: str = ""
+ self.alert_text_1: str = ""
+ self.alert_text_2: str = ""
+ self.alert_status = log.ControlsState.AlertStatus.normal
+ self.alert_size = log.ControlsState.AlertSize.none
+ self.visual_alert = car.CarControl.HUDControl.VisualAlert.none
+ self.audible_alert = car.CarControl.HUDControl.AudibleAlert.none
+ self.alert_rate: float = 0.
+
+ def add_many(self, frame: int, alerts: List[Alert], enabled: bool = True) -> None:
for a in alerts:
self.add(frame, a, enabled=enabled)
- def add(self, frame, alert, enabled=True):
+ def add(self, frame: int, alert: Alert, enabled: bool = True) -> None:
added_alert = copy.copy(alert)
added_alert.start_time = frame * DT_CTRL
@@ -54,26 +62,18 @@ class AlertManager():
# sort by priority first and then by start_time
self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True)
- def process_alerts(self, frame):
+ def process_alerts(self, frame: int) -> None:
cur_time = frame * DT_CTRL
# first get rid of all the expired alerts
self.activealerts = [a for a in self.activealerts if a.start_time +
max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time]
- current_alert = self.activealerts[0] if self.alert_present() else None
-
# start with assuming no alerts
- self.alert_type = ""
- self.alert_text_1 = ""
- self.alert_text_2 = ""
- self.alert_status = AlertStatus.normal
- self.alert_size = AlertSize.none
- self.visual_alert = VisualAlert.none
- self.audible_alert = AudibleAlert.none
- self.alert_rate = 0.
+ self.clear_current_alert()
- if current_alert:
+ current_alert = self.activealerts[0] if self.alert_present() else None
+ if current_alert is not None:
self.alert_type = current_alert.alert_type
if current_alert.start_time + current_alert.duration_sound > cur_time:
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 205c6a172..67a6838d7 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -1,9 +1,11 @@
from functools import total_ordering
+from typing import Dict, Union, Callable, Any
from cereal import log, car
+import cereal.messaging as messaging
from common.realtime import DT_CTRL
from selfdrive.config import Conversions as CV
-from selfdrive.locationd.calibration_helpers import Filter
+from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
AlertSize = log.ControlsState.AlertSize
AlertStatus = log.ControlsState.AlertStatus
@@ -97,18 +99,18 @@ class Events:
@total_ordering
class Alert:
def __init__(self,
- alert_text_1,
- alert_text_2,
+ alert_text_1: str,
+ alert_text_2: str,
alert_status,
alert_size,
alert_priority,
visual_alert,
audible_alert,
- duration_sound,
- duration_hud_alert,
- duration_text,
- alert_rate=0.,
- creation_delay=0.):
+ duration_sound: float,
+ duration_hud_alert: float,
+ duration_text: float,
+ alert_rate: float = 0.,
+ creation_delay: float = 0.):
self.alert_type = ""
self.alert_text_1 = alert_text_1
@@ -131,14 +133,14 @@ class Alert:
tst = car.CarControl.new_message()
tst.hudControl.visualAlert = self.visual_alert
- def __str__(self):
+ def __str__(self) -> str:
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str(
self.visual_alert) + " " + str(self.audible_alert)
- def __gt__(self, alert2):
+ def __gt__(self, alert2) -> bool:
return self.alert_priority > alert2.alert_priority
- def __eq__(self, alert2):
+ def __eq__(self, alert2) -> bool:
return self.alert_priority == alert2.alert_priority
class NoEntryAlert(Alert):
@@ -174,25 +176,25 @@ class EngagementAlert(Alert):
# ********** alert callback functions **********
-def below_steer_speed_alert(CP, sm, metric):
+def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
speed = int(round(CP.minSteerSpeed * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)))
- unit = "kph" if metric else "mph"
+ unit = "km/h" if metric else "mph"
return Alert(
"TAKE CONTROL",
"Steer Unavailable Below %d %s" % (speed, unit),
AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3)
-def calibration_incomplete_alert(CP, sm, metric):
- speed = int(Filter.MIN_SPEED * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
- unit = "kph" if metric else "mph"
+def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
+ speed = int(MIN_SPEED_FILTER * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
+ unit = "km/h" if metric else "mph"
return Alert(
"Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,
"Drive Above %d %s" % (speed, unit),
AlertStatus.normal, AlertSize.mid,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2)
-def no_gps_alert(CP, sm, metric):
+def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
gps_integrated = sm['health'].hwType in [log.HealthData.HwType.uno, log.HealthData.HwType.dos]
return Alert(
"Poor GPS reception",
@@ -200,13 +202,13 @@ def no_gps_alert(CP, sm, metric):
AlertStatus.normal, AlertSize.mid,
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2, creation_delay=300.)
-def wrong_car_mode_alert(CP, sm, metric):
+def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
text = "Cruise Mode Disabled"
if CP.carName == "honda":
text = "Main Switch Off"
return NoEntryAlert(text, duration_hud_alert=0.)
-EVENTS = {
+EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = {
# ********** events with no alerts **********
# ********** events only containing alerts displayed in all states **********
@@ -227,14 +229,6 @@ EVENTS = {
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
},
- EventName.startupWhitePanda: {
- ET.PERMANENT: Alert(
- "WARNING: White panda is deprecated",
- "Upgrade to comma two or black panda",
- AlertStatus.userPrompt, AlertSize.mid,
- Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
- },
-
EventName.startupMaster: {
ET.PERMANENT: Alert(
"WARNING: This branch is not tested",
@@ -287,7 +281,6 @@ EVENTS = {
EventName.communityFeatureDisallowed: {
# LOW priority to overcome Cruise Error
ET.PERMANENT: Alert(
- "",
"Community Feature Detected",
"Enable Community Features in Developer Settings",
AlertStatus.normal, AlertSize.mid,
@@ -341,7 +334,7 @@ EVENTS = {
"openpilot will not brake while gas pressed",
"",
AlertStatus.normal, AlertSize.small,
- Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .0, .0, .1),
+ Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .0, .0, .1, creation_delay=1.),
},
EventName.vehicleModelInvalid: {
@@ -413,7 +406,7 @@ EVENTS = {
"CHECK DRIVER FACE VISIBILITY",
"Driver Monitor Model Output Uncertain",
AlertStatus.normal, AlertSize.mid,
- Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 0., 1.),
+ Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 0., 1.5),
},
EventName.manualRestart: {
@@ -535,7 +528,7 @@ EVENTS = {
"TAKE CONTROL",
"Attempting Refocus: Camera Focus Invalid",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
+ Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3., creation_delay=3.1),
},
EventName.outOfSpace: {
@@ -593,7 +586,12 @@ EVENTS = {
},
EventName.calibrationInvalid: {
- ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Reposition Device and Recalibrate"),
+ ET.PERMANENT: Alert(
+ "Calibration Invalid",
+ "Reposition Device and Recalibrate",
+ AlertStatus.normal, AlertSize.mid,
+ Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+ ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Reposition Device & Recalibrate"),
ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Reposition Device & Recalibrate"),
},
@@ -716,6 +714,11 @@ EVENTS = {
},
EventName.reverseGear: {
+ ET.PERMANENT: Alert(
+ "Reverse\nGear",
+ "",
+ AlertStatus.normal, AlertSize.full,
+ Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2, creation_delay=0.5),
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Reverse Gear"),
ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
},
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index b83354065..f9bf15cc0 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -56,7 +56,7 @@ class LongitudinalMpc():
self.cur_state[0].v_ego = v
self.cur_state[0].a_ego = a
- def update(self, pm, CS, lead, v_cruise_setpoint):
+ def update(self, pm, CS, lead):
v_ego = CS.vEgo
# Setup current mpc state
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 5262f480a..f31dcb05b 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -14,9 +14,6 @@ STOPPING_BRAKE_RATE = 0.2 # brake_travel/s while trying to stop
STARTING_BRAKE_RATE = 0.8 # brake_travel/s while releasing on restart
BRAKE_STOPPING_TARGET = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
-_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints
-_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
-
RATE = 100.0
@@ -86,8 +83,7 @@ class LongControl():
v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
- self.v_pid = v_ego_pid
- self.pid.reset()
+ self.reset(v_ego_pid)
output_gb = 0.
# tracking objects and driving
@@ -113,15 +109,13 @@ class LongControl():
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
- self.v_pid = CS.vEgo
- self.pid.reset()
+ self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE
- self.v_pid = CS.vEgo
- self.pid.reset()
+ self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 553e5bb6f..0412df36d 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -15,10 +15,7 @@ from selfdrive.controls.lib.fcw import FCWChecker
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
-MAX_SPEED = 255.0
-
LON_MPC_STEP = 0.2 # first step is 0.2s
-MAX_SPEED_ERROR = 2.0
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
# lookup tables VS speed to determine min and max accels in cruise
@@ -36,9 +33,6 @@ _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.]
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
-# 75th percentile
-SPEED_PERCENTILE_IDX = 7
-
def calc_cruise_accel_limits(v_ego, following):
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
@@ -162,8 +156,8 @@ class Planner():
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
- self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
- self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
+ self.mpc1.update(pm, sm['carState'], lead_1)
+ self.mpc2.update(pm, sm['carState'], lead_2)
self.choose_solution(v_cruise_setpoint, enabled)
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index b439681a3..44dd1ec62 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -1,9 +1,7 @@
#!/usr/bin/env python3
-import gc
-
from cereal import car
from common.params import Params
-from common.realtime import set_realtime_priority
+from common.realtime import Priority, config_rt_process
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.planner import Planner
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -12,10 +10,8 @@ import cereal.messaging as messaging
def plannerd_thread(sm=None, pm=None):
- gc.disable()
- # start the loop
- set_realtime_priority(52)
+ config_rt_process(2, Priority.CTRL_LOW)
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
@@ -27,7 +23,8 @@ def plannerd_thread(sm=None, pm=None):
VM = VehicleModel(CP)
if sm is None:
- sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
+ sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'],
+ poll=['radarState', 'model'])
if pm is None:
pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 0ba14dcf2..c2643d76e 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -7,7 +7,7 @@ import cereal.messaging as messaging
from cereal import car
from common.numpy_fast import interp
from common.params import Params
-from common.realtime import Ratekeeper, set_realtime_priority
+from common.realtime import Ratekeeper, Priority, set_realtime_priority
from selfdrive.config import RADAR_TO_CAMERA
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
from selfdrive.controls.lib.radar_helpers import Cluster, Track
@@ -99,7 +99,7 @@ class RadarD():
self.ready = False
- def update(self, frame, sm, rr, has_radar):
+ def update(self, frame, sm, rr, enable_lead):
self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
if sm.updated['controlsState']:
@@ -166,7 +166,7 @@ class RadarD():
dat.radarState.radarErrors = list(rr.errors)
dat.radarState.controlsStateMonoTime = sm.logMonoTime['controlsState']
- if has_radar:
+ if enable_lead:
dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
return dat
@@ -174,7 +174,7 @@ class RadarD():
# fuses camera and radar data for best lead detection
def radard_thread(sm=None, pm=None, can_sock=None):
- set_realtime_priority(52)
+ set_realtime_priority(Priority.CTRL_LOW)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
@@ -200,7 +200,8 @@ def radard_thread(sm=None, pm=None, can_sock=None):
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
- has_radar = not CP.radarOffCan
+ # TODO: always log leads once we can hide them conditionally
+ enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
@@ -211,7 +212,7 @@ def radard_thread(sm=None, pm=None, can_sock=None):
sm.update(0)
- dat = RD.update(rk.frame, sm, rr, has_radar)
+ dat = RD.update(rk.frame, sm, rr, enable_lead)
dat.radarState.cumLagMs = -rk.remaining*1000.
pm.send('radarState', dat)
diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index 7459f7166..22d6aa068 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -6,9 +6,9 @@ import capnp
from selfdrive.version import version, dirty
from selfdrive.swaglog import cloudlog
-from common.android import ANDROID
+from common.hardware import PC
-if os.getenv("NOLOG") or os.getenv("NOCRASH") or not ANDROID:
+if os.getenv("NOLOG") or os.getenv("NOCRASH") or PC:
def capture_exception(*args, **kwargs):
pass
diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py
index 28cabe6c8..9b5681e89 100755
--- a/selfdrive/debug/cpu_usage_stat.py
+++ b/selfdrive/debug/cpu_usage_stat.py
@@ -30,8 +30,16 @@ PRINT_INTERVAL = 5
SLEEP_INTERVAL = 0.2
monitored_proc_names = [
- 'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd', 'logmessaged', 'tombstoned',
- 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'dmonitoringd', 'dmonitoringmodeld', 'camerad', 'sensord', 'updated', 'gpsd', 'athena', 'locationd', 'paramsd']
+ # openpilot procs
+ 'controlsd', 'locationd', 'loggerd','plannerd',
+ 'ubloxd', 'thermald', 'uploader', 'deleter', 'radard', 'logmessaged', 'tombstoned',
+ 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'dmonitoringd',
+ 'dmonitoringmodeld', 'camerad', 'sensord', 'updated', 'gpsd', 'athena', 'locationd', 'paramsd',
+
+ # android procs
+ 'SurfaceFlinger', 'sensors.qcom'
+]
+
cpu_time_names = ['user', 'system', 'children_user', 'children_system']
timer = getattr(time, 'monotonic', time.time)
diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py
index 63b7d758c..45bc8984c 100755
--- a/selfdrive/debug/cycle_alerts.py
+++ b/selfdrive/debug/cycle_alerts.py
@@ -3,56 +3,65 @@
# pylint: skip-file
# type: ignore
-import argparse
import time
import cereal.messaging as messaging
-from selfdrive.controls.lib.events import EVENTS, Alert
+from selfdrive.car.honda.interface import CarInterface
+from selfdrive.controls.lib.events import ET, EVENTS, Events
+from selfdrive.controls.lib.alertmanager import AlertManager
-def now_millis(): return time.time() * 1000
-ALERTS = [a for _, et in EVENTS.items() for _, a in et.items() if isinstance(a, Alert)]
+def cycle_alerts(duration=200, is_metric=False):
+ alerts = list(EVENTS.keys())
+ print(alerts)
-#from cereal import car
-#ALERTS = [a for a in ALERTS if a.audible_alert == car.CarControl.HUDControl.AudibleAlert.chimeWarningRepeat]
-
-default_alerts = sorted(ALERTS, key=lambda alert: (alert.alert_size, len(alert.alert_text_2)))
-
-def cycle_alerts(duration_millis, alerts=None):
- if alerts is None:
- alerts = default_alerts
+ CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
+ sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration',
+ 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
controls_state = messaging.pub_sock('controlsState')
+ thermal = messaging.pub_sock('thermal')
idx, last_alert_millis = 0, 0
- alert = alerts[0]
+
+ events = Events()
+ AM = AlertManager()
+
+ frame = 0
+
while 1:
- if (now_millis() - last_alert_millis) > duration_millis:
- alert = alerts[idx]
+ if frame % duration == 0:
idx = (idx + 1) % len(alerts)
- last_alert_millis = now_millis()
- print('sending {}'.format(str(alert)))
+ events.clear()
+ events.add(alerts[idx])
+
+
+ current_alert_types = [ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
+ ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY,
+ ET.ENABLE, ET.WARNING]
+ a = events.create_alerts(current_alert_types, [CP, sm, is_metric])
+ AM.add_many(frame, a)
+ AM.process_alerts(frame)
dat = messaging.new_message()
dat.init('controlsState')
- dat.controlsState.alertType = alert.alert_type
- dat.controlsState.alertText1 = alert.alert_text_1
- dat.controlsState.alertText2 = alert.alert_text_2
- dat.controlsState.alertSize = alert.alert_size
- #dat.controlsState.alertStatus = alert.alert_status
- dat.controlsState.alertSound = alert.audible_alert
+ dat.controlsState.alertText1 = AM.alert_text_1
+ dat.controlsState.alertText2 = AM.alert_text_2
+ dat.controlsState.alertSize = AM.alert_size
+ dat.controlsState.alertStatus = AM.alert_status
+ dat.controlsState.alertBlinkingRate = AM.alert_rate
+ dat.controlsState.alertType = AM.alert_type
+ dat.controlsState.alertSound = AM.audible_alert
controls_state.send(dat.to_bytes())
+ dat = messaging.new_message()
+ dat.init('thermal')
+ dat.thermal.started = True
+ thermal.send(dat.to_bytes())
+
+ frame += 1
time.sleep(0.01)
if __name__ == '__main__':
- parser = argparse.ArgumentParser()
- parser.add_argument('--duration', type=int, default=1000)
- parser.add_argument('--alert-types', nargs='+')
- args = parser.parse_args()
- alerts = None
- if args.alert_types:
- alerts = [next(a for a in ALERTS if a.alert_type==alert_type) for alert_type in args.alert_types]
-
- cycle_alerts(args.duration, alerts=alerts)
+ cycle_alerts()
diff --git a/selfdrive/debug/disable_ecu.py b/selfdrive/debug/disable_ecu.py
new file mode 100644
index 000000000..9dace9a68
--- /dev/null
+++ b/selfdrive/debug/disable_ecu.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+import traceback
+
+import cereal.messaging as messaging
+from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from selfdrive.swaglog import cloudlog
+
+EXT_DIAG_REQUEST = b'\x10\x03'
+EXT_DIAG_RESPONSE = b'\x50\x03'
+COM_CONT_REQUEST = b'\x28\x83\x03'
+COM_CONT_RESPONSE = b''
+
+def disable_ecu(ecu_addr, logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
+ print(f"ecu disable {hex(ecu_addr)} ...")
+ for i in range(retry):
+ try:
+ # enter extended diagnostic session
+ query = IsoTpParallelQuery(sendcan, logcan, bus, [ecu_addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
+ for addr, dat in query.get_data(timeout).items(): # pylint: disable=unused-variable
+ print("ecu communication control disable tx/rx ...")
+ # communication control disable tx and rx
+ query = IsoTpParallelQuery(sendcan, logcan, bus, [ecu_addr], [COM_CONT_REQUEST], [COM_CONT_RESPONSE], debug=debug)
+ query.get_data(0)
+ return True
+ print(f"ecu disable retry ({i+1}) ...")
+ except Exception:
+ cloudlog.warning(f"ecu disable exception: {traceback.format_exc()}")
+
+ return False
+
+
+if __name__ == "__main__":
+ import time
+ sendcan = messaging.pub_sock('sendcan')
+ logcan = messaging.sub_sock('can')
+ time.sleep(1)
+
+ # honda bosch radar disable
+ disabled = disable_ecu(0x18DAB0F1, logcan, sendcan, 1, debug=False)
+ print(f"disabled: {disabled}")
diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py
index cfdb387b9..112ca96a9 100755
--- a/selfdrive/debug/dump.py
+++ b/selfdrive/debug/dump.py
@@ -61,4 +61,11 @@ if __name__ == "__main__":
print("{} = {}".format(".".join(value), item))
print("")
else:
- print(evt)
+ try:
+ print(evt)
+ except UnicodeDecodeError:
+ w = evt.which()
+ s = f"( logMonoTime {evt.logMonoTime} \n {w} = "
+ s += str(evt.__getattr__(w))
+ s += f"\n valid = {evt.valid} )"
+ print(s)
diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py
index 9ece7d726..2695d457d 100755
--- a/selfdrive/debug/get_fingerprint.py
+++ b/selfdrive/debug/get_fingerprint.py
@@ -17,6 +17,9 @@ logcan = messaging.sub_sock('can')
msgs = {}
while True:
lc = messaging.recv_sock(logcan, True)
+ if lc is None:
+ continue
+
for c in lc.can:
# read also msgs sent by EON on CAN bus 0x80 and filter out the
# addr with more than 11 bits
diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py
old mode 100644
new mode 100755
index 899f932ad..e31d6cf3c
--- a/selfdrive/debug/uiview.py
+++ b/selfdrive/debug/uiview.py
@@ -3,12 +3,12 @@ import time
import cereal.messaging as messaging
from selfdrive.manager import start_managed_process, kill_managed_process
-services = ['controlsState', 'thermal'] # the services needed to be spoofed to start ui offroad
-procs = ['camerad', 'ui']
+services = ['controlsState', 'thermal', '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_thermal = [messaging.new_message(s) for s in services]
+dat_cs, dat_thermal, dat_radar = [messaging.new_message(s) for s in services]
dat_cs.controlsState.rearViewCam = False # ui checks for these two messages
dat_thermal.thermal.started = True
@@ -16,6 +16,7 @@ try:
while True:
pm.send('controlsState', dat_cs)
pm.send('thermal', dat_thermal)
+ pm.send('radarState', dat_radar)
time.sleep(1 / 100) # continually send, rate doesn't matter for thermal
except KeyboardInterrupt:
[kill_managed_process(p) for p in procs]
diff --git a/selfdrive/locationd/calibration_helpers.py b/selfdrive/locationd/calibration_helpers.py
deleted file mode 100644
index c7886d301..000000000
--- a/selfdrive/locationd/calibration_helpers.py
+++ /dev/null
@@ -1,10 +0,0 @@
-import math
-
-class Filter:
- MIN_SPEED = 7 # m/s (~15.5mph)
- MAX_YAW_RATE = math.radians(3) # per second
-
-class Calibration:
- UNCALIBRATED = 0
- CALIBRATED = 1
- INVALID = 2
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index a27e284a9..9cda80cfe 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -1,4 +1,10 @@
#!/usr/bin/env python3
+'''
+This process finds calibration values. More info on what these calibration values
+are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
+While the roll calibration is a real value that can be estimated, here we assume it's zero,
+and the image input into the neural network is not corrected for roll.
+'''
import os
import copy
@@ -6,177 +12,196 @@ import json
import numpy as np
import cereal.messaging as messaging
from selfdrive.config import Conversions as CV
-from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.swaglog import cloudlog
from common.params import Params, put_nonblocking
from common.transformations.model import model_height
-from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
- get_calib_from_vp, vp_from_rpy, H, W, FOCAL
+from common.transformations.camera import get_view_frame_from_road_frame
+from common.transformations.orientation import rot_from_euler, euler_from_rot
MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
MAX_VEL_ANGLE_STD = np.radians(0.25)
MAX_YAW_RATE_FILTER = np.radians(2) # per second
-# This is all 20Hz, blocks needed for efficiency
+# This is at model frequency, blocks needed for efficiency
+SMOOTH_CYCLES = 400
BLOCK_SIZE = 100
-INPUTS_NEEDED = 5 # allow to update VP every so many frames
+INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration
INPUTS_WANTED = 50 # We want a little bit more than we need for stability
-WRITE_CYCLES = 10 # write every 1000 cycles
-VP_INIT = np.array([W/2., H/2.])
+MAX_ALLOWED_SPREAD = np.radians(2)
+RPY_INIT = np.array([0.0,0.0,0.0])
# These values are needed to accomodate biggest modelframe
-VP_VALIDITY_CORNERS = np.array([[W//2 - 63, 300], [W//2 + 63, 520]])
+PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657])
+YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
DEBUG = os.getenv("DEBUG") is not None
-def is_calibration_valid(vp):
- return vp[0] > VP_VALIDITY_CORNERS[0, 0] and vp[0] < VP_VALIDITY_CORNERS[1, 0] and \
- vp[1] > VP_VALIDITY_CORNERS[0, 1] and vp[1] < VP_VALIDITY_CORNERS[1, 1]
+class Calibration:
+ UNCALIBRATED = 0
+ CALIBRATED = 1
+ INVALID = 2
-def sanity_clip(vp):
- if np.isnan(vp).any():
- vp = VP_INIT
- return np.array([np.clip(vp[0], VP_VALIDITY_CORNERS[0, 0] - 5, VP_VALIDITY_CORNERS[1, 0] + 5),
- np.clip(vp[1], VP_VALIDITY_CORNERS[0, 1] - 5, VP_VALIDITY_CORNERS[1, 1] + 5)])
+def is_calibration_valid(rpy):
+ return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
-def intrinsics_from_vp(vp):
- return np.array([
- [FOCAL, 0., vp[0]],
- [ 0., FOCAL, vp[1]],
- [ 0., 0., 1.]])
+def sanity_clip(rpy):
+ if np.isnan(rpy).any():
+ rpy = RPY_INIT
+ return np.array([rpy[0],
+ np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
+ np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
class Calibrator():
def __init__(self, param_put=False):
self.param_put = param_put
- self.vp = copy.copy(VP_INIT)
- self.vps = np.zeros((INPUTS_WANTED, 2))
- self.idx = 0
- self.block_idx = 0
- self.valid_blocks = 0
- self.cal_status = Calibration.UNCALIBRATED
- self.just_calibrated = False
- self.v_ego = 0
- # Read calibration
- if param_put:
- calibration_params = Params().get("CalibrationParams")
- else:
- calibration_params = None
- if calibration_params:
+ # Read saved calibration
+ calibration_params = Params().get("CalibrationParams")
+
+ rpy_init = RPY_INIT
+ valid_blocks = 0
+
+ if param_put and calibration_params:
try:
calibration_params = json.loads(calibration_params)
- self.vp = vp_from_rpy(calibration_params["calib_radians"])
- if not np.isfinite(self.vp).all():
- self.vp = copy.copy(VP_INIT)
- self.vps = np.tile(self.vp, (INPUTS_WANTED, 1))
- self.valid_blocks = calibration_params['valid_blocks']
- if not np.isfinite(self.valid_blocks) or self.valid_blocks < 0:
- self.valid_blocks = 0
- self.update_status()
+ rpy_init = calibration_params["calib_radians"]
+ valid_blocks = calibration_params['valid_blocks']
except Exception:
cloudlog.exception("CalibrationParams file found but error encountered")
+ self.reset(rpy_init, valid_blocks)
+ self.update_status()
+
+ def reset(self, rpy_init=RPY_INIT, valid_blocks=0, smooth_from=None):
+ if not np.isfinite(rpy_init).all():
+ self.rpy = copy.copy(RPY_INIT)
+ else:
+ self.rpy = rpy_init
+ if not np.isfinite(valid_blocks) or valid_blocks < 0:
+ self.valid_blocks = 0
+ else:
+ self.valid_blocks = valid_blocks
+ self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
+
+ self.idx = 0
+ self.block_idx = 0
+ self.v_ego = 0
+
+ if smooth_from is None:
+ self.old_rpy = RPY_INIT
+ self.old_rpy_weight = 0.0
+ else:
+ self.old_rpy = smooth_from
+ self.old_rpy_weight = 1.0
def update_status(self):
- start_status = self.cal_status
+ if self.valid_blocks > 0:
+ max_rpy_calib = np.array(np.max(self.rpys[:self.valid_blocks], axis=0))
+ min_rpy_calib = np.array(np.min(self.rpys[:self.valid_blocks], axis=0))
+ self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
+ else:
+ self.calib_spread = np.zeros(3)
+
if self.valid_blocks < INPUTS_NEEDED:
self.cal_status = Calibration.UNCALIBRATED
+ elif is_calibration_valid(self.rpy):
+ self.cal_status = Calibration.CALIBRATED
else:
- self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
- end_status = self.cal_status
+ self.cal_status = Calibration.INVALID
- self.just_calibrated = False
- if start_status == Calibration.UNCALIBRATED and end_status != Calibration.UNCALIBRATED:
- self.just_calibrated = True
+ # If spread is too high, assume mounting was changed and reset to last block.
+ # Make the transition smooth. Abrupt transistion are not good foor feedback loop through supercombo model.
+ if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
+ self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)
+
+ write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
+ if self.param_put and write_this_cycle:
+ # TODO: this should use the liveCalibration struct from cereal
+ cal_params = {"calib_radians": list(self.rpy),
+ "valid_blocks": int(self.valid_blocks)}
+ put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
def handle_v_ego(self, v_ego):
self.v_ego = v_ego
+ def get_smooth_rpy(self):
+ if self.old_rpy_weight > 0:
+ return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy
+ else:
+ return self.rpy
+
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
+ self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
+
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
(self.valid_blocks < INPUTS_NEEDED))
if straight_and_fast and certain_if_calib:
- # intrinsics are not eon intrinsics, since this is calibrated frame
- intrinsics = intrinsics_from_vp(self.vp)
- new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
- new_vp = new_vp[:2]/new_vp[2]
- new_vp = sanity_clip(new_vp)
+ observed_rpy = np.array([0,
+ -np.arctan2(trans[2], trans[0]),
+ np.arctan2(trans[1], trans[0])])
+ new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
+ new_rpy = sanity_clip(new_rpy)
- self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
+ self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
if self.valid_blocks > 0:
- self.vp = np.mean(self.vps[:self.valid_blocks], axis=0)
+ self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0)
+
+
self.update_status()
- if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
- calib = get_calib_from_vp(self.vp)
- cal_params = {"calib_radians": list(calib),
- "valid_blocks": self.valid_blocks}
- put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
- return new_vp
+ return new_rpy
else:
return None
def send_data(self, pm):
- calib = get_calib_from_vp(self.vp)
- if self.valid_blocks > 0:
- max_vp_calib = np.array(get_calib_from_vp(np.max(self.vps[:self.valid_blocks], axis=0)))
- min_vp_calib = np.array(get_calib_from_vp(np.min(self.vps[:self.valid_blocks], axis=0)))
- calib_spread = np.abs(max_vp_calib - min_vp_calib)
- else:
- calib_spread = np.zeros(3)
- extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
+ smooth_rpy = self.get_smooth_rpy()
+ extrinsic_matrix = get_view_frame_from_road_frame(0, smooth_rpy[1], smooth_rpy[2], model_height)
cal_send = messaging.new_message('liveCalibration')
cal_send.liveCalibration.validBlocks = self.valid_blocks
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
- cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
- cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in calib_spread]
+ cal_send.liveCalibration.rpyCalib = [float(x) for x in smooth_rpy]
+ cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in self.calib_spread]
pm.send('liveCalibration', cal_send)
def calibrationd_thread(sm=None, pm=None):
if sm is None:
- sm = messaging.SubMaster(['cameraOdometry', 'carState'])
+ sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll=['cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveCalibration'])
calibrator = Calibrator(param_put=True)
- send_counter = 0
while 1:
- sm.update()
-
- # if no inputs still publish calibration
- if not sm.updated['carState'] and not sm.updated['cameraOdometry']:
- calibrator.send_data(pm)
- continue
-
- if sm.updated['carState']:
- calibrator.handle_v_ego(sm['carState'].vEgo)
- if send_counter % 25 == 0:
- calibrator.send_data(pm)
- send_counter += 1
+ timeout = 0 if sm.frame == -1 else 100
+ sm.update(timeout)
if sm.updated['cameraOdometry']:
- new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
- sm['cameraOdometry'].rot,
- sm['cameraOdometry'].transStd,
- sm['cameraOdometry'].rotStd)
+ calibrator.handle_v_ego(sm['carState'].vEgo)
+ new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
+ sm['cameraOdometry'].rot,
+ sm['cameraOdometry'].transStd,
+ sm['cameraOdometry'].rotStd)
- if DEBUG and new_vp is not None:
- print('got new vp', new_vp)
+ if DEBUG and new_rpy is not None:
+ print('got new rpy', new_rpy)
+
+ # 4Hz driven by cameraOdometry
+ if sm.frame % 5 == 0:
+ calibrator.send_data(pm)
def main(sm=None, pm=None):
diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index ad820b4ea..1d1d35331 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -13,8 +13,6 @@ from selfdrive.swaglog import cloudlog
KalmanStatus = log.LiveLocationKalman.Status
-CARSTATE_DECIMATION = 5
-
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
@@ -32,7 +30,6 @@ class ParamsLearner:
self.speed = 0
self.steering_pressed = False
self.steering_angle = 0
- self.carstate_counter = 0
self.valid = True
@@ -51,18 +48,16 @@ class ParamsLearner:
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
elif which == 'carState':
- self.carstate_counter += 1
- if self.carstate_counter % CARSTATE_DECIMATION == 0:
- self.steering_angle = msg.steeringAngle
- self.steering_pressed = msg.steeringPressed
- self.speed = msg.vEgo
+ self.steering_angle = msg.steeringAngle
+ self.steering_pressed = msg.steeringPressed
+ self.speed = msg.vEgo
- in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
- self.active = self.speed > 5 and in_linear_region
+ in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
+ self.active = self.speed > 5 and in_linear_region
- if self.active:
- self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
- self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
+ if self.active:
+ self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
+ self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow
@@ -72,7 +67,7 @@ class ParamsLearner:
def main(sm=None, pm=None):
if sm is None:
- sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
+ sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
@@ -111,12 +106,11 @@ def main(sm=None, pm=None):
sm.update()
for which, updated in sm.updated.items():
- if not updated:
- continue
- t = sm.logMonoTime[which] * 1e-9
- learner.handle_log(t, which, sm[which])
+ if updated:
+ t = sm.logMonoTime[which] * 1e-9
+ learner.handle_log(t, which, sm[which])
- if sm.updated['carState'] and (learner.carstate_counter % CARSTATE_DECIMATION == 0):
+ if sm.updated['liveLocationKalman']:
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
@@ -135,7 +129,7 @@ def main(sm=None, pm=None):
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
- if learner.carstate_counter % 6000 == 0: # once a minute
+ if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': msg.liveParameters.steerRatio,
diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc
index 0bc8c0e6f..ce4e4dcac 100644
--- a/selfdrive/locationd/ublox_msg.cc
+++ b/selfdrive/locationd/ublox_msg.cc
@@ -193,10 +193,8 @@ inline bool UbloxMsgParser::valid_so_far() {
kj::Array UbloxMsgParser::gen_solution() {
nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
- capnp::MallocMessageBuilder msg_builder;
- cereal::Event::Builder event = msg_builder.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto gpsLoc = event.initGpsLocationExternal();
+ MessageBuilder msg_builder;
+ auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal();
gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
gpsLoc.setFlags(msg->flags);
gpsLoc.setLatitude(msg->lat * 1e-07);
@@ -236,11 +234,8 @@ kj::Array UbloxMsgParser::gen_raw() {
return kj::Array();
}
rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)];
- capnp::MallocMessageBuilder msg_builder;
- cereal::Event::Builder event = msg_builder.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto gnss = event.initUbloxGnss();
- auto mr = gnss.initMeasurementReport();
+ MessageBuilder msg_builder;
+ auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport();
mr.setRcvTow(msg->rcvTow);
mr.setGpsWeek(msg->week);
mr.setLeapSeconds(msg->leapS);
@@ -295,11 +290,8 @@ kj::Array UbloxMsgParser::gen_nav_data() {
nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words;
if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) {
EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]);
- capnp::MallocMessageBuilder msg_builder;
- cereal::Event::Builder event = msg_builder.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto gnss = event.initUbloxGnss();
- auto eph = gnss.initEphemeris();
+ MessageBuilder msg_builder;
+ auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
eph.setSvId(ephem_data.svId);
eph.setToc(ephem_data.toc);
eph.setGpsWeek(ephem_data.gpsWeek);
@@ -343,11 +335,8 @@ kj::Array UbloxMsgParser::gen_nav_data() {
kj::Array UbloxMsgParser::gen_mon_hw() {
mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
- capnp::MallocMessageBuilder msg_builder;
- cereal::Event::Builder event = msg_builder.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto gnss = event.initUbloxGnss();
- auto hwStatus = gnss.initHwStatus();
+ MessageBuilder msg_builder;
+ auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
hwStatus.setNoisePerMS(msg->noisePerMS);
hwStatus.setAgcCnt(msg->agcCnt);
hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus);
diff --git a/selfdrive/locationd/ubloxd_test.cc b/selfdrive/locationd/ubloxd_test.cc
index 8653cedea..d1118e4d2 100644
--- a/selfdrive/locationd/ubloxd_test.cc
+++ b/selfdrive/locationd/ubloxd_test.cc
@@ -47,15 +47,11 @@ Message * poll_ubloxraw_msg(Poller * poller) {
size_t consuming = min(len - consumed, 128);
if(consumed < len) {
// create message
- capnp::MallocMessageBuilder msg_builder;
- cereal::Event::Builder event = msg_builder.initRoot();
- event.setLogMonoTime(nanos_since_boot());
-
- auto ublox_raw = event.initUbloxRaw(consuming);
+ MessageBuilder msg_builder;
+ auto ublox_raw = msg_builder.initEvent().initUbloxRaw(consuming);
memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
- auto words = capnp::messageToFlatArray(msg_builder);
- auto bytes = words.asBytes();
+ auto bytes = msg_builder.toBytes();
Message * msg = new ZMQMessage();
msg->init((char*)bytes.begin(), bytes.size());
diff --git a/selfdrive/logcatd/SConscript b/selfdrive/logcatd/SConscript
index 67ad67300..7f87cbadd 100644
--- a/selfdrive/logcatd/SConscript
+++ b/selfdrive/logcatd/SConscript
@@ -1,2 +1,6 @@
-Import('env', 'cereal', 'messaging')
-env.Program('logcatd.cc', LIBS=[cereal, messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj'])
+Import('env', 'cereal', 'messaging', 'arch')
+
+if arch == "aarch64":
+ env.Program('logcatd', 'logcatd_android.cc', LIBS=[cereal, messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj'])
+else:
+ env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, 'zmq', 'capnp', 'kj', 'systemd', 'json11'])
diff --git a/selfdrive/logcatd/logcatd.cc b/selfdrive/logcatd/logcatd_android.cc
similarity index 88%
rename from selfdrive/logcatd/logcatd.cc
rename to selfdrive/logcatd/logcatd_android.cc
index 181e29df1..9a1753b1d 100644
--- a/selfdrive/logcatd/logcatd.cc
+++ b/selfdrive/logcatd/logcatd_android.cc
@@ -40,10 +40,8 @@ int main() {
continue;
}
- capnp::MallocMessageBuilder msg;
- cereal::Event::Builder event = msg.initRoot();
- event.setLogMonoTime(nanos_since_boot());
- auto androidEntry = event.initAndroidLog();
+ MessageBuilder msg;
+ auto androidEntry = msg.initEvent().initAndroidLog();
androidEntry.setId(log_msg.id());
androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec);
androidEntry.setPriority(entry.priority);
diff --git a/selfdrive/logcatd/logcatd_systemd.cc b/selfdrive/logcatd/logcatd_systemd.cc
new file mode 100644
index 000000000..adc187914
--- /dev/null
+++ b/selfdrive/logcatd/logcatd_systemd.cc
@@ -0,0 +1,71 @@
+#include
+#include
+#include
+#include