diff --git a/.gitignore b/.gitignore
index 5fc18bb07..3e87dcc43 100644
--- a/.gitignore
+++ b/.gitignore
@@ -58,6 +58,7 @@ one
openpilot
notebooks
xx
+yy
hyperthneed
panda_jungle
provisioning
diff --git a/Jenkinsfile b/Jenkinsfile
index bcdb7c99f..d12c8e494 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -1,7 +1,7 @@
def phone(String ip, String step_label, String cmd) {
withCredentials([file(credentialsId: 'id_rsa', variable: 'key_file')]) {
def ssh_cmd = """
-ssh -tt -o StrictHostKeyChecking=no -i ${key_file} -p 8022 'comma@${ip}' /usr/bin/bash <<'EOF'
+ssh -tt -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' /usr/bin/bash <<'END'
set -e
@@ -29,7 +29,7 @@ cd ${env.TEST_DIR} || true
${cmd}
exit 0
-EOF"""
+END"""
sh script: ssh_cmd, label: step_label
}
@@ -57,38 +57,29 @@ pipeline {
}
stages {
-
- stage('Build release2') {
- agent {
- docker {
- image 'python:3.7.3'
- args '--user=root'
- }
- }
+ stage('build releases') {
when {
branch 'devel-staging'
}
- steps {
- phone_steps("eon-build", [
- ["build release2-staging & dashcam-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
- ])
- }
- }
- stage('Build release3') {
- agent {
- docker {
- image 'python:3.7.3'
- args '--user=root'
+ parallel {
+ stage('release2') {
+ agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
+ steps {
+ phone_steps("eon-build", [
+ ["build release2-staging & dashcam-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
+ ])
+ }
+ }
+
+ stage('release3') {
+ agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
+ steps {
+ phone_steps("tici", [
+ ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
+ ])
+ }
}
- }
- when {
- branch 'devel-staging'
- }
- steps {
- phone_steps("tici", [
- ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
- ])
}
}
@@ -105,43 +96,8 @@ pipeline {
}
stages {
-
- /*
- stage('PC tests') {
- agent {
- dockerfile {
- filename 'Dockerfile.openpilotci'
- args '--privileged --shm-size=1G --user=root'
- }
- }
- stages {
- stage('Build') {
- steps {
- sh 'scons -j$(nproc)'
- }
- }
- }
- post {
- always {
- // fix permissions since docker runs as another user
- sh "chmod -R 777 ."
- }
- }
- }
- */
-
stage('On-device Tests') {
- agent {
- docker {
- /*
- filename 'Dockerfile.ondevice_ci'
- args "--privileged -v /dev:/dev --shm-size=1G --user=root"
- */
- image 'python:3.7.3'
- args '--user=root'
- }
- }
-
+ agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
stages {
stage('parallel tests') {
parallel {
@@ -247,6 +203,15 @@ pipeline {
}
}
+ stage('C3: replay') {
+ steps {
+ phone_steps("tici-party", [
+ ["build", "cd selfdrive/manager && ./build.py"],
+ ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"],
+ ])
+ }
+ }
+
}
}
diff --git a/README.md b/README.md
index b9a58f6a1..b012883d7 100755
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-![](https://user-images.githubusercontent.com/37757984/127420744-89ca219c-8f8e-46d3-bccf-c1cb53b81bb1.png)
+![](https://i.imgur.com/b0ZyIx5.jpg)
Table of Contents
=======================
@@ -21,16 +21,16 @@ What is openpilot?
- |
- |
- |
- |
+ |
+ |
+ |
+ |
- |
- |
- |
- |
+ |
+ |
+ |
+ |
@@ -40,7 +40,7 @@ Running in a car
To use openpilot in a car, you need four things
* This software. It's free and available right here.
-* One of [the 140+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported, but has adaptive cruise control and lane keeping assist, it's likely able to run openpilot.
+* One of [the 150+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported, but has adaptive cruise control and lane keeping assist, it's likely able to run openpilot.
* A supported device to run this software. This can be a [comma two](https://comma.ai/shop/products/two), [comma three](https://comma.ai/shop/products/three), or if you like to experiment, a [Ubuntu computer with webcams](https://github.com/commaai/openpilot/tree/master/tools/webcam).
* A way to connect to your car. With a comma two or three, you need only a [car harness](https://comma.ai/shop/products/car-harness). With an EON Gold or PC, you also need a [black panda](https://comma.ai/shop/products/panda).
diff --git a/RELEASES.md b/RELEASES.md
index 12a90fba6..5d4f114b8 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,22 @@
+Version 0.8.12 (2021-12-15)
+========================
+ * New driving model
+ * Improved behavior around exits
+ * Better pose accuracy at high speeds, allowing max speed of 90mph
+ * Fully incorporated comma three data into all parts of training stack
+ * Improved follow distance
+ * Better longitudinal policy, especially in low speed traffic
+ * New alert sounds
+ * AGNOS 3
+ * Display burn in mitigation
+ * Improved audio amplifier configuration
+ * System reliability improvements
+ * Update Python to 3.8.10
+ * Raw logs upload moved to connect.comma.ai
+ * Fixed HUD alerts on newer Honda Bosch thanks to csouers!
+ * Audi Q3 2020-21 support thanks to jyoung8607!
+ * Lexus RC 2020 support thanks to ErichMoraga!
+
Version 0.8.11 (2021-11-29)
========================
* Support for CAN FD on the red panda
diff --git a/SConstruct b/SConstruct
index 67acd3eac..14450cec9 100644
--- a/SConstruct
+++ b/SConstruct
@@ -60,7 +60,6 @@ if arch == "aarch64" and TICI:
arch = "larch64"
USE_WEBCAM = os.getenv("USE_WEBCAM") is not None
-USE_FRAME_STREAM = os.getenv("USE_FRAME_STREAM") is not None
lenv = {
"PATH": os.environ['PATH'],
@@ -91,7 +90,6 @@ if arch == "aarch64" or arch == "larch64":
"/usr/lib",
"/system/vendor/lib64",
"/system/comma/usr/lib",
- "#third_party/nanovg",
f"#third_party/acados/{arch}/lib",
]
@@ -210,7 +208,6 @@ env = Environment(
"#third_party/linux/include",
"#third_party/snpe/include",
"#third_party/mapbox-gl-native-qt/include",
- "#third_party/nanovg",
"#third_party/qrcode",
"#third_party",
"#cereal",
@@ -352,7 +349,7 @@ if GetOption("clazy"):
qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
-Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM', 'USE_FRAME_STREAM')
+Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM')
SConscript(['selfdrive/common/SConscript'])
Import('_common', '_gpucommon', '_gpu_libs')
@@ -387,7 +384,7 @@ rednose_config = {
},
}
-if arch != "aarch64":
+if arch not in ["aarch64", "larch64"]:
rednose_config['to_build'].update({
'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
diff --git a/cereal/car.capnp b/cereal/car.capnp
index ce81c7eaf..5c8c339b7 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -351,14 +351,17 @@ struct CarControl {
enum AudibleAlert {
none @0;
- chimeEngage @1;
- chimeDisengage @2;
- chimeError @3;
- chimeWarning1 @4; # unused
- chimeWarningRepeat @5;
- chimeWarningRepeatInfinite @6;
- chimePrompt @7;
- chimeWarning2RepeatInfinite @8;
+
+ engage @1;
+ disengage @2;
+ refuse @3;
+
+ warningSoft @4;
+ warningImmediate @5;
+
+ prompt @6;
+ promptRepeat @7;
+ promptDistracted @8;
}
}
@@ -379,6 +382,7 @@ struct CarParams {
enableDsu @5 :Bool; # driving support unit
enableApgs @6 :Bool; # advanced parking guidance system
enableBsm @56 :Bool; # blind spot monitoring
+ flags @64 :UInt32; # flags for car specific quirks
minEnableSpeed @7 :Float32;
minSteerSpeed @8 :Float32;
@@ -442,6 +446,8 @@ struct CarParams {
fingerprintSource @49: FingerprintSource;
networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network
+ wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds
+
struct SafetyConfig {
safetyModel @0 :SafetyModel;
safetyParam @1 :Int16;
@@ -520,7 +526,7 @@ struct CarParams {
allOutput @17;
gmAscm @18;
noOutput @19; # like silent but without silent CAN TXs
- hondaBoschHarness @20;
+ hondaBosch @20;
volkswagenPq @21;
subaruLegacy @22; # pre-Global platform
hyundaiLegacy @23;
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 92ec6429d..473a9f6b8 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -611,6 +611,8 @@ struct ControlsState @0x97ff69c53601abf1 {
delta @8 :Float32;
output @9 :Float32;
saturated @10 :Bool;
+ steeringAngleDesiredDeg @11 :Float32;
+ steeringRateDesiredDeg @12 :Float32;
}
struct LateralPIDState {
@@ -623,6 +625,7 @@ struct ControlsState @0x97ff69c53601abf1 {
f @6 :Float32;
output @7 :Float32;
saturated @8 :Bool;
+ steeringAngleDesiredDeg @9 :Float32;
}
struct LateralLQRState {
@@ -632,6 +635,7 @@ struct ControlsState @0x97ff69c53601abf1 {
output @3 :Float32;
lqrOutput @4 :Float32;
saturated @5 :Bool;
+ steeringAngleDesiredDeg @6 :Float32;
}
struct LateralAngleState {
@@ -639,6 +643,7 @@ struct ControlsState @0x97ff69c53601abf1 {
steeringAngleDeg @1 :Float32;
output @2 :Float32;
saturated @3 :Bool;
+ steeringAngleDesiredDeg @4 :Float32;
}
struct LateralDebugState {
@@ -1480,6 +1485,7 @@ struct Event {
# navigation
navInstruction @82 :NavInstruction;
navRoute @83 :NavRoute;
+ navThumbnail @84: Thumbnail;
# *********** debug ***********
testJoystick @52 :Joystick;
diff --git a/cereal/services.py b/cereal/services.py
index 7b9287780..89f437359 100755
--- a/cereal/services.py
+++ b/cereal/services.py
@@ -64,6 +64,7 @@ services = {
"uploaderState": (True, 0., 1),
"navInstruction": (True, 0.),
"navRoute": (True, 0.),
+ "navThumbnail": (True, 0.),
# debug
"testJoystick": (False, 0.),
diff --git a/cereal/visionipc/visionbuf.h b/cereal/visionipc/visionbuf.h
index 418cbacb9..b27a08b26 100644
--- a/cereal/visionipc/visionbuf.h
+++ b/cereal/visionipc/visionbuf.h
@@ -15,9 +15,9 @@ enum VisionStreamType {
VISION_STREAM_RGB_BACK,
VISION_STREAM_RGB_FRONT,
VISION_STREAM_RGB_WIDE,
- VISION_STREAM_YUV_BACK,
- VISION_STREAM_YUV_FRONT,
- VISION_STREAM_YUV_WIDE,
+ VISION_STREAM_ROAD,
+ VISION_STREAM_DRIVER,
+ VISION_STREAM_WIDE_ROAD,
VISION_STREAM_RGB_MAP,
VISION_STREAM_MAX,
};
diff --git a/cereal/visionipc/visionipc_client.cc b/cereal/visionipc/visionipc_client.cc
index da406067a..ece239356 100644
--- a/cereal/visionipc/visionipc_client.cc
+++ b/cereal/visionipc/visionipc_client.cc
@@ -55,7 +55,7 @@ bool VisionIpcClient::connect(bool blocking){
VisionBuf bufs[VISIONIPC_MAX_FDS];
r = ipc_sendrecv_with_fds(false, socket_fd, &bufs, sizeof(bufs), fds, VISIONIPC_MAX_FDS, &num_buffers);
- assert(num_buffers > 0);
+ assert(num_buffers >= 0);
assert(r == sizeof(VisionBuf) * num_buffers);
// Import buffers
diff --git a/cereal/visionipc/visionipc_pyx.pyx b/cereal/visionipc/visionipc_pyx.pyx
index c0ba4c3ee..3168f1e66 100644
--- a/cereal/visionipc/visionipc_pyx.pyx
+++ b/cereal/visionipc/visionipc_pyx.pyx
@@ -19,9 +19,9 @@ cpdef enum VisionStreamType:
VISION_STREAM_RGB_BACK
VISION_STREAM_RGB_FRONT
VISION_STREAM_RGB_WIDE
- VISION_STREAM_YUV_BACK
- VISION_STREAM_YUV_FRONT
- VISION_STREAM_YUV_WIDE
+ VISION_STREAM_ROAD
+ VISION_STREAM_DRIVER
+ VISION_STREAM_WIDE_ROAD
cdef class VisionIpcServer:
diff --git a/cereal/visionipc/visionipc_tests.cc b/cereal/visionipc/visionipc_tests.cc
index a68bda0b8..4d1df05f5 100644
--- a/cereal/visionipc/visionipc_tests.cc
+++ b/cereal/visionipc/visionipc_tests.cc
@@ -13,10 +13,10 @@ static void zmq_sleep(int milliseconds=1000){
TEST_CASE("Connecting"){
VisionIpcServer server("camerad");
- server.create_buffers(VISION_STREAM_YUV_BACK, 1, false, 100, 100);
+ server.create_buffers(VISION_STREAM_ROAD, 1, false, 100, 100);
server.start_listener();
- VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false);
+ VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
REQUIRE(client.connected);
@@ -25,10 +25,10 @@ TEST_CASE("Connecting"){
TEST_CASE("Check buffers"){
size_t width = 100, height = 200, num_buffers = 5;
VisionIpcServer server("camerad");
- server.create_buffers(VISION_STREAM_YUV_BACK, num_buffers, false, width, height);
+ server.create_buffers(VISION_STREAM_ROAD, num_buffers, false, width, height);
server.start_listener();
- VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false);
+ VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
REQUIRE(client.buffers[0].width == width);
@@ -39,11 +39,11 @@ TEST_CASE("Check buffers"){
TEST_CASE("Check yuv/rgb"){
VisionIpcServer server("camerad");
- server.create_buffers(VISION_STREAM_YUV_BACK, 1, false, 100, 100);
+ server.create_buffers(VISION_STREAM_ROAD, 1, false, 100, 100);
server.create_buffers(VISION_STREAM_RGB_BACK, 1, true, 100, 100);
server.start_listener();
- VisionIpcClient client_yuv = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false);
+ VisionIpcClient client_yuv = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
VisionIpcClient client_rgb = VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, false);
client_yuv.connect();
client_rgb.connect();
@@ -54,14 +54,14 @@ TEST_CASE("Check yuv/rgb"){
TEST_CASE("Send single buffer"){
VisionIpcServer server("camerad");
- server.create_buffers(VISION_STREAM_YUV_BACK, 1, true, 100, 100);
+ server.create_buffers(VISION_STREAM_ROAD, 1, true, 100, 100);
server.start_listener();
- VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false);
+ VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
zmq_sleep();
- VisionBuf * buf = server.get_buffer(VISION_STREAM_YUV_BACK);
+ VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
REQUIRE(buf != nullptr);
*((uint64_t*)buf->addr) = 1234;
@@ -83,14 +83,14 @@ TEST_CASE("Send single buffer"){
TEST_CASE("Test no conflate"){
VisionIpcServer server("camerad");
- server.create_buffers(VISION_STREAM_YUV_BACK, 1, true, 100, 100);
+ server.create_buffers(VISION_STREAM_ROAD, 1, true, 100, 100);
server.start_listener();
- VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false);
+ VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
zmq_sleep();
- VisionBuf * buf = server.get_buffer(VISION_STREAM_YUV_BACK);
+ VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
REQUIRE(buf != nullptr);
VisionIpcBufExtra extra = {0};
@@ -111,14 +111,14 @@ TEST_CASE("Test no conflate"){
TEST_CASE("Test conflate"){
VisionIpcServer server("camerad");
- server.create_buffers(VISION_STREAM_YUV_BACK, 1, true, 100, 100);
+ server.create_buffers(VISION_STREAM_ROAD, 1, true, 100, 100);
server.start_listener();
- VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true);
+ VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, true);
REQUIRE(client.connect());
zmq_sleep();
- VisionBuf * buf = server.get_buffer(VISION_STREAM_YUV_BACK);
+ VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
REQUIRE(buf != nullptr);
VisionIpcBufExtra extra = {0};
diff --git a/common/api/__init__.py b/common/api/__init__.py
index 5b1d66cf7..8b83dfc64 100644
--- a/common/api/__init__.py
+++ b/common/api/__init__.py
@@ -3,7 +3,7 @@ import os
import requests
from datetime import datetime, timedelta
from common.basedir import PERSIST
-from selfdrive.version import version
+from selfdrive.version import get_version
API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
@@ -34,13 +34,13 @@ class Api():
if isinstance(token, bytes):
token = token.decode('utf8')
return token
-
+
def api_get(endpoint, method='GET', timeout=None, access_token=None, **params):
headers = {}
if access_token is not None:
- headers['Authorization'] = "JWT "+access_token
+ headers['Authorization'] = "JWT " + access_token
- headers['User-Agent'] = "openpilot-" + version
+ headers['User-Agent'] = "openpilot-" + get_version()
return requests.request(method, API_HOST + "/" + endpoint, timeout=timeout, headers=headers, params=params)
diff --git a/common/cython_hacks.py b/common/cython_hacks.py
deleted file mode 100644
index d0e154746..000000000
--- a/common/cython_hacks.py
+++ /dev/null
@@ -1,23 +0,0 @@
-import os
-import sysconfig
-from Cython.Distutils import build_ext
-
-def get_ext_filename_without_platform_suffix(filename):
- name, ext = os.path.splitext(filename)
- ext_suffix = sysconfig.get_config_var('EXT_SUFFIX')
-
- if ext_suffix == ext:
- return filename
-
- ext_suffix = ext_suffix.replace(ext, '')
- idx = name.find(ext_suffix)
-
- if idx == -1:
- return filename
- else:
- return name[:idx] + ext
-
-class BuildExtWithoutPlatformSuffix(build_ext):
- def get_ext_filename(self, ext_name):
- filename = super().get_ext_filename(ext_name)
- return get_ext_filename_without_platform_suffix(filename)
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 1215909fc..5d6458fad 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -84,6 +84,12 @@ bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
bigmodel_frame_from_calib_frame = np.dot(bigmodel_intrinsics,
get_view_frame_from_calib_frame(0, 0, 0, 0))
+sbigmodel_frame_from_road_frame = np.dot(sbigmodel_intrinsics,
+ get_view_frame_from_road_frame(0, 0, 0, model_height))
+
+sbigmodel_frame_from_calib_frame = np.dot(sbigmodel_intrinsics,
+ get_view_frame_from_calib_frame(0, 0, 0, 0))
+
medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics,
get_view_frame_from_road_frame(0, 0, 0, model_height))
diff --git a/docs/CARS.md b/docs/CARS.md
index 30f3131c7..9c762ee7b 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -7,8 +7,8 @@
| Acura | ILX 2016-19 | AcuraWatch Plus | openpilot | 25mph1 | 25mph |
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 12mph |
| Acura | RDX 2019-21 | All | Stock | 0mph | 3mph |
-| Honda | Accord 2018-20 | All | Stock | 0mph | 3mph |
-| Honda | Accord Hybrid 2018-20 | All | Stock | 0mph | 3mph |
+| Honda | Accord 2018-21 | All | Stock | 0mph | 3mph |
+| Honda | Accord Hybrid 2018-21 | All | Stock | 0mph | 3mph |
| Honda | Civic Hatchback 2017-21 | Honda Sensing | Stock | 0mph | 12mph |
| Honda | Civic Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph |
| Honda | Civic Coupe 2019-20 | All | Stock | 0mph | 2mph2 |
@@ -34,9 +34,10 @@
| Lexus | ES Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph |
| Lexus | ES Hybrid 2019-21 | All | openpilot | 0mph | 0mph |
| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph |
-| Lexus | NX 2018 | All | Stock3| 0mph | 0mph |
+| Lexus | NX 2018-2019 | All | Stock3| 0mph | 0mph |
| Lexus | NX 2020 | All | openpilot | 0mph | 0mph |
| Lexus | NX Hybrid 2018-19 | All | Stock3| 0mph | 0mph |
+| Lexus | RC 2020 | All | Stock | 22mph | 0mph |
| Lexus | RX 2016-18 | All | Stock3| 0mph | 0mph |
| Lexus | RX 2020-21 | All | openpilot | 0mph | 0mph |
| Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph |
@@ -49,7 +50,7 @@
| Toyota | Camry 2021-22 | All | openpilot | 0mph4 | 0mph |
| Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph4 | 0mph |
| Toyota | Camry Hybrid 2021-22 | All | openpilot | 0mph | 0mph |
-| Toyota | C-HR 2017-20 | All | Stock | 0mph | 0mph |
+| Toyota | C-HR 2017-21 | All | Stock | 0mph | 0mph |
| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph |
| Toyota | Corolla 2017-19 | All | Stock3| 20mph1 | 0mph |
| Toyota | Corolla 2020-22 | All | openpilot | 0mph | 0mph |
@@ -82,6 +83,7 @@
| Audi | A3 2014-19 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | A3 Sportback e-tron 2017-18 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | Q2 2018 | ACC + Lane Assist | Stock | 0mph | 0mph |
+| Audi | Q3 2020-21 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Audi | S3 2015 | ACC + Lane Assist | Stock | 0mph | 0mph |
| Buick | Regal 20181 | Adaptive Cruise | openpilot | 0mph | 7mph |
| Cadillac | ATS 20181 | Adaptive Cruise | openpilot | 0mph | 7mph |
@@ -120,7 +122,7 @@
| Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph |
| Kia | Ceed 2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Forte 2018-21 | SCC + LKAS | Stock | 0mph | 0mph |
-| Kia | K5 2021 | SCC + LFA | Stock | 0mph | 0mph |
+| Kia | K5 2021-22 | SCC + LFA | Stock | 0mph | 0mph |
| Kia | Niro EV 2019-21 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Niro Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Niro PHEV 2019 | SCC + LKAS | Stock | 10mph | 32mph |
diff --git a/docs/CONTRIBUTING.md b/docs/CONTRIBUTING.md
index 8dcdeee50..b9da6c576 100644
--- a/docs/CONTRIBUTING.md
+++ b/docs/CONTRIBUTING.md
@@ -1,16 +1,35 @@
# How to contribute
-Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use.
+Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use. Check out our [post about externalization](https://blog.comma.ai/a-2020-theme-externalization/).
-Most open source development activity is coordinated through our [GitHub Discussions](https://github.com/commaai/openpilot/discussions) and [Discord](https://discord.comma.ai). A lot of documentation is available on our [blog](https://blog.comma.ai/).
+Most open source development activity is coordinated through our [GitHub Discussions](https://github.com/commaai/openpilot/discussions) and [Discord](https://discord.comma.ai). A lot of documentation is available at https://docs.comma.ai and on our [blog](https://blog.comma.ai/).
-## Getting Started
+### Getting Started
* Setup your [development environment](../tools/)
* Join our [Discord](https://discord.comma.ai)
* Make sure you have a [GitHub account](https://github.com/signup/free)
* Fork [our repositories](https://github.com/commaai) on GitHub
+## Pull Requests
+
+Pull requests should be against the master branch. Welcomed contributions include bug reports, car ports, and any [open issue](https://github.com/commaai/openpilot/issues). If you're unsure about a contribution, feel free to open a discussion, issue, or draft PR to discuss the problem you're trying to solve.
+
+A good pull request has all of the following:
+* a clearly stated purpose
+* every line changed directly contributes to the stated purpose
+* verification, i.e. how did you test your PR?
+* justification
+ * if you've optimized something, post benchmarks to prove it's better
+ * if you've improved your car's tuning, post before and after plots
+* passes the CI tests
+
+### Car Ports
+
+We've released a [Model Port guide](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) for porting to Toyota/Lexus models.
+
+If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/).
+
## Testing
### Automated Testing
@@ -20,27 +39,3 @@ All PRs and commits are automatically checked by GitHub Actions. Check out `.git
### Code Style and Linting
Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`.
-
-## Car Ports
-
-We've released a [Model Port guide](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) for porting to Toyota/Lexus models.
-
-If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/).
-
-## Pull Requests
-
-Pull requests should be against the master branch. Before running master on in-car hardware, you'll need to clone the submodules too. That can be done by recursively cloning the repository:
-```
-git clone https://github.com/commaai/openpilot.git --recursive
-```
-Or alternatively, when on the master branch:
-```
-git submodule update --init
-```
-The reasons for having submodules on a dedicated repository and our new development philosophy can be found in our [post about externalization](https://blog.comma.ai/a-2020-theme-externalization/).
-Modules that are in seperate repositories include:
-* cereal
-* laika
-* opendbc
-* panda
-* rednose
diff --git a/launch_env.sh b/launch_env.sh
index 47d98cc92..cb0a0572d 100755
--- a/launch_env.sh
+++ b/launch_env.sh
@@ -11,7 +11,7 @@ if [ -z "$REQUIRED_NEOS_VERSION" ]; then
fi
if [ -z "$AGNOS_VERSION" ]; then
- export AGNOS_VERSION="2"
+ export AGNOS_VERSION="3"
fi
if [ -z "$PASSIVE" ]; then
diff --git a/models/supercombo.dlc b/models/supercombo.dlc
index 3a4657590..02d0a56f8 100644
Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ
diff --git a/opendbc/acura_ilx_2016_can_generated.dbc b/opendbc/acura_ilx_2016_can_generated.dbc
index 3ad7520cb..065fe7d5a 100644
--- a/opendbc/acura_ilx_2016_can_generated.dbc
+++ b/opendbc/acura_ilx_2016_can_generated.dbc
@@ -323,4 +323,3 @@ VAL_ 506 FCW 3 "fcw" 2 "fcw" 1 "fcw" 0 "no_fcw" ;
VAL_ 780 HUD_LEAD 3 "no_car" 2 "solid_car" 1 "dashed_car" 0 "no_car" ;
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/acura_rdx_2018_can_generated.dbc b/opendbc/acura_rdx_2018_can_generated.dbc
index 48dbddd80..2acbe3fd7 100644
--- a/opendbc/acura_rdx_2018_can_generated.dbc
+++ b/opendbc/acura_rdx_2018_can_generated.dbc
@@ -310,4 +310,3 @@ VAL_ 392 GEAR 26 "S" 4 "D" 3 "N" 2 "R" 1 "P" ;
VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_speed_lockout" 2 "no_torque_alert_1" 0 "normal" ;
VAL_ 422 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/acura_rdx_2020_can_generated.dbc b/opendbc/acura_rdx_2020_can_generated.dbc
index a114e4e13..3fadba455 100644
--- a/opendbc/acura_rdx_2020_can_generated.dbc
+++ b/opendbc/acura_rdx_2020_can_generated.dbc
@@ -432,4 +432,3 @@ VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_accord_2018_can_generated.dbc b/opendbc/honda_accord_2018_can_generated.dbc
index 1c78b38b6..0b79ceef4 100644
--- a/opendbc/honda_accord_2018_can_generated.dbc
+++ b/opendbc/honda_accord_2018_can_generated.dbc
@@ -489,4 +489,3 @@ VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
index fe9a13af8..cb2a3592a 100644
--- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
+++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
@@ -490,4 +490,3 @@ VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc
index 8e7199cc9..0ddb0bd40 100644
--- a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc
+++ b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc
@@ -485,4 +485,3 @@ VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc
index 56168a6fb..9315c822a 100644
--- a/opendbc/honda_civic_touring_2016_can_generated.dbc
+++ b/opendbc/honda_civic_touring_2016_can_generated.dbc
@@ -394,4 +394,3 @@ VAL_ 884 DASHBOARD_ALERT 0 "none" 51 "acc_problem" 55 "cmbs_problem" 75 "key_not
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 927 ACC_ALERTS 29 "esp_active_acc_canceled" 10 "b_pedal_applied" 9 "speed_too_low" 8 "speed_too_high" 7 "p_brake_applied" 6 "gear_no_d" 5 "seatbelt" 4 "too_steep_downhill" 3 "too_steep_uphill" 2 "too_close" 1 "no_vehicle_ahead" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc
index cef50babc..3be8848bd 100644
--- a/opendbc/honda_crv_ex_2017_can_generated.dbc
+++ b/opendbc/honda_crv_ex_2017_can_generated.dbc
@@ -496,4 +496,3 @@ VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_crv_executive_2016_can_generated.dbc b/opendbc/honda_crv_executive_2016_can_generated.dbc
index d4249ea02..55611dd9e 100644
--- a/opendbc/honda_crv_executive_2016_can_generated.dbc
+++ b/opendbc/honda_crv_executive_2016_can_generated.dbc
@@ -316,4 +316,3 @@ VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ;
VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_crv_hybrid_2019_can_generated.dbc b/opendbc/honda_crv_hybrid_2019_can_generated.dbc
index 4a6ad4b82..01a24c96c 100644
--- a/opendbc/honda_crv_hybrid_2019_can_generated.dbc
+++ b/opendbc/honda_crv_hybrid_2019_can_generated.dbc
@@ -478,4 +478,3 @@ VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_crv_touring_2016_can_generated.dbc b/opendbc/honda_crv_touring_2016_can_generated.dbc
index 35a3da361..45d85cad7 100644
--- a/opendbc/honda_crv_touring_2016_can_generated.dbc
+++ b/opendbc/honda_crv_touring_2016_can_generated.dbc
@@ -320,4 +320,3 @@ VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ;
VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_fit_ex_2018_can_generated.dbc b/opendbc/honda_fit_ex_2018_can_generated.dbc
index 0459cf5b8..df47a001d 100644
--- a/opendbc/honda_fit_ex_2018_can_generated.dbc
+++ b/opendbc/honda_fit_ex_2018_can_generated.dbc
@@ -345,4 +345,3 @@ VAL_ 422 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "c
VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ;
VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_insight_ex_2019_can_generated.dbc b/opendbc/honda_insight_ex_2019_can_generated.dbc
index 024f68d3c..5bb0ab01c 100644
--- a/opendbc/honda_insight_ex_2019_can_generated.dbc
+++ b/opendbc/honda_insight_ex_2019_can_generated.dbc
@@ -474,4 +474,3 @@ BO_ 1029 DOORS_STATUS: 8 BDY
VAL_ 419 GEAR 10 "R" 1 "D" 0 "P";
VAL_ 419 GEAR_SHIFTER 32 "D" 16 "N" 8 "R" 4 "P" ;
-CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_odyssey_exl_2018_generated.dbc b/opendbc/honda_odyssey_exl_2018_generated.dbc
index a27fc5be4..586d2f8ab 100644
--- a/opendbc/honda_odyssey_exl_2018_generated.dbc
+++ b/opendbc/honda_odyssey_exl_2018_generated.dbc
@@ -357,4 +357,3 @@ VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
VAL_ 927 ACC_ALERTS 29 "esp_active_acc_canceled" 10 "b_pedal_applied" 9 "speed_too_low" 8 "speed_too_high" 7 "p_brake_applied" 6 "gear_no_d" 5 "seatbelt" 4 "too_steep_downhill" 3 "too_steep_uphill" 2 "too_close" 1 "no_vehicle_ahead" ;
VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_pilot_touring_2017_can_generated.dbc b/opendbc/honda_pilot_touring_2017_can_generated.dbc
index e9217d3e4..2cc61de8b 100644
--- a/opendbc/honda_pilot_touring_2017_can_generated.dbc
+++ b/opendbc/honda_pilot_touring_2017_can_generated.dbc
@@ -313,4 +313,3 @@ VAL_ 422 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "c
VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ;
VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc
index 02989aabc..19ded5414 100644
--- a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc
+++ b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc
@@ -308,4 +308,3 @@ VAL_ 422 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "c
VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ;
VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
-CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc
index 883f2ee0a..06fcd3dc1 100644
--- a/opendbc/lexus_ct200h_2018_pt_generated.dbc
+++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc
index 30e8cbbaf..571dfee5d 100644
--- a/opendbc/lexus_is_2018_pt_generated.dbc
+++ b/opendbc/lexus_is_2018_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/lexus_nx300_2018_pt_generated.dbc b/opendbc/lexus_nx300_2018_pt_generated.dbc
index 7b4fcaa13..5351a4f49 100644
--- a/opendbc/lexus_nx300_2018_pt_generated.dbc
+++ b/opendbc/lexus_nx300_2018_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc
index c7cb4616d..0ec03a45c 100644
--- a/opendbc/lexus_nx300h_2018_pt_generated.dbc
+++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc
index 7a1b1a704..e4ea2289b 100644
--- a/opendbc/lexus_rx_350_2016_pt_generated.dbc
+++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
index b481318a1..fd9f217f2 100644
--- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
+++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc
index 57a63e128..cfa86aac1 100644
--- a/opendbc/tesla_can.dbc
+++ b/opendbc/tesla_can.dbc
@@ -748,4 +748,3 @@ VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RES
VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ;
-CM_ "CHFFR_METRIC 1160 DAS_steeringAngleRequest STEER_ANGLE 0.1098666 180; CHFFR_METRIC 264 DI_motorRPM ENGINE_RPM 1 0";
diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc
index 8043d75ae..901616d58 100644
--- a/opendbc/toyota_avalon_2017_pt_generated.dbc
+++ b/opendbc/toyota_avalon_2017_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
index 72ef797a3..55ebbb6a4 100644
--- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc
index 9ff7b5852..5fa0e8399 100644
--- a/opendbc/toyota_corolla_2017_pt_generated.dbc
+++ b/opendbc/toyota_corolla_2017_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc
index 7c279f0da..6745ef9a0 100644
--- a/opendbc/toyota_highlander_2017_pt_generated.dbc
+++ b/opendbc/toyota_highlander_2017_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
index a2b5da03a..c8870edfd 100644
--- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
index a6d4dc6d7..8102d48cc 100644
--- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc
index c84fa415f..58ca9c51a 100644
--- a/opendbc/toyota_nodsu_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc
index af9603c7d..c3857583c 100644
--- a/opendbc/toyota_prius_2017_pt_generated.dbc
+++ b/opendbc/toyota_prius_2017_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc
index 41021cce3..f70b86982 100644
--- a/opendbc/toyota_rav4_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_2017_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
index 9dba21a70..e6345fa8c 100644
--- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
index 05e282b07..9141a34b0 100644
--- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
+++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
@@ -370,7 +370,6 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
-CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
CM_ "Imported file _comma.dbc starts here";
diff --git a/opendbc/vw_mqb_2010.dbc b/opendbc/vw_mqb_2010.dbc
index 3a032c056..2302035d8 100644
--- a/opendbc/vw_mqb_2010.dbc
+++ b/opendbc/vw_mqb_2010.dbc
@@ -33,7 +33,7 @@ NS_ :
BS_:
-BU_: Airbag_MQB BAP_Tester_MQB BMS_MQB Datenlogger_MQB Gateway_MQB Getriebe_DQ_Hybrid_MQB Getriebe_DQ_MQB LEH_MQB Motor_Diesel_MQB Motor_Hybrid_MQB Motor_Otto_MQB SAK_MQB Waehlhebel_MQB Vector__XXX 9 l c i XXX
+BU_: Airbag_MQB BAP_Tester_MQB BMS_MQB Datenlogger_MQB Gateway_MQB Getriebe_DQ_Hybrid_MQB Getriebe_DQ_MQB LEH_MQB Motor_Diesel_MQB Motor_Hybrid_MQB Motor_Otto_MQB SAK_MQB Waehlhebel_MQB Vector__XXX l c i XXX
BO_ 290 ACC_06: 8 Gateway_MQB
@@ -1253,19 +1253,17 @@ BO_ 780 ACC_02: 8 XXX
SG_ ACC_Status_Anzeige : 61|3@1+ (1.0,0.0) [0.0|7] "" XXX
BO_ 302 ACC_07: 8 XXX
- SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
- SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
- SG_ XXX_Maybe_Engine_Start_Request : 12|2@1+ (1,0) [0|1] "" XXX
- SG_ XXX_Always_1 : 14|1@1+ (1,0) [0|1] "" XXX
- SG_ XXX_Maybe_Engine_Stop_Release : 15|1@1+ (1,0) [0|1] "" XXX
- SG_ XXX_Unknown : 16|8@1+ (1,0) [0|255] "" XXX
- SG_ ACC_Engaged : 27|1@1+ (1,0) [0|1] "Boolean" XXX
- SG_ ACC_Anhalten : 28|1@1+ (1,0) [0|1] "Boolean" XXX
- SG_ ACC_Anhaltevorgang : 29|1@1+ (1,0) [0|1] "Boolean" XXX
- SG_ ACC_Anfahrvorgang : 30|1@1+ (1,0) [0|1] "Boolean" XXX
- SG_ ACC_Anfahren : 31|1@1+ (1,0) [0|1] "Boolean" XXX
- SG_ XXX_Lead_Car_Relative_Speed : 32|8@1+ (1,-153) [0|255] "" XXX
- SG_ ACC_Sollbeschleunigung_01 : 53|11@1+ (0.005,-7.22) [-7.22|3.005] "Unit_MeterPerSeconSquar" XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ ACC_Distance_to_Stop : 12|11@1+ (0.01,0) [0|1] "m" XXX
+ SG_ ACC_Hold_Request : 23|1@1+ (1,0) [0|1] "x" XXX
+ SG_ ACC_Boost_Request : 24|1@1+ (1,0) [0|1] "" XXX
+ SG_ ACC_Freewheel_Request : 25|1@1+ (1,0) [0|1] "" XXX
+ SG_ ACC_Freewheel_Type : 26|2@1+ (1,0) [0|3] "enum" XXX
+ SG_ ACC_Hold_Type : 28|3@1+ (1,0) [0|15] "enum" XXX
+ SG_ ACC_Hold_Release : 31|1@1+ (1,0) [0|1] "" XXX
+ SG_ ACC_Accel_Secondary : 32|8@1+ (0.03,-4.6) [-4.6|2.99] "m/s2" XXX
+ SG_ ACC_Accel_TSK : 53|11@1+ (0.005,-7.22) [-7.22|3.005] "m/s2" XXX
BO_ 264 Fahrwerk_01: 8 XXX
SG_ Fahrwerk_01_BZ : 8|4@1+ (1,0) [0|15] "" XXX
@@ -1371,25 +1369,24 @@ BO_ 391 EV_Gearshift: 8 XXX
SG_ RegenBrakingMode : 12|2@1+ (1,0) [0|3] "" XXX
CM_ SG_ 134 LWI_Lenkradwinkel "Steering angle WITH variable ratio effect included";
-CM_ SG_ 159 EPS_HCA_Status "Status of Heading Control Assist feature"
+CM_ SG_ 159 EPS_HCA_Status "Status of Heading Control Assist feature";
CM_ SG_ 159 EPS_Lenkmoment "Steering input by driver, torque";
CM_ SG_ 159 EPS_VZ_Lenkmoment "Steering input by driver, direction";
CM_ SG_ 159 EPS_Berechneter_LW "Raw steering angle, degrees";
-CM_ SG_ 159 EPS_VZ_BLW "Raw steering angle, direction"
+CM_ SG_ 159 EPS_VZ_BLW "Raw steering angle, direction";
CM_ SG_ 173 COUNTERXX "Message not renamed to COUNTER because J533 rate-limiting makes it look like messages are being lost";
-CM_ SG_ 294 3 "May be zero when sent by older cameras";
-CM_ SG_ 294 7 "May be zero when sent by older cameras";
-CM_ SG_ 294 254 "May be zero when sent by older cameras";
+CM_ SG_ 294 SET_ME_0X3 "May be zero when sent by older cameras";
+CM_ SG_ 294 SET_ME_0X07 "May be zero when sent by older cameras";
+CM_ SG_ 294 SET_ME_0XFE "May be zero when sent by older cameras";
CM_ SG_ 294 Assist_Torque "Heading control input, torque";
CM_ SG_ 294 Assist_VZ "Heading control input, direction (sign)";
CM_ SG_ 294 HCA_Available "Must be 1 for steering rack to accept HCA commands";
-CM_ SG_ 302 XXX_Unknown "Weird format but changes with some other bits, maybe a data-level checksum?";
-CM_ SG_ 302 ACC_Engaged "ACC engaged and regulating speed";
-CM_ SG_ 302 ACC_Sollbeschleunigung_01 "Requested acceleration, mirrors ACC_06.ACC_Sollbeschleunigung_02";
-CM_ SG_ 302 ACC_Anfahren "Start up, mirrors ACC_06.Anfahren";
-CM_ SG_ 302 ACC_Anhaltevorgang "Triggers/requests ESP standstill, reflected in ESP_Anhaltevorgang_ACC_aktiv";
-CM_ SG_ 302 ACC_Anfahrvorgang "Releases ESP hold";
-CM_ SG_ 302 ACC_Anhalten "Halt, mirrors ACC_06.Anhalten";
+CM_ SG_ 302 ACC_Hold_Request "Active request for ABS brake hold in ACC_Hold_Type";
+CM_ SG_ 302 ACC_Boost_Request "Hybrid engine start related";
+CM_ SG_ 302 ACC_Freewheel_Request "Active request for DSG sailing/coasting in ACC_Freewheel_Type";
+CM_ SG_ 302 ACC_Hold_Release "Request to ABS to release brake hold";
+CM_ SG_ 302 ACC_Accel_Secondary "Target acceleration of the secondary controller";
+CM_ SG_ 302 ACC_Accel_TSK "Mirror of request to TSK to implement a target acceleration";
CM_ SG_ 870 Hazard_Switch "Four-way flashers active";
CM_ SG_ 870 Comfort_Signal_Left "Comfort turn signal active, left";
CM_ SG_ 870 Comfort_Signal_Right "Comfort turn signal active, right";
@@ -1415,6 +1412,9 @@ CM_ SG_ 391 GearPosition "Traditional PRND plus B-mode aggressive regen, B-mode
CM_ SG_ 960 ZAS_Kl_15 "Indicates ignition on";
VAL_ 159 EPS_HCA_Status 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active";
VAL_ 173 GE_Fahrstufe 5 "P" 6 "R" 7 "N" 8 "D" 9 "S" 10 "E" 14 "T";
+VAL_ 288 TSK_Status 0 "init" 1 "disabled" 2 "enabled" 3 "regulating" 4 "accel_pedal_override" 5 "brake_only" 6 "temp_fault" 7 "perm_fault";
+VAL_ 302 ACC_Freewheel_Type 0 "freewheel_released" 1 "freewheel_not_permitted" 2 "freewheel_not_released" 3 "freewheel_requested" ;
+VAL_ 302 ACC_Hold_Type 0 "no_request" 1 "hold" 2 "park" 3 "hold_standby" 4 "startup" 5 "loosen_over_ramp" ;
VAL_ 391 GearPosition 2 "P" 3 "R" 4 "N" 5 "D" 6 "D";
VAL_ 391 RegenBrakingMode 0 "default" 1 "B1" 2 "B2" 3 "B3";
VAL_ 870 Fast_Send_Rate_Active 0 "1 Hz" 1 "50 Hz";
diff --git a/panda/board/can_definitions.h b/panda/board/can_definitions.h
index 552cf3ff9..074f4e7a6 100644
--- a/panda/board/can_definitions.h
+++ b/panda/board/can_definitions.h
@@ -25,5 +25,5 @@ typedef struct {
#define CANPACKET_HEAD_SIZE 5U
-#define WORD_TO_BYTE_ARRAY(dst8, src32) 0[dst8] = ((src32) & 0xFF); 1[dst8] = (((src32) >> 8) & 0xFF); 2[dst8] = (((src32) >> 16) & 0xFF); 3[dst8] = (((src32) >> 24) & 0xFF)
-#define BYTE_ARRAY_TO_WORD(dst32, src8) ((dst32) = 0[src8] | (1[src8] << 8) | (2[src8] << 16) | (3[src8] << 24))
+#define WORD_TO_BYTE_ARRAY(dst8, src32) 0[dst8] = ((src32) & 0xFFU); 1[dst8] = (((src32) >> 8U) & 0xFFU); 2[dst8] = (((src32) >> 16U) & 0xFFU); 3[dst8] = (((src32) >> 24U) & 0xFFU)
+#define BYTE_ARRAY_TO_WORD(dst32, src8) ((dst32) = 0[src8] | (1[src8] << 8U) | (2[src8] << 16U) | (3[src8] << 24U))
diff --git a/panda/board/drivers/bxcan.h b/panda/board/drivers/bxcan.h
index 1d1745cd3..4733491d0 100644
--- a/panda/board/drivers/bxcan.h
+++ b/panda/board/drivers/bxcan.h
@@ -112,7 +112,7 @@ void process_can(uint8_t can_number) {
to_push.returned = 1U;
to_push.rejected = 0U;
to_push.extended = (CAN->sTxMailBox[0].TIR >> 2) & 0x1U;
- to_push.addr = (to_push.extended != 0) ? (CAN->sTxMailBox[0].TIR >> 3) : (CAN->sTxMailBox[0].TIR >> 21);
+ to_push.addr = (to_push.extended != 0U) ? (CAN->sTxMailBox[0].TIR >> 3) : (CAN->sTxMailBox[0].TIR >> 21);
to_push.data_len_code = CAN->sTxMailBox[0].TDTR & 0xFU;
to_push.bus = bus_number;
WORD_TO_BYTE_ARRAY(&to_push.data[0], CAN->sTxMailBox[0].TDLR);
@@ -141,7 +141,7 @@ void process_can(uint8_t can_number) {
if (can_pop(can_queues[bus_number], &to_send)) {
can_tx_cnt += 1;
// only send if we have received a packet
- CAN->sTxMailBox[0].TIR = ((to_send.extended != 0) ? (to_send.addr << 3) : (to_send.addr << 21)) | (to_send.extended << 2);
+ CAN->sTxMailBox[0].TIR = ((to_send.extended != 0U) ? (to_send.addr << 3) : (to_send.addr << 21)) | (to_send.extended << 2);
CAN->sTxMailBox[0].TDTR = to_send.data_len_code;
BYTE_ARRAY_TO_WORD(CAN->sTxMailBox[0].TDLR, &to_send.data[0]);
BYTE_ARRAY_TO_WORD(CAN->sTxMailBox[0].TDHR, &to_send.data[4]);
@@ -173,7 +173,7 @@ void can_rx(uint8_t can_number) {
to_push.returned = 0U;
to_push.rejected = 0U;
to_push.extended = (CAN->sFIFOMailBox[0].RIR >> 2) & 0x1U;
- to_push.addr = (to_push.extended != 0) ? (CAN->sFIFOMailBox[0].RIR >> 3) : (CAN->sFIFOMailBox[0].RIR >> 21);
+ to_push.addr = (to_push.extended != 0U) ? (CAN->sFIFOMailBox[0].RIR >> 3) : (CAN->sFIFOMailBox[0].RIR >> 21);
to_push.data_len_code = CAN->sFIFOMailBox[0].RDTR & 0xFU;
to_push.bus = bus_number;
WORD_TO_BYTE_ARRAY(&to_push.data[0], CAN->sFIFOMailBox[0].RDLR);
diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h
index 9ee7415a1..5ec840568 100644
--- a/panda/board/drivers/can_common.h
+++ b/panda/board/drivers/can_common.h
@@ -206,18 +206,18 @@ void ignition_can_hook(CANPacket_t *to_push) {
// since the 0x1F1 signal can briefly go low immediately after ignition
if ((addr == 0x160) && (len == 5)) {
// this message isn't all zeros when ignition is on
- ignition_cadillac = GET_BYTES_04(to_push) != 0;
+ ignition_cadillac = GET_BYTES_04(to_push) != 0U;
}
if ((addr == 0x1F1) && (len == 8)) {
// Bit 5 is ignition "on"
- bool ignition_gm = ((GET_BYTE(to_push, 0) & 0x20) != 0);
+ bool ignition_gm = ((GET_BYTE(to_push, 0) & 0x20U) != 0U);
ignition_can = ignition_gm || ignition_cadillac;
}
// Tesla exception
if ((addr == 0x348) && (len == 8)) {
// GTW_status
- ignition_can = (GET_BYTE(to_push, 0) & 0x1) != 0;
+ ignition_can = (GET_BYTE(to_push, 0) & 0x1U) != 0U;
}
// Mazda exception
diff --git a/panda/board/drivers/fdcan.h b/panda/board/drivers/fdcan.h
index 601fc2d69..c5428d027 100644
--- a/panda/board/drivers/fdcan.h
+++ b/panda/board/drivers/fdcan.h
@@ -60,13 +60,13 @@ void process_can(uint8_t can_number) {
canfd_fifo *fifo;
fifo = (canfd_fifo *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE));
- fifo->header[0] = (to_send.extended << 30) | ((to_send.extended != 0) ? (to_send.addr) : (to_send.addr << 18));
+ fifo->header[0] = (to_send.extended << 30) | ((to_send.extended != 0U) ? (to_send.addr) : (to_send.addr << 18));
fifo->header[1] = (to_send.data_len_code << 16) | (bus_config[can_number].canfd_enabled << 21) | (bus_config[can_number].brs_enabled << 20);
uint8_t data_len_w = (dlc_to_len[to_send.data_len_code] / 4U);
- data_len_w += ((dlc_to_len[to_send.data_len_code] % 4) > 0) ? 1U : 0U;
+ data_len_w += ((dlc_to_len[to_send.data_len_code] % 4U) > 0U) ? 1U : 0U;
for (unsigned int i = 0; i < data_len_w; i++) {
- BYTE_ARRAY_TO_WORD(fifo->data_word[i], &to_send.data[i*4]);
+ BYTE_ARRAY_TO_WORD(fifo->data_word[i], &to_send.data[i*4U]);
}
CANx->TXBAR = (1UL << tx_index);
@@ -142,14 +142,14 @@ void can_rx(uint8_t can_number) {
to_push.returned = 0U;
to_push.rejected = 0U;
to_push.extended = (fifo->header[0] >> 30) & 0x1U;
- to_push.addr = ((to_push.extended != 0) ? (fifo->header[0] & 0x1FFFFFFFU) : ((fifo->header[0] >> 18) & 0x7FFU));
+ to_push.addr = ((to_push.extended != 0U) ? (fifo->header[0] & 0x1FFFFFFFU) : ((fifo->header[0] >> 18) & 0x7FFU));
to_push.bus = bus_number;
to_push.data_len_code = ((fifo->header[1] >> 16) & 0xFU);
uint8_t data_len_w = (dlc_to_len[to_push.data_len_code] / 4U);
- data_len_w += ((dlc_to_len[to_push.data_len_code] % 4) > 0) ? 1U : 0U;
+ data_len_w += ((dlc_to_len[to_push.data_len_code] % 4U) > 0U) ? 1U : 0U;
for (unsigned int i = 0; i < data_len_w; i++) {
- WORD_TO_BYTE_ARRAY(&to_push.data[i*4], fifo->data_word[i]);
+ WORD_TO_BYTE_ARRAY(&to_push.data[i*4U], fifo->data_word[i]);
}
// forwarding (panda only)
diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h
index 9b95e0dd8..2e5225691 100644
--- a/panda/board/drivers/gmlan_alt.h
+++ b/panda/board/drivers/gmlan_alt.h
@@ -91,7 +91,7 @@ int get_bit_message(char *out, CANPacket_t *to_bang) {
int dlc_len = GET_LEN(to_bang);
len = append_int(pkt, len, 0, 1); // Start-of-frame
- if (to_bang->extended != 0) {
+ if (to_bang->extended != 0U) {
// extended identifier
len = append_int(pkt, len, GET_ADDR(to_bang) >> 18, 11); // Identifier
len = append_int(pkt, len, 3, 2); // SRR+IDE
diff --git a/panda/board/early_init.h b/panda/board/early_init.h
index 0841fc876..ae9a90362 100644
--- a/panda/board/early_init.h
+++ b/panda/board/early_init.h
@@ -38,9 +38,9 @@ void early_initialization(void) {
// if wrong chip, reboot
volatile unsigned int id = DBGMCU->IDCODE;
- if ((id & 0xFFFU) != MCU_IDCODE) {
- enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
- }
+ if ((id & 0xFFFU) != MCU_IDCODE) {
+ enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
+ }
// setup interrupt table
SCB->VTOR = (uint32_t)&g_pfnVectors;
diff --git a/panda/board/pedal/README b/panda/board/pedal/README
index cf779db25..9e004d6bd 100644
--- a/panda/board/pedal/README
+++ b/panda/board/pedal/README
@@ -1,8 +1,8 @@
-This is the firmware for the comma pedal. It borrows a lot from panda.
+# pedal
-The comma pedal is a gas pedal interceptor for Honda/Acura. It allows you to "virtually" press the pedal.
+This is the firmware for the comma pedal.
-This is the open source software. Note that it is not ready to use yet.
+The comma pedal is a gas pedal interceptor for Honda/Acura and Toyota/Lexus. It allows you to "virtually" press the pedal and borrows a lot from panda.
== Test Plan ==
diff --git a/panda/board/safety.h b/panda/board/safety.h
index a833dc1b1..1f1bcba72 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -33,7 +33,7 @@
#define SAFETY_ALLOUTPUT 17U
#define SAFETY_GM_ASCM 18U
#define SAFETY_NOOUTPUT 19U
-#define SAFETY_HONDA_BOSCH_HARNESS 20U
+#define SAFETY_HONDA_BOSCH 20U
#define SAFETY_VOLKSWAGEN_PQ 21U
#define SAFETY_SUBARU_LEGACY 22U
#define SAFETY_HYUNDAI_LEGACY 23U
@@ -241,7 +241,7 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_TOYOTA, &toyota_hooks},
{SAFETY_ELM327, &elm327_hooks},
{SAFETY_GM, &gm_hooks},
- {SAFETY_HONDA_BOSCH_HARNESS, &honda_bosch_harness_hooks},
+ {SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
{SAFETY_HYUNDAI, &hyundai_hooks},
{SAFETY_CHRYSLER, &chrysler_hooks},
{SAFETY_SUBARU, &subaru_hooks},
diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h
index 35474f346..f94f75e7b 100644
--- a/panda/board/safety/safety_chrysler.h
+++ b/panda/board/safety/safety_chrysler.h
@@ -19,7 +19,7 @@ AddrCheckStruct chrysler_addr_checks[] = {
addr_checks chrysler_rx_checks = {chrysler_addr_checks, CHRYSLER_ADDR_CHECK_LEN};
static uint8_t chrysler_get_checksum(CANPacket_t *to_push) {
- int checksum_byte = GET_LEN(to_push) - 1;
+ int checksum_byte = GET_LEN(to_push) - 1U;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
}
@@ -67,7 +67,7 @@ static int chrysler_rx_hook(CANPacket_t *to_push) {
chrysler_get_checksum, chrysler_compute_checksum,
chrysler_get_counter);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
// Measured eps torque
@@ -80,7 +80,7 @@ static int chrysler_rx_hook(CANPacket_t *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 500) {
- int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
+ int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38U) >> 3) == 7U;
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = 1;
}
@@ -100,12 +100,12 @@ static int chrysler_rx_hook(CANPacket_t *to_push) {
// exit controls on rising edge of gas press
if (addr == 308) {
- gas_pressed = ((GET_BYTE(to_push, 5) & 0x7F) != 0) && ((int)vehicle_speed > CHRYSLER_GAS_THRSLD);
+ gas_pressed = ((GET_BYTE(to_push, 5) & 0x7FU) != 0U) && ((int)vehicle_speed > CHRYSLER_GAS_THRSLD);
}
// exit controls on rising edge of brake press
if (addr == 320) {
- brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5;
+ brake_pressed = (GET_BYTE(to_push, 0) & 0x7U) == 5U;
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
controls_allowed = 0;
}
@@ -174,7 +174,7 @@ static int chrysler_tx_hook(CANPacket_t *to_send) {
// FORCE CANCEL: only the cancel button press is allowed
if (addr == 571) {
- if ((GET_BYTE(to_send, 0) != 1) || ((GET_BYTE(to_send, 1) & 1) == 1)) {
+ if ((GET_BYTE(to_send, 0) != 1U) || ((GET_BYTE(to_send, 1) & 1U) == 1U)) {
tx = 0;
}
}
diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h
index 6a8f05838..ccdc54e6e 100644
--- a/panda/board/safety/safety_ford.h
+++ b/panda/board/safety/safety_ford.h
@@ -24,8 +24,8 @@ static int ford_rx_hook(CANPacket_t *to_push) {
// state machine to enter and exit controls
if (addr == 0x83) {
- bool cancel = GET_BYTE(to_push, 1) & 0x1;
- bool set_or_resume = GET_BYTE(to_push, 3) & 0x30;
+ bool cancel = GET_BYTE(to_push, 1) & 0x1U;
+ bool set_or_resume = GET_BYTE(to_push, 3) & 0x30U;
if (cancel) {
controls_allowed = 0;
}
@@ -37,7 +37,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (addr == 0x165) {
- brake_pressed = GET_BYTE(to_push, 0) & 0x20;
+ brake_pressed = GET_BYTE(to_push, 0) & 0x20U;
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
controls_allowed = 0;
}
@@ -46,7 +46,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
// exit controls on rising edge of gas press
if (addr == 0x204) {
- gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0;
+ gas_pressed = ((GET_BYTE(to_push, 0) & 0x03U) | GET_BYTE(to_push, 1)) != 0U;
if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
@@ -83,7 +83,7 @@ static int ford_tx_hook(CANPacket_t *to_send) {
if (addr == 0x3CA) {
if (!current_controls_allowed) {
// bits 7-4 need to be 0xF to disallow lkas commands
- if ((GET_BYTE(to_send, 0) & 0xF0) != 0xF0) {
+ if ((GET_BYTE(to_send, 0) & 0xF0U) != 0xF0U) {
tx = 0;
}
}
@@ -92,7 +92,7 @@ static int ford_tx_hook(CANPacket_t *to_send) {
// FORCE CANCEL: safety check only relevant when spamming the cancel button
// ensuring that set and resume aren't sent
if (addr == 0x83) {
- if ((GET_BYTE(to_send, 3) & 0x30) != 0) {
+ if ((GET_BYTE(to_send, 3) & 0x30U) != 0U) {
tx = 0;
}
}
diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h
index 6ef30f279..9a9fef611 100644
--- a/panda/board/safety/safety_gm.h
+++ b/panda/board/safety/safety_gm.h
@@ -38,11 +38,11 @@ static int gm_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &gm_rx_checks, NULL, NULL, NULL);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
if (addr == 388) {
- int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7);
+ int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7U) << 8) | GET_BYTE(to_push, 7);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&torque_driver, torque_driver_new);
@@ -56,7 +56,7 @@ static int gm_rx_hook(CANPacket_t *to_push) {
// ACC steering wheel buttons
if (addr == 481) {
- int button = (GET_BYTE(to_push, 5) & 0x70) >> 4;
+ int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
switch (button) {
case 2: // resume
case 3: // set
@@ -74,16 +74,16 @@ static int gm_rx_hook(CANPacket_t *to_push) {
if (addr == 241) {
// Brake pedal's potentiometer returns near-zero reading
// even when pedal is not pressed
- brake_pressed = GET_BYTE(to_push, 1) >= 10;
+ brake_pressed = GET_BYTE(to_push, 1) >= 10U;
}
if (addr == 417) {
- gas_pressed = GET_BYTE(to_push, 6) != 0;
+ gas_pressed = GET_BYTE(to_push, 6) != 0U;
}
// exit controls on regen paddle
if (addr == 189) {
- bool regen = GET_BYTE(to_push, 0) & 0x20;
+ bool regen = GET_BYTE(to_push, 0) & 0x20U;
if (regen) {
controls_allowed = 0;
}
diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
index 961a999b5..ed19fb8a6 100644
--- a/panda/board/safety/safety_honda.h
+++ b/panda/board/safety/safety_honda.h
@@ -7,8 +7,8 @@
// brake rising edge
// brake > 0mph
const CanMsg HONDA_N_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x200, 0, 6}, {0x30C, 0, 8}, {0x33D, 0, 5}};
-const CanMsg HONDA_BH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}}; // Bosch Harness
-const CanMsg HONDA_BH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1, 8}, {0x1FA, 1, 8}, {0x30C, 1, 8}, {0x33D, 1, 5}, {0x39F, 1, 8}, {0x18DAB0F1, 1, 8}}; // Bosch Harness w/ gas and brakes
+const CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}, {0x33DA, 0, 5}, {0x33DB, 0, 8}}; // Bosch
+const CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1, 8}, {0x1FA, 1, 8}, {0x30C, 1, 8}, {0x33D, 1, 5}, {0x33DA, 1, 5}, {0x33DB, 1, 8}, {0x39F, 1, 8}, {0x18DAB0F1, 1, 8}}; // Bosch w/ gas and brakes
// Roughly calculated using the offsets in openpilot +5%:
// In openpilot: ((gas1_norm + gas2_norm)/2) > 15
@@ -17,7 +17,7 @@ const CanMsg HONDA_BH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1,
// assuming that 2*(gain_dbc1*gas1) == (gain_dbc2*gas2)
// In this safety: ((gas1 + (gas2/2))/2) > THRESHOLD
const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 344;
-#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2) // avg between 2 tracks
+#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U ) / 2U) // avg between 2 tracks
const int HONDA_BOSCH_NO_GAS_VALUE = -30000; // value sent when not requesting gas
const int HONDA_BOSCH_GAS_MAX = 2000;
const int HONDA_BOSCH_ACCEL_MIN = -350; // max braking == -3.5m/s2
@@ -28,28 +28,28 @@ AddrCheckStruct honda_nidec_addr_checks[] = {
{0x296, 0, 4, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U}, { 0 }}},
{.msg = {{0x158, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{0x17C, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
+ {.msg = {{0x326, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 100000U}, { 0 }, { 0 }}},
};
#define HONDA_NIDEC_ADDR_CHECKS_LEN (sizeof(honda_nidec_addr_checks) / sizeof(honda_nidec_addr_checks[0]))
-// For Nidecs with main on signal on 0x326
+// For Nidecs with main on signal on an alternate msg
AddrCheckStruct honda_nidec_alt_addr_checks[] = {
{.msg = {{0x1A6, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U},
{0x296, 0, 4, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U}, { 0 }}},
{.msg = {{0x158, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{0x17C, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
- {.msg = {{0x326, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 100000U}, { 0 }, { 0 }}},
};
#define HONDA_NIDEC_ALT_ADDR_CHECKS_LEN (sizeof(honda_nidec_alt_addr_checks) / sizeof(honda_nidec_alt_addr_checks[0]))
-// Bosch harness has pt on bus 1
-AddrCheckStruct honda_bh_addr_checks[] = {
+// Bosch has pt on bus 1
+AddrCheckStruct honda_bosch_addr_checks[] = {
{.msg = {{0x296, 1, 4, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U}, { 0 }, { 0 }}},
{.msg = {{0x158, 1, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{0x17C, 1, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
{0x1BE, 1, 3, .check_checksum = true, .max_counter = 3U, .expected_timestep = 20000U}, { 0 }}},
{.msg = {{0x326, 1, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 100000U}, { 0 }, { 0 }}},
};
-#define HONDA_BH_ADDR_CHECKS_LEN (sizeof(honda_bh_addr_checks) / sizeof(honda_bh_addr_checks[0]))
+#define HONDA_BOSCH_ADDR_CHECKS_LEN (sizeof(honda_bosch_addr_checks) / sizeof(honda_bosch_addr_checks[0]))
const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
const uint16_t HONDA_PARAM_BOSCH_LONG = 2;
@@ -59,12 +59,12 @@ int honda_brake = 0;
bool honda_alt_brake_msg = false;
bool honda_fwd_brake = false;
bool honda_bosch_long = false;
-enum {HONDA_N_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW;
+enum {HONDA_NIDEC, HONDA_BOSCH} honda_hw = HONDA_NIDEC;
addr_checks honda_rx_checks = {honda_nidec_addr_checks, HONDA_NIDEC_ADDR_CHECKS_LEN};
static uint8_t honda_get_checksum(CANPacket_t *to_push) {
- int checksum_byte = GET_LEN(to_push) - 1;
+ int checksum_byte = GET_LEN(to_push) - 1U;
return (uint8_t)(GET_BYTE(to_push, checksum_byte)) & 0xFU;
}
@@ -86,7 +86,7 @@ static uint8_t honda_compute_checksum(CANPacket_t *to_push) {
}
static uint8_t honda_get_counter(CANPacket_t *to_push) {
- int counter_byte = GET_LEN(to_push) - 1;
+ int counter_byte = GET_LEN(to_push) - 1U;
return ((uint8_t)(GET_BYTE(to_push, counter_byte)) >> 4U) & 0x3U;
}
@@ -109,7 +109,7 @@ static int honda_rx_hook(CANPacket_t *to_push) {
// check ACC main state
// 0x326 for all Bosch and some Nidec, 0x1A6 for some Nidec
if ((addr == 0x326) || (addr == 0x1A6)) {
- acc_main_on = GET_BIT(to_push, (addr == 0x326) ? 28 : 47);
+ acc_main_on = GET_BIT(to_push, ((addr == 0x326) ? 28U : 47U));
if (!acc_main_on) {
controls_allowed = 0;
}
@@ -119,7 +119,7 @@ static int honda_rx_hook(CANPacket_t *to_push) {
// 0x1A6 for the ILX, 0x296 for the Civic Touring
if ((addr == 0x1A6) || (addr == 0x296)) {
// check for button presses
- int button = (GET_BYTE(to_push, 0) & 0xE0) >> 5;
+ int button = (GET_BYTE(to_push, 0) & 0xE0U) >> 5;
switch (button) {
case 1: // main
case 2: // cancel
@@ -144,7 +144,7 @@ static int honda_rx_hook(CANPacket_t *to_push) {
// accord, crv: 0x1BE bit 4
bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
if (is_user_brake_msg) {
- brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
+ brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10U) : (GET_BYTE((to_push), 6) & 0x20U);
}
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
@@ -157,15 +157,15 @@ static int honda_rx_hook(CANPacket_t *to_push) {
if (!gas_interceptor_detected) {
if (addr == 0x17C) {
- gas_pressed = GET_BYTE(to_push, 0) != 0;
+ gas_pressed = GET_BYTE(to_push, 0) != 0U;
}
}
// disable stock Honda AEB in unsafe mode
if ( !(unsafe_mode & UNSAFE_DISABLE_STOCK_AEB) ) {
if ((bus == 2) && (addr == 0x1FA)) {
- bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20;
- int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3);
+ bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20U;
+ int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3U);
// Forward AEB when stock braking is higher than openpilot braking
// only stop forwarding when AEB event is over
@@ -180,14 +180,14 @@ static int honda_rx_hook(CANPacket_t *to_push) {
}
bool stock_ecu_detected = false;
- int bus_rdr_car = (honda_hw == HONDA_BH_HW) ? 0 : 2; // radar bus, car side
- int pt_bus = (honda_hw == HONDA_BH_HW) ? 1 : 0;
+ int bus_rdr_car = (honda_hw == HONDA_BOSCH) ? 0 : 2; // radar bus, car side
+ int pt_bus = (honda_hw == HONDA_BOSCH) ? 1 : 0;
if (safety_mode_cnt > RELAY_TRNS_TIMEOUT) {
// If steering controls messages are received on the destination bus, it's an indication
// that the relay might be malfunctioning
if ((addr == 0xE4) || (addr == 0x194)) {
- if (((honda_hw != HONDA_N_HW) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_N_HW) && (bus == 0))) {
+ if (((honda_hw != HONDA_NIDEC) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_NIDEC) && (bus == 0))) {
stock_ecu_detected = true;
}
}
@@ -215,10 +215,10 @@ static int honda_tx_hook(CANPacket_t *to_send) {
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
- if ((honda_hw == HONDA_BH_HW) && !honda_bosch_long) {
- tx = msg_allowed(to_send, HONDA_BH_TX_MSGS, sizeof(HONDA_BH_TX_MSGS)/sizeof(HONDA_BH_TX_MSGS[0]));
- } else if ((honda_hw == HONDA_BH_HW) && honda_bosch_long) {
- tx = msg_allowed(to_send, HONDA_BH_LONG_TX_MSGS, sizeof(HONDA_BH_LONG_TX_MSGS)/sizeof(HONDA_BH_LONG_TX_MSGS[0]));
+ if ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) {
+ tx = msg_allowed(to_send, HONDA_BOSCH_TX_MSGS, sizeof(HONDA_BOSCH_TX_MSGS)/sizeof(HONDA_BOSCH_TX_MSGS[0]));
+ } else if ((honda_hw == HONDA_BOSCH) && honda_bosch_long) {
+ tx = msg_allowed(to_send, HONDA_BOSCH_LONG_TX_MSGS, sizeof(HONDA_BOSCH_LONG_TX_MSGS)/sizeof(HONDA_BOSCH_LONG_TX_MSGS[0]));
} else {
tx = msg_allowed(to_send, HONDA_N_TX_MSGS, sizeof(HONDA_N_TX_MSGS)/sizeof(HONDA_N_TX_MSGS[0]));
}
@@ -231,11 +231,11 @@ static int honda_tx_hook(CANPacket_t *to_send) {
pedal_pressed = pedal_pressed || gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD);
}
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
- int bus_pt = (honda_hw == HONDA_BH_HW)? 1 : 0;
+ int bus_pt = (honda_hw == HONDA_BOSCH) ? 1 : 0;
// BRAKE: safety check (nidec)
if ((addr == 0x1FA) && (bus == bus_pt)) {
- honda_brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3);
+ honda_brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3U);
if (!current_controls_allowed) {
if (honda_brake != 0) {
tx = 0;
@@ -251,7 +251,7 @@ static int honda_tx_hook(CANPacket_t *to_send) {
// BRAKE/GAS: safety check (bosch)
if ((addr == 0x1DF) && (bus == bus_pt)) {
- int accel = (GET_BYTE(to_send, 3) << 3) | ((GET_BYTE(to_send, 4) >> 5) & 0x7);
+ int accel = (GET_BYTE(to_send, 3) << 3) | ((GET_BYTE(to_send, 4) >> 5) & 0x7U);
accel = to_signed(accel, 11);
if (!current_controls_allowed) {
if (accel != 0) {
@@ -284,9 +284,9 @@ static int honda_tx_hook(CANPacket_t *to_send) {
}
}
- // Bosch supplemental control check
+ // Bosch supplemental control check
if (addr == 0xE5) {
- if ((GET_BYTES_04(to_send) != 0x10800004) || ((GET_BYTES_48(to_send) & 0x00FFFFFF) != 0x0)) {
+ if ((GET_BYTES_04(to_send) != 0x10800004U) || ((GET_BYTES_48(to_send) & 0x00FFFFFFU) != 0x0U)) {
tx = 0;
}
}
@@ -304,14 +304,14 @@ static int honda_tx_hook(CANPacket_t *to_send) {
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((addr == 0x296) && !current_controls_allowed && (bus == bus_pt)) {
- if (((GET_BYTE(to_send, 0) >> 5) & 0x7) != 2) {
+ if (((GET_BYTE(to_send, 0) >> 5) & 0x7U) != 2U) {
tx = 0;
}
}
// Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (addr == 0x18DAB0F1) {
- if ((GET_BYTES_04(to_send) != 0x00803E02) || (GET_BYTES_48(to_send) != 0x0)) {
+ if ((GET_BYTES_04(to_send) != 0x00803E02U) || (GET_BYTES_48(to_send) != 0x0U)) {
tx = 0;
}
}
@@ -325,26 +325,22 @@ static const addr_checks* honda_nidec_init(int16_t param) {
controls_allowed = false;
relay_malfunction_reset();
gas_interceptor_detected = 0;
- honda_hw = HONDA_N_HW;
+ honda_hw = HONDA_NIDEC;
honda_alt_brake_msg = false;
honda_bosch_long = false;
- honda_rx_checks = (addr_checks){honda_nidec_addr_checks, HONDA_NIDEC_ADDR_CHECKS_LEN};
-
- /*
if (GET_FLAG(param, HONDA_PARAM_NIDEC_ALT)) {
honda_rx_checks = (addr_checks){honda_nidec_alt_addr_checks, HONDA_NIDEC_ALT_ADDR_CHECKS_LEN};
} else {
honda_rx_checks = (addr_checks){honda_nidec_addr_checks, HONDA_NIDEC_ADDR_CHECKS_LEN};
}
- */
return &honda_rx_checks;
}
-static const addr_checks* honda_bosch_harness_init(int16_t param) {
+static const addr_checks* honda_bosch_init(int16_t param) {
controls_allowed = false;
relay_malfunction_reset();
- honda_hw = HONDA_BH_HW;
+ honda_hw = HONDA_BOSCH;
// Checking for alternate brake override from safety parameter
honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE);
@@ -353,7 +349,7 @@ static const addr_checks* honda_bosch_harness_init(int16_t param) {
honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG);
#endif
- honda_rx_checks = (addr_checks){honda_bh_addr_checks, HONDA_BH_ADDR_CHECKS_LEN};
+ honda_rx_checks = (addr_checks){honda_bosch_addr_checks, HONDA_BOSCH_ADDR_CHECKS_LEN};
return &honda_rx_checks;
}
@@ -384,17 +380,15 @@ static int honda_nidec_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
static int honda_bosch_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
- int bus_rdr_cam = (honda_hw == HONDA_BH_HW) ? 2 : 1; // radar bus, camera side
- int bus_rdr_car = (honda_hw == HONDA_BH_HW) ? 0 : 2; // radar bus, car side
- if (bus_num == bus_rdr_car) {
- bus_fwd = bus_rdr_cam;
+ if (bus_num == 0) {
+ bus_fwd = 2;
}
- if (bus_num == bus_rdr_cam) {
+ if (bus_num == 2) {
int addr = GET_ADDR(to_fwd);
- int is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D);
+ int is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D) || (addr == 0x33DA) || (addr == 0x33DB);
if (!is_lkas_msg) {
- bus_fwd = bus_rdr_car;
+ bus_fwd = 0;
}
}
@@ -409,8 +403,8 @@ const safety_hooks honda_nidec_hooks = {
.fwd = honda_nidec_fwd_hook,
};
-const safety_hooks honda_bosch_harness_hooks = {
- .init = honda_bosch_harness_init,
+const safety_hooks honda_bosch_hooks = {
+ .init = honda_bosch_init,
.rx = honda_rx_hook,
.tx = honda_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
index 4e4f0e65a..883b8c6b2 100644
--- a/panda/board/safety/safety_hyundai.h
+++ b/panda/board/safety/safety_hyundai.h
@@ -74,15 +74,15 @@ static uint8_t hyundai_get_counter(CANPacket_t *to_push) {
uint8_t cnt;
if (addr == 608) {
- cnt = (GET_BYTE(to_push, 7) >> 4) & 0x3;
+ cnt = (GET_BYTE(to_push, 7) >> 4) & 0x3U;
} else if (addr == 902) {
cnt = ((GET_BYTE(to_push, 3) >> 6) << 2) | (GET_BYTE(to_push, 1) >> 6);
} else if (addr == 916) {
- cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7;
+ cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7U;
} else if (addr == 1057) {
- cnt = GET_BYTE(to_push, 7) & 0xF;
+ cnt = GET_BYTE(to_push, 7) & 0xFU;
} else if (addr == 1265) {
- cnt = (GET_BYTE(to_push, 3) >> 4) & 0xF;
+ cnt = (GET_BYTE(to_push, 3) >> 4) & 0xFU;
} else {
cnt = 0;
}
@@ -94,11 +94,11 @@ static uint8_t hyundai_get_checksum(CANPacket_t *to_push) {
uint8_t chksum;
if (addr == 608) {
- chksum = GET_BYTE(to_push, 7) & 0xF;
+ chksum = GET_BYTE(to_push, 7) & 0xFU;
} else if (addr == 902) {
chksum = ((GET_BYTE(to_push, 7) >> 6) << 2) | (GET_BYTE(to_push, 5) >> 6);
} else if (addr == 916) {
- chksum = GET_BYTE(to_push, 6) & 0xF;
+ chksum = GET_BYTE(to_push, 6) & 0xFU;
} else if (addr == 1057) {
chksum = GET_BYTE(to_push, 7) >> 4;
} else {
@@ -149,11 +149,11 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
hyundai_get_checksum, hyundai_compute_checksum,
hyundai_get_counter);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
if (addr == 593) {
- int torque_driver_new = ((GET_BYTES_04(to_push) & 0x7ff) * 0.79) - 808; // scale down new driver torque signal to match previous one
+ int torque_driver_new = ((GET_BYTES_04(to_push) & 0x7ffU) * 0.79) - 808; // scale down new driver torque signal to match previous one
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
@@ -161,7 +161,7 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
if (hyundai_longitudinal) {
// ACC steering wheel buttons
if (addr == 1265) {
- int button = GET_BYTE(to_push, 0) & 0x7;
+ int button = GET_BYTE(to_push, 0) & 0x7U;
switch (button) {
case 1: // resume
case 2: // set
@@ -178,7 +178,7 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 1057) {
// 2 bits: 13-14
- int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
+ int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3U;
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = 1;
}
@@ -191,24 +191,23 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
// read gas pressed signal
if ((addr == 881) && hyundai_ev_gas_signal) {
- gas_pressed = (((GET_BYTE(to_push, 4) & 0x7F) << 1) | GET_BYTE(to_push, 3) >> 7) != 0;
+ gas_pressed = (((GET_BYTE(to_push, 4) & 0x7FU) << 1) | GET_BYTE(to_push, 3) >> 7) != 0U;
} else if ((addr == 881) && hyundai_hybrid_gas_signal) {
- gas_pressed = GET_BYTE(to_push, 7) != 0;
+ gas_pressed = GET_BYTE(to_push, 7) != 0U;
} else if (addr == 608) { // ICE
- gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0;
+ gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0U;
} else {
}
// sample wheel speed, averaging opposite corners
if (addr == 902) {
- int hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL
- hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL
+ int hyundai_speed = (GET_BYTES_04(to_push) & 0x3FFFU) + ((GET_BYTES_48(to_push) >> 16) & 0x3FFFU); // FL + RR
hyundai_speed /= 2;
vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD;
}
if (addr == 916) {
- brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
+ brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0U;
}
bool stock_ecu_detected = (addr == 832);
@@ -237,8 +236,8 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
// FCA11: Block any potential actuation
if (addr == 909) {
int CR_VSM_DecCmd = GET_BYTE(to_send, 1);
- int FCA_CmdAct = (GET_BYTE(to_send, 2) >> 5) & 1;
- int CF_VSM_DecCmdAct = (GET_BYTE(to_send, 3) >> 7) & 1;
+ int FCA_CmdAct = (GET_BYTE(to_send, 2) >> 5) & 1U;
+ int CF_VSM_DecCmdAct = (GET_BYTE(to_send, 3) >> 7) & 1U;
if ((CR_VSM_DecCmd != 0) || (FCA_CmdAct != 0) || (CF_VSM_DecCmdAct != 0)) {
tx = 0;
@@ -247,11 +246,11 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
// ACCEL: safety check
if (addr == 1057) {
- int desired_accel_raw = (((GET_BYTE(to_send, 4) & 0x7) << 8) | GET_BYTE(to_send, 3)) - 1023;
- int desired_accel_val = ((GET_BYTE(to_send, 5) << 3) | (GET_BYTE(to_send, 4) >> 5)) - 1023;
+ int desired_accel_raw = (((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) - 1023U;
+ int desired_accel_val = ((GET_BYTE(to_send, 5) << 3) | (GET_BYTE(to_send, 4) >> 5)) - 1023U;
int aeb_decel_cmd = GET_BYTE(to_send, 2);
- int aeb_req = (GET_BYTE(to_send, 6) >> 6) & 1;
+ int aeb_req = (GET_BYTE(to_send, 6) >> 6) & 1U;
bool violation = 0;
@@ -273,7 +272,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
// LKA STEER: safety check
if (addr == 832) {
- int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024;
+ int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ffU) - 1024U;
uint32_t ts = microsecond_timer_get();
bool violation = 0;
@@ -320,7 +319,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
// UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (addr == 2000) {
- if ((GET_BYTES_04(to_send) != 0x00803E02) || (GET_BYTES_48(to_send) != 0x0)) {
+ if ((GET_BYTES_04(to_send) != 0x00803E02U) || (GET_BYTES_48(to_send) != 0x0U)) {
tx = 0;
}
}
@@ -329,7 +328,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
// ensuring that only the cancel button press is sent (VAL 4) when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((addr == 1265) && !controls_allowed) {
- if ((GET_BYTES_04(to_send) & 0x7) != 4) {
+ if ((GET_BYTES_04(to_send) & 0x7U) != 4U) {
tx = 0;
}
}
diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h
index 01cc29f1b..48632f830 100644
--- a/panda/board/safety/safety_mazda.h
+++ b/panda/board/safety/safety_mazda.h
@@ -9,10 +9,10 @@
// CAN bus numbers
#define MAZDA_MAIN 0
-#define MAZDA_AUX 1
-#define MAZDA_CAM 2
+#define MAZDA_AUX 1
+#define MAZDA_CAM 2
-#define MAZDA_MAX_STEER 2048
+#define MAZDA_MAX_STEER 2048U
// max delta torque allowed for real time checks
#define MAZDA_MAX_RT_DELTA 940
@@ -39,7 +39,7 @@ addr_checks mazda_rx_checks = {mazda_addr_checks, MAZDA_ADDR_CHECKS_LEN};
// track msgs coming from OP so that we know what CAM msgs to drop and what to forward
static int mazda_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &mazda_rx_checks, NULL, NULL, NULL);
- if (valid && (GET_BUS(to_push) == MAZDA_MAIN)) {
+ if (valid && ((int)GET_BUS(to_push) == MAZDA_MAIN)) {
int addr = GET_ADDR(to_push);
if (addr == MAZDA_ENGINE_DATA) {
@@ -49,14 +49,14 @@ static int mazda_rx_hook(CANPacket_t *to_push) {
}
if (addr == MAZDA_STEER_TORQUE) {
- int torque_driver_new = GET_BYTE(to_push, 0) - 127;
+ int torque_driver_new = GET_BYTE(to_push, 0) - 127U;
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == MAZDA_CRZ_CTRL) {
- bool cruise_engaged = GET_BYTE(to_push, 0) & 8;
+ bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U;
if (cruise_engaged) {
if (!cruise_engaged_prev) {
controls_allowed = 1;
@@ -68,11 +68,11 @@ static int mazda_rx_hook(CANPacket_t *to_push) {
}
if (addr == MAZDA_ENGINE_DATA) {
- gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0));
+ gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0U));
}
if (addr == MAZDA_PEDALS) {
- brake_pressed = (GET_BYTE(to_push, 0) & 0x10);
+ brake_pressed = (GET_BYTE(to_push, 0) & 0x10U);
}
generic_rx_checks((addr == MAZDA_LKAS));
@@ -94,7 +94,7 @@ static int mazda_tx_hook(CANPacket_t *to_send) {
// steer cmd checks
if (addr == MAZDA_LKAS) {
- int desired_torque = (((GET_BYTE(to_send, 0) & 0x0f) << 8) | GET_BYTE(to_send, 1)) - MAZDA_MAX_STEER;
+ int desired_torque = (((GET_BYTE(to_send, 0) & 0x0FU) << 8) | GET_BYTE(to_send, 1)) - MAZDA_MAX_STEER;
bool violation = 0;
uint32_t ts = microsecond_timer_get();
diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h
index 2c2c6f680..920a11a22 100644
--- a/panda/board/safety/safety_nissan.h
+++ b/panda/board/safety/safety_nissan.h
@@ -53,7 +53,7 @@ static int nissan_rx_hook(CANPacket_t *to_push) {
if (addr == 0x2) {
// Current steering angle
// Factor -0.1, little endian
- int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFF);
+ int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFFU);
// Need to multiply by 10 here as LKAS and Steering wheel are different base unit
angle_meas_new = to_signed(angle_meas_new, 16) * 10;
@@ -71,9 +71,9 @@ static int nissan_rx_hook(CANPacket_t *to_push) {
// X-Trail 0x15c, Leaf 0x239
if ((addr == 0x15c) || (addr == 0x239)) {
if (addr == 0x15c){
- gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)) > 3;
+ gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3U)) > 3U;
} else {
- gas_pressed = GET_BYTE(to_push, 0) > 3;
+ gas_pressed = GET_BYTE(to_push, 0) > 3U;
}
}
}
@@ -81,15 +81,15 @@ static int nissan_rx_hook(CANPacket_t *to_push) {
// X-trail 0x454, Leaf 0x239
if ((addr == 0x454) || (addr == 0x239)) {
if (addr == 0x454){
- brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
+ brake_pressed = (GET_BYTE(to_push, 2) & 0x80U) != 0U;
} else {
- brake_pressed = ((GET_BYTE(to_push, 4) >> 5) & 1) != 0;
+ brake_pressed = ((GET_BYTE(to_push, 4) >> 5) & 1U) != 0U;
}
}
// Handle cruise enabled
if ((addr == 0x30f) && (((bus == 2) && (!nissan_alt_eps)) || ((bus == 1) && (nissan_alt_eps)))) {
- bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1;
+ bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1U;
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = 1;
@@ -117,8 +117,8 @@ static int nissan_tx_hook(CANPacket_t *to_send) {
// steer cmd checks
if (addr == 0x169) {
- int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3));
- bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1;
+ int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3U));
+ bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1U;
// offeset 1310 * NISSAN_DEG_TO_CAN
desired_angle = desired_angle - 131000;
@@ -154,7 +154,7 @@ static int nissan_tx_hook(CANPacket_t *to_send) {
// acc button check, only allow cancel button to be sent
if (addr == 0x20b) {
// Violation of any button other than cancel is pressed
- violation |= ((GET_BYTE(to_send, 1) & 0x3d) > 0);
+ violation |= ((GET_BYTE(to_send, 1) & 0x3dU) > 0U);
}
if (violation) {
diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h
index 36eecb513..602803438 100644
--- a/panda/board/safety/safety_subaru.h
+++ b/panda/board/safety/safety_subaru.h
@@ -42,7 +42,7 @@ static uint8_t subaru_get_checksum(CANPacket_t *to_push) {
}
static uint8_t subaru_get_counter(CANPacket_t *to_push) {
- return (uint8_t)(GET_BYTE(to_push, 1) & 0xF);
+ return (uint8_t)(GET_BYTE(to_push, 1) & 0xFU);
}
static uint8_t subaru_compute_checksum(CANPacket_t *to_push) {
@@ -60,18 +60,18 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &subaru_rx_checks,
subaru_get_checksum, subaru_compute_checksum, subaru_get_counter);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
if (addr == 0x119) {
int torque_driver_new;
- torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
+ torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FFU);
torque_driver_new = -1 * to_signed(torque_driver_new, 11);
update_sample(&torque_driver, torque_driver_new);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x240) {
- int cruise_engaged = ((GET_BYTES_48(to_push) >> 9) & 1);
+ int cruise_engaged = ((GET_BYTES_48(to_push) >> 9) & 1U);
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = 1;
}
@@ -83,18 +83,17 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
// sample wheel speed, averaging opposite corners
if (addr == 0x13a) {
- int subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR
- subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF; // RL
+ int subaru_speed = ((GET_BYTES_04(to_push) >> 12) & 0x1FFFU) + ((GET_BYTES_48(to_push) >> 6) & 0x1FFFU); // FR + RL
subaru_speed /= 2;
vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD;
}
if (addr == 0x13c) {
- brake_pressed = ((GET_BYTE(to_push, 7) >> 6) & 1);
+ brake_pressed = ((GET_BYTE(to_push, 7) >> 6) & 1U);
}
if (addr == 0x40) {
- gas_pressed = GET_BYTE(to_push, 4) != 0;
+ gas_pressed = GET_BYTE(to_push, 4) != 0U;
}
generic_rx_checks((addr == 0x122));
@@ -106,7 +105,7 @@ static int subaru_legacy_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &subaru_l_rx_checks, NULL, NULL, NULL);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
if (addr == 0x371) {
int torque_driver_new;
@@ -117,7 +116,7 @@ static int subaru_legacy_rx_hook(CANPacket_t *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x144) {
- int cruise_engaged = ((GET_BYTES_48(to_push) >> 17) & 1);
+ int cruise_engaged = ((GET_BYTES_48(to_push) >> 17) & 1U);
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = 1;
}
@@ -129,18 +128,17 @@ static int subaru_legacy_rx_hook(CANPacket_t *to_push) {
// sample wheel speed, averaging opposite corners
if (addr == 0xD4) {
- int subaru_speed = (GET_BYTES_04(to_push) >> 16) & 0xFFFF; // FR
- subaru_speed += GET_BYTES_48(to_push) & 0xFFFF; // RL
+ int subaru_speed = ((GET_BYTES_04(to_push) >> 16) & 0xFFFFU) + (GET_BYTES_48(to_push) & 0xFFFFU); // FR + RL
subaru_speed /= 2;
vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD;
}
if (addr == 0xD1) {
- brake_pressed = ((GET_BYTES_04(to_push) >> 16) & 0xFF) > 0;
+ brake_pressed = ((GET_BYTES_04(to_push) >> 16) & 0xFFU) > 0U;
}
if (addr == 0x140) {
- gas_pressed = GET_BYTE(to_push, 0) != 0;
+ gas_pressed = GET_BYTE(to_push, 0) != 0U;
}
generic_rx_checks((addr == 0x164));
@@ -158,7 +156,7 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
// steer cmd checks
if (addr == 0x122) {
- int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x1FFF);
+ int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x1FFFU);
bool violation = 0;
uint32_t ts = microsecond_timer_get();
@@ -218,7 +216,7 @@ static int subaru_legacy_tx_hook(CANPacket_t *to_send) {
// steer cmd checks
if (addr == 0x164) {
- int desired_torque = ((GET_BYTES_04(to_send) >> 8) & 0x1FFF);
+ int desired_torque = ((GET_BYTES_04(to_send) >> 8) & 0x1FFFU);
bool violation = 0;
uint32_t ts = microsecond_timer_get();
diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h
index 359c90078..ccbff7d60 100644
--- a/panda/board/safety/safety_tesla.h
+++ b/panda/board/safety/safety_tesla.h
@@ -62,25 +62,25 @@ static int tesla_rx_hook(CANPacket_t *to_push) {
if(addr == 0x370) {
// Steering angle: (0.1 * val) - 819.2 in deg.
// Store it 1/10 deg to match steering request
- int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3F) << 8) | GET_BYTE(to_push, 5)) - 8192;
+ int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3FU) << 8) | GET_BYTE(to_push, 5)) - 8192U;
update_sample(&angle_meas, angle_meas_new);
}
}
if(addr == (tesla_powertrain ? 0x116 : 0x118)) {
// Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS
- vehicle_speed = (((((GET_BYTE(to_push, 3) & 0x0F) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
+ vehicle_speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
vehicle_moving = ABS(vehicle_speed) > 0.1;
}
if(addr == (tesla_powertrain ? 0x106 : 0x108)) {
// Gas pressed
- gas_pressed = (GET_BYTE(to_push, 6) != 0);
+ gas_pressed = (GET_BYTE(to_push, 6) != 0U);
}
if(addr == (tesla_powertrain ? 0x1f8 : 0x20a)) {
// Brake pressed
- brake_pressed = (((GET_BYTE(to_push, 0) & 0x0C) >> 2) != 1);
+ brake_pressed = (((GET_BYTE(to_push, 0) & 0x0CU) >> 2) != 1U);
}
if(addr == (tesla_powertrain ? 0x256 : 0x368)) {
@@ -129,7 +129,7 @@ static int tesla_tx_hook(CANPacket_t *to_send) {
if(!tesla_powertrain && (addr == 0x488)) {
// Steering control: (0.1 * val) - 1638.35 in deg.
// We use 1/10 deg as a unit here
- int raw_angle_can = (((GET_BYTE(to_send, 0) & 0x7F) << 8) | GET_BYTE(to_send, 1));
+ int raw_angle_can = (((GET_BYTE(to_send, 0) & 0x7FU) << 8) | GET_BYTE(to_send, 1));
int desired_angle = raw_angle_can - 16384;
int steer_control_type = GET_BYTE(to_send, 2) >> 6;
bool steer_control_enabled = (steer_control_type != 0) && // NONE
@@ -164,7 +164,7 @@ static int tesla_tx_hook(CANPacket_t *to_send) {
if(!tesla_powertrain && (addr == 0x45)) {
// No button other than cancel can be sent by us
- int control_lever_status = (GET_BYTE(to_send, 0) & 0x3F);
+ int control_lever_status = (GET_BYTE(to_send, 0) & 0x3FU);
if((control_lever_status != 0) && (control_lever_status != 1)) {
violation = true;
}
@@ -174,14 +174,14 @@ static int tesla_tx_hook(CANPacket_t *to_send) {
// DAS_control: longitudinal control message
if (tesla_longitudinal) {
// No AEB events may be sent by openpilot
- int aeb_event = GET_BYTE(to_send, 2) & 0x03;
+ int aeb_event = GET_BYTE(to_send, 2) & 0x03U;
if (aeb_event != 0) {
violation = true;
}
// Don't allow any acceleration limits above the safety limits
- int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1F) << 4) | (GET_BYTE(to_send, 5) >> 4);
- int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0F) << 5) | (GET_BYTE(to_send, 4) >> 3);
+ int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1FU) << 4) | (GET_BYTE(to_send, 5) >> 4);
+ int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0FU) << 5) | (GET_BYTE(to_send, 4) >> 3);
float accel_max = (0.04 * raw_accel_max) - 15;
float accel_min = (0.04 * raw_accel_min) - 15;
diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index 843acab0b..f3d5eff7e 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -24,7 +24,7 @@ const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph
// gas_norm2 = ((gain_dbc*gas2) + offset2_dbc)
// In this safety: ((gas1 + gas2)/2) > THRESHOLD
const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 845;
-#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2) // avg between 2 tracks
+#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
const CanMsg TOYOTA_TX_MSGS[] = {{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, // DSU bus 0
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, // DSU bus 1
@@ -55,7 +55,7 @@ static uint8_t toyota_compute_checksum(CANPacket_t *to_push) {
}
static uint8_t toyota_get_checksum(CANPacket_t *to_push) {
- int checksum_byte = GET_LEN(to_push) - 1;
+ int checksum_byte = GET_LEN(to_push) - 1U;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
}
@@ -64,7 +64,7 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &toyota_rx_checks,
toyota_get_checksum, toyota_compute_checksum, NULL);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
// get eps motor torque (0.66 factor in dbc)
@@ -87,7 +87,7 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
// exit controls on rising edge of gas press
if (addr == 0x1D2) {
// 5th bit is CRUISE_ACTIVE
- int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
+ int cruise_engaged = GET_BYTE(to_push, 0) & 0x20U;
if (!cruise_engaged) {
controls_allowed = 0;
}
@@ -98,7 +98,7 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
// sample gas pedal
if (!gas_interceptor_detected) {
- gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0;
+ gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1U) == 0U;
}
}
@@ -106,9 +106,9 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
if (addr == 0xaa) {
int speed = 0;
// sum 4 wheel speeds
- for (int i=0; i<8; i+=2) {
- int next_byte = i + 1; // hack to deal with misra 10.8
- speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte) - 0x1a6f;
+ for (uint8_t i=0U; i<8U; i+=2U) {
+ int wheel_speed = (GET_BYTE(to_push, i) << 8U) + GET_BYTE(to_push, (i+1U));
+ speed += wheel_speed - 0x1a6f;
}
vehicle_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD;
}
@@ -116,7 +116,7 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
if ((addr == 0x224) || (addr == 0x226)) {
int byte = (addr == 0x224) ? 0 : 4;
- brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
+ brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1U) != 0U;
}
// sample gas interceptor
@@ -176,8 +176,8 @@ static int toyota_tx_hook(CANPacket_t *to_send) {
// only sent to prevent dash errors, no actuation is accepted
if (addr == 0x191) {
// check the STEER_REQUEST, STEER_REQUEST_2, and STEER_ANGLE_CMD signals
- bool lta_request = (GET_BYTE(to_send, 0) & 1) != 0;
- bool lta_request2 = ((GET_BYTE(to_send, 3) >> 1) & 1) != 0;
+ bool lta_request = (GET_BYTE(to_send, 0) & 1U) != 0U;
+ bool lta_request2 = ((GET_BYTE(to_send, 3) >> 1) & 1U) != 0U;
int lta_angle = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);
lta_angle = to_signed(lta_angle, 16);
diff --git a/panda/board/safety/safety_volkswagen.h b/panda/board/safety/safety_volkswagen.h
index 42c6cac2f..8666c8807 100644
--- a/panda/board/safety/safety_volkswagen.h
+++ b/panda/board/safety/safety_volkswagen.h
@@ -145,7 +145,7 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &volkswagen_mqb_rx_checks,
volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
// Update in-motion state by sampling front wheel speeds
@@ -163,8 +163,8 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) {
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
if (addr == MSG_LH_EPS_03) {
- int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
- int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
+ int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1FU) << 8);
+ int sign = (GET_BYTE(to_push, 6) & 0x80U) >> 7;
if (sign == 1) {
torque_driver_new *= -1;
}
@@ -174,7 +174,7 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) {
// Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
// Signal: TSK_06.TSK_Status
if (addr == MSG_TSK_06) {
- int acc_status = (GET_BYTE(to_push, 3) & 0x7);
+ int acc_status = (GET_BYTE(to_push, 3) & 0x7U);
int cruise_engaged = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = 1;
@@ -187,12 +187,12 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) {
// Signal: Motor_20.MO_Fahrpedalrohwert_01
if (addr == MSG_MOTOR_20) {
- gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0;
+ gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFFU) != 0U;
}
// Signal: ESP_05.ESP_Fahrer_bremst
if (addr == MSG_ESP_05) {
- brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
+ brake_pressed = (GET_BYTE(to_push, 3) & 0x4U) >> 2;
}
generic_rx_checks((addr == MSG_HCA_01));
@@ -205,13 +205,13 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &volkswagen_pq_rx_checks,
volkswagen_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter);
- if (valid && (GET_BUS(to_push) == 0)) {
+ if (valid && (GET_BUS(to_push) == 0U)) {
int addr = GET_ADDR(to_push);
// Update in-motion state from speed value.
// Signal: Bremse_1.Geschwindigkeit_neu__Bremse_1_
if (addr == MSG_BREMSE_1) {
- int speed = ((GET_BYTE(to_push, 2) & 0xFE) >> 1) | (GET_BYTE(to_push, 3) << 7);
+ int speed = ((GET_BYTE(to_push, 2) & 0xFEU) >> 1) | (GET_BYTE(to_push, 3) << 7);
// DBC speed scale 0.01: 0.3m/s = 108.
vehicle_moving = speed > 108;
}
@@ -220,8 +220,8 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
// Signal: Lenkhilfe_3.LH3_LM (absolute torque)
// Signal: Lenkhilfe_3.LH3_LMSign (direction)
if (addr == MSG_LENKHILFE_3) {
- int torque_driver_new = GET_BYTE(to_push, 2) | ((GET_BYTE(to_push, 3) & 0x3) << 8);
- int sign = (GET_BYTE(to_push, 3) & 0x4) >> 2;
+ int torque_driver_new = GET_BYTE(to_push, 2) | ((GET_BYTE(to_push, 3) & 0x3U) << 8);
+ int sign = (GET_BYTE(to_push, 3) & 0x4U) >> 2;
if (sign == 1) {
torque_driver_new *= -1;
}
@@ -231,7 +231,7 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
// Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
// Signal: Motor_2.GRA_Status
if (addr == MSG_MOTOR_2) {
- int acc_status = (GET_BYTE(to_push, 2) & 0xC0) >> 6;
+ int acc_status = (GET_BYTE(to_push, 2) & 0xC0U) >> 6;
int cruise_engaged = ((acc_status == 1) || (acc_status == 2)) ? 1 : 0;
if (cruise_engaged && !cruise_engaged_prev) {
controls_allowed = 1;
@@ -249,7 +249,7 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
// Signal: Motor_2.Bremslichtschalter
if (addr == MSG_MOTOR_2) {
- brake_pressed = (GET_BYTE(to_push, 2) & 0x1);
+ brake_pressed = (GET_BYTE(to_push, 2) & 0x1U);
}
generic_rx_checks((addr == MSG_HCA_1));
@@ -309,8 +309,8 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send) {
// Signal: HCA_01.Assist_Torque (absolute torque)
// Signal: HCA_01.Assist_VZ (direction)
if (addr == MSG_HCA_01) {
- int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3F) << 8);
- int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7;
+ int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3FU) << 8);
+ int sign = (GET_BYTE(to_send, 3) & 0x80U) >> 7;
if (sign == 1) {
desired_torque *= -1;
}
@@ -324,7 +324,7 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send) {
// This avoids unintended engagements while still allowing resume spam
if ((addr == MSG_GRA_ACC_01) && !controls_allowed) {
// disallow resume and set: bits 16 and 19
- if ((GET_BYTE(to_send, 2) & 0x9) != 0) {
+ if ((GET_BYTE(to_send, 2) & 0x9U) != 0U) {
tx = 0;
}
}
@@ -345,9 +345,9 @@ static int volkswagen_pq_tx_hook(CANPacket_t *to_send) {
// Signal: HCA_1.LM_Offset (absolute torque)
// Signal: HCA_1.LM_Offsign (direction)
if (addr == MSG_HCA_1) {
- int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x7F) << 8);
+ int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x7FU) << 8);
desired_torque = desired_torque / 32; // DBC scale from PQ network to centi-Nm
- int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7;
+ int sign = (GET_BYTE(to_send, 3) & 0x80U) >> 7;
if (sign == 1) {
desired_torque *= -1;
}
@@ -361,7 +361,7 @@ static int volkswagen_pq_tx_hook(CANPacket_t *to_send) {
// This avoids unintended engagements while still allowing resume spam
if ((addr == MSG_GRA_NEU) && !controls_allowed) {
// disallow resume and set: bits 16 and 17
- if ((GET_BYTE(to_send, 2) & 0x3) != 0) {
+ if ((GET_BYTE(to_send, 2) & 0x3U) != 0U) {
tx = 0;
}
}
diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h
index d5705b482..11438a486 100644
--- a/panda/board/safety_declarations.h
+++ b/panda/board/safety_declarations.h
@@ -1,7 +1,7 @@
-#define GET_BIT(msg, b) (((msg)->data[(int)((b) / 8)] >> ((b) % 8)) & 0x1)
+#define GET_BIT(msg, b) (((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U)
#define GET_BYTE(msg, b) ((msg)->data[(b)])
-#define GET_BYTES_04(msg) ((msg)->data[0] | ((msg)->data[1] << 8) | ((msg)->data[2] << 16) | ((msg)->data[3] << 24))
-#define GET_BYTES_48(msg) ((msg)->data[4] | ((msg)->data[5] << 8) | ((msg)->data[6] << 16) | ((msg)->data[7] << 24))
+#define GET_BYTES_04(msg) ((msg)->data[0] | ((msg)->data[1] << 8U) | ((msg)->data[2] << 16U) | ((msg)->data[3] << 24U))
+#define GET_BYTES_48(msg) ((msg)->data[4] | ((msg)->data[5] << 8U) | ((msg)->data[6] << 16U) | ((msg)->data[7] << 24U))
#define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask))
const int MAX_WRONG_COUNTERS = 5;
diff --git a/panda/board/stm32h7/clock.h b/panda/board/stm32h7/clock.h
index 627fedee5..10febe15a 100644
--- a/panda/board/stm32h7/clock.h
+++ b/panda/board/stm32h7/clock.h
@@ -10,6 +10,9 @@ void clock_init(void) {
// enable external oscillator HSE
register_set_bits(&(RCC->CR), RCC_CR_HSEON);
while ((RCC->CR & RCC_CR_HSERDY) == 0);
+ // enable internal HSI48 for USB FS kernel
+ register_set_bits(&(RCC->CR), RCC_CR_HSI48ON);
+ while ((RCC->CR & RCC_CR_HSI48RDY) == 0);
// Specify the frequency source for PLL1, divider for DIVM1, DIVM2, DIVM3 : HSE, 5, 5, 5
register_set(&(RCC->PLLCKSELR), RCC_PLLCKSELR_PLLSRC_HSE | RCC_PLLCKSELR_DIVM1_0 | RCC_PLLCKSELR_DIVM1_2 | RCC_PLLCKSELR_DIVM2_0 | RCC_PLLCKSELR_DIVM2_2 | RCC_PLLCKSELR_DIVM3_0 | RCC_PLLCKSELR_DIVM3_2, 0x3F3F3F3U);
@@ -23,16 +26,6 @@ void clock_init(void) {
while((RCC->CR & RCC_CR_PLL1RDY) == 0);
// *** PLL1 end ***
- // *** PLL3 start ***
- // Specify multiplier N and dividers P, Q, R for PLL1 : 48, 2, 5, 2 (PLL3Q 48Mhz for USB FS)
- register_set(&(RCC->PLL3DIVR), 0x104022FU, 0x7F7FFFFFU);
- // Specify the input and output frequency ranges, enable dividers for PLL1
- register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLL3RGE_2 | RCC_PLLCFGR_DIVP3EN | RCC_PLLCFGR_DIVQ3EN | RCC_PLLCFGR_DIVR3EN, 0x1C00C00U);
- // Enable PLL1
- register_set_bits(&(RCC->CR), RCC_CR_PLL3ON);
- while((RCC->CR & RCC_CR_PLL3RDY) == 0);
- // *** PLL1 end ***
-
//////////////OTHER CLOCKS////////////////////
// RCC HCLK Clock Source / RCC APB3 Clock Source / RCC SYS Clock Source
register_set(&(RCC->D1CFGR), RCC_D1CFGR_HPRE_DIV2 | RCC_D1CFGR_D1PPRE_DIV2 | RCC_D1CFGR_D1CPRE_DIV1, 0xF7FU);
@@ -49,8 +42,8 @@ void clock_init(void) {
register_set_bits(&(RCC->AHB4ENR), RCC_APB4ENR_SYSCFGEN);
//////////////END OTHER CLOCKS////////////////////
- // Configure clock source for USB (PLL3Q at 48Mhz)
- register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_1, RCC_D2CCIP2R_USBSEL);
+ // Configure clock source for USB (HSI at 48Mhz)
+ register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_1 | RCC_D2CCIP2R_USBSEL_0, RCC_D2CCIP2R_USBSEL);
// Configure clock source for FDCAN (PLL1Q at 80Mhz)
register_set(&(RCC->D2CCIP1R), RCC_D2CCIP1R_FDCANSEL_0, RCC_D2CCIP1R_FDCANSEL);
// Configure clock source for ADC1,2,3 (per_ck(currently HSE))
diff --git a/panda/board/usb_protocol.h b/panda/board/usb_protocol.h
index 0e50aa7a0..a20b6c1b4 100644
--- a/panda/board/usb_protocol.h
+++ b/panda/board/usb_protocol.h
@@ -1,39 +1,39 @@
typedef struct {
- uint8_t ptr;
- uint8_t tail_size;
+ int ptr;
+ int tail_size;
uint8_t data[72];
uint8_t counter;
} usb_asm_buffer;
usb_asm_buffer ep1_buffer = {.ptr = 0, .tail_size = 0, .counter = 0};
-uint32_t total_rx_size = 0;
+int total_rx_size = 0;
int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
- uint8_t pos = 1;
+ int pos = 1;
uint8_t *usbdata8 = (uint8_t *)usbdata;
usbdata8[0] = ep1_buffer.counter;
// Send tail of previous message if it is in buffer
if (ep1_buffer.ptr > 0) {
- if (ep1_buffer.ptr <= 63U) {
+ if (ep1_buffer.ptr <= 63) {
(void)memcpy(&usbdata8[pos], ep1_buffer.data, ep1_buffer.ptr);
pos += ep1_buffer.ptr;
ep1_buffer.ptr = 0;
} else {
- (void)memcpy(&usbdata8[pos], ep1_buffer.data, 63U);
- ep1_buffer.ptr = ep1_buffer.ptr - 63U;
- (void)memcpy(ep1_buffer.data, &ep1_buffer.data[63U], ep1_buffer.ptr);
- pos += 63U;
+ (void)memcpy(&usbdata8[pos], ep1_buffer.data, 63);
+ ep1_buffer.ptr = ep1_buffer.ptr - 63;
+ (void)memcpy(ep1_buffer.data, &ep1_buffer.data[63], ep1_buffer.ptr);
+ pos += 63;
}
}
if (total_rx_size > MAX_EP1_CHUNK_PER_BULK_TRANSFER) {
total_rx_size = 0;
- ep1_buffer.counter = 0;
+ ep1_buffer.counter = 0U;
} else {
CANPacket_t can_packet;
while ((pos < len) && can_pop(&can_rx_q, &can_packet)) {
- uint8_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[can_packet.data_len_code];
+ int pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[can_packet.data_len_code];
if ((pos + pckt_len) <= len) {
(void)memcpy(&usbdata8[pos], &can_packet, pckt_len);
pos += pckt_len;
@@ -50,7 +50,7 @@ int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
total_rx_size += pos;
}
if (pos != len) {
- ep1_buffer.counter = 0;
+ ep1_buffer.counter = 0U;
total_rx_size = 0;
}
if (pos <= 1) { pos = 0; }
@@ -64,17 +64,17 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
UNUSED(hardwired);
uint8_t *usbdata8 = (uint8_t *)usbdata;
// Got first packet from a stream, resetting buffer and counter
- if (usbdata8[0] == 0) {
- ep3_buffer.counter = 0;
+ if (usbdata8[0] == 0U) {
+ ep3_buffer.counter = 0U;
ep3_buffer.ptr = 0;
ep3_buffer.tail_size = 0;
}
// Assembling can message with data from buffer
if (usbdata8[0] == ep3_buffer.counter) {
- uint8_t pos = 1;
+ int pos = 1;
ep3_buffer.counter++;
if (ep3_buffer.ptr != 0) {
- if (ep3_buffer.tail_size <= 63U) {
+ if (ep3_buffer.tail_size <= 63) {
CANPacket_t to_push;
(void)memcpy(&ep3_buffer.data[ep3_buffer.ptr], &usbdata8[pos], ep3_buffer.tail_size);
(void)memcpy(&to_push, ep3_buffer.data, ep3_buffer.ptr + ep3_buffer.tail_size);
@@ -84,15 +84,15 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
ep3_buffer.tail_size = 0;
} else {
(void)memcpy(&ep3_buffer.data[ep3_buffer.ptr], &usbdata8[pos], len - pos);
- ep3_buffer.tail_size -= 63U;
- ep3_buffer.ptr += 63U;
- pos += 63U;
+ ep3_buffer.tail_size -= 63;
+ ep3_buffer.ptr += 63;
+ pos += 63;
}
}
while (pos < len) {
- uint8_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(usbdata8[pos] >> 4U)];
- if ((pos + pckt_len) <= (uint8_t)len) {
+ int pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(usbdata8[pos] >> 4U)];
+ if ((pos + pckt_len) <= len) {
CANPacket_t to_push;
(void)memcpy(&to_push, &usbdata8[pos], pckt_len);
can_send(&to_push, to_push.bus, false);
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index e491f3104..87c811fea 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -193,7 +193,7 @@ class Panda(object):
SAFETY_ALLOUTPUT = 17
SAFETY_GM_ASCM = 18
SAFETY_NOOUTPUT = 19
- SAFETY_HONDA_BOSCH_HARNESS = 20
+ SAFETY_HONDA_BOSCH = 20
SAFETY_VOLKSWAGEN_PQ = 21
SAFETY_SUBARU_LEGACY = 22
SAFETY_HYUNDAI_LEGACY = 23
diff --git a/release/files_common b/release/files_common
index aeae351df..3fc354396 100644
--- a/release/files_common
+++ b/release/files_common
@@ -36,7 +36,6 @@ common/filter_simple.py
common/stat_live.py
common/spinner.py
common/text_window.py
-common/cython_hacks.py
common/SConscript
common/kalman/.gitignore
@@ -306,6 +305,8 @@ selfdrive/loggerd/omx_encoder.h
selfdrive/loggerd/logger.cc
selfdrive/loggerd/logger.h
selfdrive/loggerd/loggerd.cc
+selfdrive/loggerd/loggerd.h
+selfdrive/loggerd/main.cc
selfdrive/loggerd/bootlog.cc
selfdrive/loggerd/raw_logger.cc
selfdrive/loggerd/raw_logger.h
@@ -366,8 +367,6 @@ selfdrive/camerad/snapshot/*
selfdrive/camerad/include/*
selfdrive/camerad/cameras/camera_common.h
selfdrive/camerad/cameras/camera_common.cc
-selfdrive/camerad/cameras/camera_frame_stream.cc
-selfdrive/camerad/cameras/camera_frame_stream.h
selfdrive/camerad/cameras/camera_qcom.cc
selfdrive/camerad/cameras/camera_qcom.h
selfdrive/camerad/cameras/camera_replay.cc
@@ -444,9 +443,6 @@ selfdrive/assets/training/*
third_party/SConscript
-third_party/nanovg/*.c
-third_party/nanovg/*.h
-
third_party/libgralloc/**
third_party/linux/**
third_party/opencl/**
diff --git a/release/files_tici b/release/files_tici
index 29c8353b8..59cc41918 100644
--- a/release/files_tici
+++ b/release/files_tici
@@ -4,7 +4,6 @@ selfdrive/timezoned.py
selfdrive/assets/navigation/*
selfdrive/assets/training_wide/*
-selfdrive/assets/sounds_tici/*
selfdrive/camerad/cameras/camera_qcom2.cc
selfdrive/camerad/cameras/camera_qcom2.h
diff --git a/release/verify.sh b/release/verify.sh
new file mode 100755
index 000000000..2ebd50a29
--- /dev/null
+++ b/release/verify.sh
@@ -0,0 +1,16 @@
+#!/bin/bash
+
+set -e
+
+RED="\033[0;31m"
+GREEN="\033[0;32m"
+CLEAR="\033[0m"
+
+BRANCHES="devel dashcam dashcam3 release2 release3"
+for b in $BRANCHES; do
+ if git diff --quiet origin/$b origin/$b-staging && [ "$(git rev-parse origin/$b)" = "$(git rev-parse origin/$b-staging)" ]; then
+ printf "%-10s $GREEN ok $CLEAR\n" "$b"
+ else
+ printf "%-10s $RED mismatch $CLEAR\n" "$b"
+ fi
+done
diff --git a/selfdrive/assets/offroad/fcc.html b/selfdrive/assets/offroad/fcc.html
index 7f03a7204..793bea533 100644
--- a/selfdrive/assets/offroad/fcc.html
+++ b/selfdrive/assets/offroad/fcc.html
@@ -11,7 +11,7 @@
FCC ID: 2AOHHTURBOXSOMD845
Quectel/EG25-G
- FCC ID: XMR201903EG25GM
+ FCC ID: XMR201903EG25G
This device complies with Part 15 of the FCC Rules.
Operation is subject to the following two conditions:
diff --git a/selfdrive/assets/sounds/disengage.wav b/selfdrive/assets/sounds/disengage.wav
new file mode 100644
index 000000000..ba583c41f
Binary files /dev/null and b/selfdrive/assets/sounds/disengage.wav differ
diff --git a/selfdrive/assets/sounds/disengaged.wav b/selfdrive/assets/sounds/disengaged.wav
deleted file mode 100644
index 3aa8e51e6..000000000
Binary files a/selfdrive/assets/sounds/disengaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/engage.wav b/selfdrive/assets/sounds/engage.wav
new file mode 100644
index 000000000..41e9b2d58
Binary files /dev/null and b/selfdrive/assets/sounds/engage.wav differ
diff --git a/selfdrive/assets/sounds/engaged.wav b/selfdrive/assets/sounds/engaged.wav
deleted file mode 100644
index 1451f937f..000000000
Binary files a/selfdrive/assets/sounds/engaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/error.wav b/selfdrive/assets/sounds/error.wav
deleted file mode 100644
index e805181ae..000000000
Binary files a/selfdrive/assets/sounds/error.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/prompt.wav b/selfdrive/assets/sounds/prompt.wav
new file mode 100644
index 000000000..420e9fabe
Binary files /dev/null and b/selfdrive/assets/sounds/prompt.wav differ
diff --git a/selfdrive/assets/sounds/prompt_distracted.wav b/selfdrive/assets/sounds/prompt_distracted.wav
new file mode 100644
index 000000000..c3d4475ca
Binary files /dev/null and b/selfdrive/assets/sounds/prompt_distracted.wav differ
diff --git a/selfdrive/assets/sounds/refuse.wav b/selfdrive/assets/sounds/refuse.wav
new file mode 100644
index 000000000..0e80f7d12
Binary files /dev/null and b/selfdrive/assets/sounds/refuse.wav differ
diff --git a/selfdrive/assets/sounds/warning_1.wav b/selfdrive/assets/sounds/warning_1.wav
deleted file mode 100644
index 43ca74cc5..000000000
Binary files a/selfdrive/assets/sounds/warning_1.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav
deleted file mode 100644
index 4909f1198..000000000
Binary files a/selfdrive/assets/sounds/warning_2.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/warning_repeat.wav b/selfdrive/assets/sounds/warning_immediate.wav
similarity index 100%
rename from selfdrive/assets/sounds/warning_repeat.wav
rename to selfdrive/assets/sounds/warning_immediate.wav
diff --git a/selfdrive/assets/sounds/warning_soft.wav b/selfdrive/assets/sounds/warning_soft.wav
new file mode 100644
index 000000000..261c7e137
Binary files /dev/null and b/selfdrive/assets/sounds/warning_soft.wav differ
diff --git a/selfdrive/assets/sounds_tici/disengaged.wav b/selfdrive/assets/sounds_tici/disengaged.wav
deleted file mode 100644
index 182adffa2..000000000
Binary files a/selfdrive/assets/sounds_tici/disengaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/engaged.wav b/selfdrive/assets/sounds_tici/engaged.wav
deleted file mode 100644
index 5c4c8b4d1..000000000
Binary files a/selfdrive/assets/sounds_tici/engaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/error.wav b/selfdrive/assets/sounds_tici/error.wav
deleted file mode 100644
index c4e280e9a..000000000
Binary files a/selfdrive/assets/sounds_tici/error.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/warning_1.wav b/selfdrive/assets/sounds_tici/warning_1.wav
deleted file mode 100644
index c937b3be0..000000000
Binary files a/selfdrive/assets/sounds_tici/warning_1.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/warning_2.wav b/selfdrive/assets/sounds_tici/warning_2.wav
deleted file mode 100644
index 49188db88..000000000
Binary files a/selfdrive/assets/sounds_tici/warning_2.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/warning_repeat.wav b/selfdrive/assets/sounds_tici/warning_repeat.wav
deleted file mode 100644
index bb3bfd0d5..000000000
Binary files a/selfdrive/assets/sounds_tici/warning_repeat.wav and /dev/null differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 779045bf9..d19692661 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -30,7 +30,7 @@ from selfdrive.hardware import HARDWARE, PC
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.swaglog import cloudlog, SWAGLOG_DIR
-from selfdrive.version import version, get_version, get_git_remote, get_git_branch, get_git_commit
+from selfdrive.version import get_version, get_origin, get_short_branch, get_commit
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
@@ -176,17 +176,19 @@ def getMessage(service=None, timeout=1000):
def getVersion():
return {
"version": get_version(),
- "remote": get_git_remote(),
- "branch": get_git_branch(),
- "commit": get_git_commit(),
+ "remote": get_origin(),
+ "branch": get_short_branch(),
+ "commit": get_commit(),
}
@dispatcher.add_method
-def setNavDestination(latitude=0, longitude=0):
+def setNavDestination(latitude=0, longitude=0, place_name=None, place_details=None):
destination = {
"latitude": latitude,
"longitude": longitude,
+ "place_name": place_name,
+ "place_details": place_details,
}
Params().put("NavDestination", json.dumps(destination))
@@ -551,7 +553,7 @@ def main():
except socket.timeout:
try:
r = requests.get("http://api.commadotai.com/v1/me", allow_redirects=False,
- headers={"User-Agent": f"openpilot-{version}"}, timeout=15.0)
+ headers={"User-Agent": f"openpilot-{get_version()}"}, timeout=15.0)
if r.status_code == 302 and r.headers['Location'].startswith("http://u.web2go.com"):
params.put_bool("PrimeRedirected", True)
except Exception:
diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py
index 7bb717e07..fa95eacd8 100755
--- a/selfdrive/athena/manage_athenad.py
+++ b/selfdrive/athena/manage_athenad.py
@@ -6,7 +6,7 @@ from multiprocessing import Process
from common.params import Params
from selfdrive.manager.process import launcher
from selfdrive.swaglog import cloudlog
-from selfdrive.version import version, dirty
+from selfdrive.version import get_version, get_dirty
ATHENA_MGR_PID_PARAM = "AthenadPid"
@@ -14,7 +14,7 @@ ATHENA_MGR_PID_PARAM = "AthenadPid"
def main():
params = Params()
dongle_id = params.get("DongleId").decode('utf-8')
- cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty)
+ cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=get_dirty())
try:
while 1:
diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py
index 39dfb3c23..e06bae506 100755
--- a/selfdrive/athena/registration.py
+++ b/selfdrive/athena/registration.py
@@ -1,14 +1,13 @@
-#!/usr/bin/env python3
-import os
+#/!/usr/bin/env python3
import time
import json
import jwt
+from pathlib import Path
from datetime import datetime, timedelta
from common.api import api_get
from common.params import Params
from common.spinner import Spinner
-from common.file_helpers import mkdirs_exists_ok
from common.basedir import PERSIST
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.hardware import HARDWARE
@@ -27,19 +26,11 @@ def register(show_spinner=False) -> str:
dongle_id = params.get("DongleId", encoding='utf8')
needs_registration = None in (IMEI, HardwareSerial, dongle_id)
- # create a key for auth
- # your private key is kept on your device persist partition and never sent to our servers
- # do not erase your persist partition
- if not os.path.isfile(PERSIST+"/comma/id_rsa.pub"):
- needs_registration = True
- cloudlog.warning("generating your personal RSA key")
- mkdirs_exists_ok(PERSIST+"/comma")
- assert os.system("openssl genrsa -out "+PERSIST+"/comma/id_rsa.tmp 2048") == 0
- assert os.system("openssl rsa -in "+PERSIST+"/comma/id_rsa.tmp -pubout -out "+PERSIST+"/comma/id_rsa.tmp.pub") == 0
- os.rename(PERSIST+"/comma/id_rsa.tmp", PERSIST+"/comma/id_rsa")
- os.rename(PERSIST+"/comma/id_rsa.tmp.pub", PERSIST+"/comma/id_rsa.pub")
-
- if needs_registration:
+ pubkey = Path(PERSIST+"/comma/id_rsa.pub")
+ if not pubkey.is_file():
+ dongle_id = UNREGISTERED_DONGLE_ID
+ cloudlog.warning(f"missing public key: {pubkey}")
+ elif needs_registration:
if show_spinner:
spinner = Spinner()
spinner.update("registering device")
diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore
index 1f653bde8..e8daa2ef2 100644
--- a/selfdrive/boardd/.gitignore
+++ b/selfdrive/boardd/.gitignore
@@ -1,2 +1,3 @@
boardd
boardd_api_impl.cpp
+tests/test_boardd_usbprotocol
diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript
index f2a1f3f7b..07ded56e1 100644
--- a/selfdrive/boardd/SConscript
+++ b/selfdrive/boardd/SConscript
@@ -1,6 +1,9 @@
Import('env', 'envCython', 'common', 'cereal', 'messaging')
-env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
+libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
+env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs)
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
+if GetOption('test'):
+ env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc'], LIBS=libs)
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index ce73662e4..d09738a70 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -62,7 +62,7 @@ std::atomic pigeon_active(false);
ExitHandler do_exit;
-std::string get_time_str(const struct tm &time) {
+static std::string get_time_str(const struct tm &time) {
char s[30] = {'\0'};
std::strftime(s, std::size(s), "%Y-%m-%d %H:%M:%S", &time);
return s;
@@ -70,11 +70,43 @@ std::string get_time_str(const struct tm &time) {
bool check_all_connected(const std::vector &pandas) {
for (const auto& panda : pandas) {
- if (!panda->connected) return false;
+ if (!panda->connected) {
+ do_exit = true;
+ return false;
+ }
}
return true;
}
+enum class SyncTimeDir { TO_PANDA, FROM_PANDA };
+
+void sync_time(Panda *panda, SyncTimeDir dir) {
+ if (!panda->has_rtc) return;
+
+ setenv("TZ", "UTC", 1);
+ struct tm sys_time = util::get_time();
+ struct tm rtc_time = panda->get_rtc();
+
+ if (dir == SyncTimeDir::TO_PANDA) {
+ if (util::time_valid(sys_time)) {
+ // Write time to RTC if it looks reasonable
+ double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
+ if (std::abs(seconds) > 1.1) {
+ panda->set_rtc(sys_time);
+ LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s",
+ seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
+ }
+ }
+ } else if (dir == SyncTimeDir::FROM_PANDA) {
+ if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) {
+ const struct timeval tv = {mktime(&rtc_time), 0};
+ settimeofday(&tv, 0);
+ LOGE("System time wrong, setting from RTC. System: %s RTC: %s",
+ get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
+ }
+ }
+}
+
bool safety_setter_thread(std::vector pandas) {
LOGD("Starting safety setter thread");
@@ -168,40 +200,22 @@ Panda *usb_connect(std::string serial="", uint32_t index=0) {
std::call_once(connected_once, &Panda::set_usb_power_mode, panda, cereal::PeripheralState::UsbPowerMode::CDP);
#endif
- if (panda->has_rtc) {
- setenv("TZ","UTC",1);
- struct tm sys_time = util::get_time();
- struct tm rtc_time = panda->get_rtc();
-
- if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) {
- LOGE("System time wrong, setting from RTC. System: %s RTC: %s",
- get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
- const struct timeval tv = {mktime(&rtc_time), 0};
- settimeofday(&tv, 0);
- }
- }
-
+ sync_time(panda.get(), SyncTimeDir::FROM_PANDA);
return panda.release();
}
void can_send_thread(std::vector pandas, bool fake_send) {
- LOGD("start send thread");
+ util::set_thread_name("boardd_can_send");
AlignedBuffer aligned_buf;
- Context * context = Context::create();
- SubSocket * subscriber = SubSocket::create(context, "sendcan");
+ std::unique_ptr context(Context::create());
+ std::unique_ptr subscriber(SubSocket::create(context.get(), "sendcan"));
assert(subscriber != NULL);
subscriber->setTimeout(100);
// run as fast as messages come in
- while (!do_exit) {
- if (!check_all_connected(pandas)) {
- do_exit = true;
- break;
- }
-
- Message * msg = subscriber->receive();
-
+ while (!do_exit && check_all_connected(pandas)) {
+ std::unique_ptr msg(subscriber->receive());
if (!msg) {
if (errno == EINTR) {
do_exit = true;
@@ -209,27 +223,20 @@ void can_send_thread(std::vector pandas, bool fake_send) {
continue;
}
- capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg));
+ capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot();
//Dont send if older than 1 second
- if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
- if (!fake_send) {
- for (const auto& panda : pandas) {
- panda->can_send(event.getSendcan());
- }
+ if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
+ for (const auto& panda : pandas) {
+ panda->can_send(event.getSendcan());
}
}
-
- delete msg;
}
-
- delete subscriber;
- delete context;
}
void can_recv_thread(std::vector pandas) {
- LOGD("start recv thread");
+ util::set_thread_name("boardd_can_recv");
// can = 8006
PubMaster pm({"can"});
@@ -239,12 +246,7 @@ void can_recv_thread(std::vector pandas) {
uint64_t next_frame_time = nanos_since_boot() + dt;
std::vector raw_can_data;
- while (!do_exit) {
- if (!check_all_connected(pandas)){
- do_exit = true;
- break;
- }
-
+ while (!do_exit && check_all_connected(pandas)) {
bool comms_healthy = true;
raw_can_data.clear();
for (const auto& panda : pandas) {
@@ -315,7 +317,7 @@ bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool s
for (uint32_t i=0; i &pandas, bool s
}
#endif
- // TODO: do we still need this?
if (!panda->comms_healthy) {
evt.setValid(false);
}
@@ -407,6 +408,8 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
}
void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofing_started) {
+ util::set_thread_name("boardd_panda_state");
+
Params params;
Panda *peripheral_panda = pandas[0];
bool ignition_last = false;
@@ -415,15 +418,12 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin
LOGD("start panda state thread");
// run at 2hz
- while (!do_exit) {
- if(!check_all_connected(pandas)) {
- do_exit = true;
- break;
- }
-
+ while (!do_exit && check_all_connected(pandas)) {
+ // send out peripheralState
send_peripheral_state(pm, peripheral_panda);
ignition = send_panda_states(pm, pandas, spoofing_started);
+ // TODO: make this check fast, currently takes 16ms
// check if we have new pandas and are offroad
if (!ignition && (pandas.size() != Panda::list().size())) {
LOGW("Reconnecting to changed amount of pandas!");
@@ -431,7 +431,7 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin
break;
}
- // clear VIN, CarParams, and set new safety on car start
+ // clear ignition-based params and set new safety on car start
if (ignition && !ignition_last) {
params.clearAll(CLEAR_ON_IGNITION_ON);
if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
@@ -454,7 +454,8 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin
void peripheral_control_thread(Panda *panda) {
- LOGD("start peripheral control thread");
+ util::set_thread_name("boardd_peripheral_control");
+
SubMaster sm({"deviceState", "driverCameraState"});
uint64_t last_front_frame_t = 0;
@@ -525,21 +526,8 @@ void peripheral_control_thread(Panda *panda) {
}
// Write to rtc once per minute when no ignition present
- if ((panda->has_rtc) && !ignition && (cnt % 120 == 1)) {
- // Write time to RTC if it looks reasonable
- setenv("TZ","UTC",1);
- struct tm sys_time = util::get_time();
-
- if (util::time_valid(sys_time)) {
- struct tm rtc_time = panda->get_rtc();
- double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
-
- if (std::abs(seconds) > 1.1) {
- panda->set_rtc(sys_time);
- LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s",
- seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
- }
- }
+ if (!ignition && (cnt % 120 == 1)) {
+ sync_time(panda, SyncTimeDir::TO_PANDA);
}
}
}
@@ -552,10 +540,12 @@ static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) {
}
void pigeon_thread(Panda *panda) {
+ util::set_thread_name("boardd_pigeon");
+
PubMaster pm({"ubloxRaw"});
bool ignition_last = false;
- Pigeon *pigeon = Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda);
+ std::unique_ptr pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda));
std::unordered_map last_recv_time;
std::unordered_map cls_max_dt = {
@@ -623,8 +613,6 @@ void pigeon_thread(Panda *panda) {
// 10ms - 100 Hz
util::sleep_for(10);
}
-
- delete pigeon;
}
int main(int argc, char *argv[]) {
@@ -632,9 +620,9 @@ int main(int argc, char *argv[]) {
if (!Hardware::PC()) {
int err;
- err = set_realtime_priority(54);
+ err = util::set_realtime_priority(54);
assert(err == 0);
- err = set_core_affinity({Hardware::TICI() ? 4 : 3});
+ err = util::set_core_affinity({Hardware::TICI() ? 4 : 3});
assert(err == 0);
}
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 78b123f69..751f735ca 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -352,7 +352,7 @@ void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) {
usb_write(0xf9, bus, (speed * 10));
}
-uint8_t Panda::len_to_dlc(uint8_t len) {
+static uint8_t len_to_dlc(uint8_t len) {
if (len <= 8) {
return len;
}
@@ -363,114 +363,98 @@ uint8_t Panda::len_to_dlc(uint8_t len) {
}
}
+static void write_packet(uint8_t *dest, int *write_pos, const uint8_t *src, size_t size) {
+ for (int i = 0, &pos = *write_pos; i < size; ++i, ++pos) {
+ // Insert counter every 64 bytes (first byte of 64 bytes USB packet)
+ if (pos % USBPACKET_MAX_SIZE == 0) {
+ dest[pos] = pos / USBPACKET_MAX_SIZE;
+ pos++;
+ }
+ dest[pos] = src[i];
+ }
+}
+
+void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list,
+ std::function write_func) {
+ int32_t pos = 0;
+ uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
+
+ for (auto cmsg : can_data_list) {
+ // check if the message is intended for this panda
+ uint8_t bus = cmsg.getSrc();
+ if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) {
+ continue;
+ }
+ auto can_data = cmsg.getDat();
+ uint8_t data_len_code = len_to_dlc(can_data.size());
+ assert(can_data.size() <= ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8));
+ assert(can_data.size() == dlc_to_len[data_len_code]);
+
+ can_header header;
+ header.addr = cmsg.getAddress();
+ header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
+ header.data_len_code = data_len_code;
+ header.bus = bus - bus_offset;
+
+ write_packet(send_buf, &pos, (uint8_t *)&header, sizeof(can_header));
+ write_packet(send_buf, &pos, (uint8_t *)can_data.begin(), can_data.size());
+ if (pos >= USB_TX_SOFT_LIMIT) {
+ write_func(send_buf, pos);
+ pos = 0;
+ }
+ }
+
+ // send remaining packets
+ if (pos > 0) write_func(send_buf, pos);
+}
+
void Panda::can_send(capnp::List::Reader can_data_list) {
- if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) {
- send.resize(can_data_list.size() * CANPACKET_MAX_SIZE);
- }
-
- int msg_count = 0;
- while (msg_count < can_data_list.size()) {
- uint32_t pos = 0;
- while (pos < USB_TX_SOFT_LIMIT) {
- if (msg_count == can_data_list.size()) { break; }
- auto cmsg = can_data_list[msg_count];
-
- // check if the message is intended for this panda
- uint8_t bus = cmsg.getSrc();
- if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) {
- msg_count++;
- continue;
- }
- auto can_data = cmsg.getDat();
- uint8_t data_len_code = len_to_dlc(can_data.size());
- assert(can_data.size() <= (hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8);
- assert(can_data.size() == dlc_to_len[data_len_code]);
-
- can_header header;
- header.addr = cmsg.getAddress();
- header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
- header.data_len_code = data_len_code;
- header.bus = bus - bus_offset;
- memcpy(&send[pos], &header, CANPACKET_HEAD_SIZE);
- memcpy(&send[pos+CANPACKET_HEAD_SIZE], can_data.begin(), can_data.size());
-
- pos += CANPACKET_HEAD_SIZE + dlc_to_len[data_len_code];
- msg_count++;
- }
-
- if (pos > 0) { // Helps not to spam with ZLP
- // Counter needs to be inserted every 64 bytes (first byte of 64 bytes USB packet)
- uint8_t counter = 0;
- uint8_t to_write[USB_TX_SOFT_LIMIT+128];
- int ptr = 0;
- for (int i = 0; i < pos; i += 63) {
- to_write[ptr] = counter;
- int copy_size = ((pos - i) < 63) ? (pos - i) : 63;
- memcpy(&to_write[ptr+1], &(send.data()[i]) , copy_size);
- ptr += copy_size + 1;
- counter++;
- }
- usb_bulk_write(3, to_write, ptr, 5);
- }
- }
+ pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) {
+ usb_bulk_write(3, data, size, 5);
+ });
}
bool Panda::can_receive(std::vector& out_vec) {
uint8_t data[RECV_SIZE];
int recv = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE);
-
- // Not sure if this can happen
- if (recv < 0) recv = 0;
-
- if (recv == RECV_SIZE) {
- LOGW("Receive buffer full");
- }
-
if (!comms_healthy) {
return false;
}
+ if (recv == RECV_SIZE) {
+ LOGW("Panda receive buffer full");
+ }
- static uint8_t tail[CANPACKET_MAX_SIZE];
- uint8_t tail_size = 0;
- uint8_t counter = 0;
- for (int i = 0; i < recv; i += USBPACKET_MAX_SIZE) {
- // Check for counter every 64 bytes (length of USB packet)
- if (counter != data[i]) {
+ return (recv <= 0) ? true : unpack_can_buffer(data, recv, out_vec);
+}
+
+bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec) {
+ recv_buf.clear();
+ for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) {
+ if (data[i] != i / USBPACKET_MAX_SIZE) {
LOGE("CAN: MALFORMED USB RECV PACKET");
- break;
+ comms_healthy = false;
+ return false;
}
- counter++;
- uint8_t chunk_len = ((recv - i) > USBPACKET_MAX_SIZE) ? 63 : (recv - i - 1); // as 1 is always reserved for counter
- uint8_t chunk[USBPACKET_MAX_SIZE + CANPACKET_MAX_SIZE];
- memcpy(chunk, tail, tail_size);
- memcpy(&chunk[tail_size], &data[i+1], chunk_len);
- chunk_len += tail_size;
- tail_size = 0;
- uint8_t pos = 0;
- while (pos < chunk_len) {
- uint8_t data_len = dlc_to_len[(chunk[pos] >> 4)];
- uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len;
- if (pckt_len <= (chunk_len - pos)) {
- can_header header;
- memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE);
+ int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i));
+ recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]);
+ }
- can_frame &canData = out_vec.emplace_back();
- canData.busTime = 0;
- canData.address = header.addr;
- canData.src = header.bus + bus_offset;
+ int pos = 0;
+ while (pos < recv_buf.size()) {
+ can_header header;
+ memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE);
- if (header.rejected) { canData.src += CANPACKET_REJECTED; }
- if (header.returned) { canData.src += CANPACKET_RETURNED; }
- canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], data_len);
+ can_frame &canData = out_vec.emplace_back();
+ canData.busTime = 0;
+ canData.address = header.addr;
+ canData.src = header.bus + bus_offset;
+ if (header.rejected) { canData.src += CANPACKET_REJECTED; }
+ if (header.returned) { canData.src += CANPACKET_RETURNED; }
- pos += pckt_len;
- } else {
- // Keep partial CAN packet until next USB packet
- tail_size = (chunk_len - pos);
- memcpy(tail, &chunk[pos], tail_size);
- break;
- }
- }
+ const uint8_t data_len = dlc_to_len[header.data_len_code];
+ canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len);
+
+ pos += CANPACKET_HEAD_SIZE + data_len;
}
return true;
}
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 4be9454cf..fe6918948 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -3,6 +3,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -17,7 +18,7 @@
#define PANDA_BUS_CNT 4
#define RECV_SIZE (0x4000U)
#define USB_TX_SOFT_LIMIT (0x100U)
-#define USBPACKET_MAX_SIZE (0x40U)
+#define USBPACKET_MAX_SIZE (0x40)
#define CANPACKET_HEAD_SIZE 5U
#define CANPACKET_MAX_SIZE 72U
#define CANPACKET_REJECTED (0xC0U)
@@ -68,7 +69,7 @@ class Panda {
libusb_context *ctx = NULL;
libusb_device_handle *dev_handle = NULL;
std::mutex usb_lock;
- std::vector send;
+ std::vector recv_buf;
void handle_usb_issue(int err, const char func[]);
void cleanup();
@@ -110,7 +111,13 @@ class Panda {
void send_heartbeat();
void set_can_speed_kbps(uint16_t bus, uint16_t speed);
void set_data_speed_kbps(uint16_t bus, uint16_t speed);
- uint8_t len_to_dlc(uint8_t len);
void can_send(capnp::List::Reader can_data_list);
bool can_receive(std::vector& out_vec);
+
+protected:
+ // for unit tests
+ Panda(uint32_t bus_offset) : bus_offset(bus_offset) {}
+ void pack_can_buffer(const capnp::List::Reader &can_data_list,
+ std::function write_func);
+ bool unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec);
};
diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc
index b8735cc3b..912f4b03e 100644
--- a/selfdrive/boardd/pigeon.cc
+++ b/selfdrive/boardd/pigeon.cc
@@ -24,8 +24,8 @@ extern ExitHandler do_exit;
const std::string ack = "\xb5\x62\x05\x01\x02\x00";
const std::string nack = "\xb5\x62\x05\x00\x02\x00";
-const std::string sos_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00";
-const std::string sos_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00";
+const std::string sos_save_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00";
+const std::string sos_save_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00";
Pigeon * Pigeon::connect(Panda * p) {
PandaPigeon * pigeon = new PandaPigeon();
@@ -72,6 +72,25 @@ bool Pigeon::send_with_ack(const std::string &cmd) {
return wait_for_ack();
}
+sos_restore_response Pigeon::wait_for_backup_restore_status(int timeout_ms) {
+ std::string s;
+ const double start_t = millis_since_boot();
+ while (!do_exit) {
+ s += receive();
+
+ size_t position = s.find("\xb5\x62\x09\x14\x08\x00\x03");
+ if (position != std::string::npos && s.size() >= (position + 11)) {
+ return static_cast(s[position + 10]);
+ } else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) {
+ LOGE("No backup restore response from ublox");
+ return error;
+ }
+
+ util::sleep_for(1); // Allow other threads to be scheduled
+ }
+ return error;
+}
+
void Pigeon::init() {
for (int i = 0; i < 10; i++) {
if (do_exit) return;
@@ -118,6 +137,22 @@ void Pigeon::init() {
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue;
+ // check the backup restore status
+ send("\xB5\x62\x09\x14\x00\x00\x1D\x60"s);
+ sos_restore_response restore_status = wait_for_backup_restore_status();
+ switch(restore_status) {
+ case restored:
+ LOGW("almanac backup restored");
+ // clear the backup
+ send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s);
+ break;
+ case no_backup:
+ LOGW("no almanac backup found");
+ break;
+ default:
+ LOGE("failed to restore almanac backup, status: %d", restore_status);
+ }
+
auto time = util::get_time();
if (util::time_valid(time)) {
LOGW("Sending current time to ublox");
@@ -139,7 +174,7 @@ void Pigeon::stop() {
// Store almanac in flash
send("\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC"s);
- if (wait_for_ack(sos_ack, sos_nack)) {
+ if (wait_for_ack(sos_save_ack, sos_save_nack)) {
LOGW("Done storing almanac");
} else {
LOGE("Error storing almanac");
diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h
index 0a82bcdd9..c9ea4739d 100644
--- a/selfdrive/boardd/pigeon.h
+++ b/selfdrive/boardd/pigeon.h
@@ -7,6 +7,14 @@
#include "selfdrive/boardd/panda.h"
+enum sos_restore_response : int {
+ unknown = 0,
+ failed = 1,
+ restored = 2,
+ no_backup = 3,
+ error = -1
+};
+
class Pigeon {
public:
static Pigeon* connect(Panda * p);
@@ -18,6 +26,7 @@ class Pigeon {
bool wait_for_ack();
bool wait_for_ack(const std::string &ack, const std::string &nack, int timeout_ms = 1000);
bool send_with_ack(const std::string &cmd);
+ sos_restore_response wait_for_backup_restore_status(int timeout_ms = 1000);
virtual void set_baud(int baud) = 0;
virtual void send(const std::string &s) = 0;
virtual std::string receive() = 0;
diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript
index d018704d6..85bc756bc 100644
--- a/selfdrive/camerad/SConscript
+++ b/selfdrive/camerad/SConscript
@@ -1,4 +1,4 @@
-Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM', 'USE_FRAME_STREAM')
+Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM')
libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon]
@@ -18,15 +18,12 @@ else:
env.Append(CFLAGS = '-DWEBCAM')
env.Append(CPPPATH = ['/usr/include/opencv4', '/usr/local/include/opencv4'])
else:
- if USE_FRAME_STREAM:
- cameras = ['cameras/camera_frame_stream.cc']
- else:
- libs += ['avutil', 'avcodec', 'avformat', 'swscale', 'bz2', 'ssl', 'curl', 'crypto']
- # TODO: import replay_lib from root SConstruct
- cameras = ['cameras/camera_replay.cc',
- env.Object('camera-util', '#/selfdrive/ui/replay/util.cc'),
- env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc'),
- env.Object('camera-filereader', '#/selfdrive/ui/replay/filereader.cc')]
+ libs += ['avutil', 'avcodec', 'avformat', 'bz2', 'ssl', 'curl', 'crypto']
+ # TODO: import replay_lib from root SConstruct
+ cameras = ['cameras/camera_replay.cc',
+ env.Object('camera-util', '#/selfdrive/ui/replay/util.cc'),
+ env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc'),
+ env.Object('camera-filereader', '#/selfdrive/ui/replay/filereader.cc')]
if arch == "Darwin":
del libs[libs.index('OpenCL')]
diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc
index 4db6eaa12..6dcbf18c4 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -28,8 +28,6 @@
#include "selfdrive/camerad/cameras/camera_replay.h"
#endif
-const int YUV_COUNT = 100;
-
class Debayer {
public:
Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) {
@@ -109,7 +107,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height);
rgb_stride = vipc_server->get_buffer(rgb_type)->stride;
- vipc_server->create_buffers(yuv_type, YUV_COUNT, false, rgb_width, rgb_height);
+ vipc_server->create_buffers(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height);
if (ci->bayer) {
debayer = new Debayer(device_id, context, this, s);
@@ -353,7 +351,7 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
} else {
thread_name = "WideRoadCamera";
}
- set_thread_name(thread_name);
+ util::set_thread_name(thread_name);
uint32_t cnt = 0;
while (!do_exit) {
@@ -410,11 +408,11 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip));
}
-void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) {
+void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
int j = Hardware::TICI() ? 1 : 3;
if (cnt % j == 0) {
- sm->update(0);
- driver_cam_auto_exposure(c, *sm);
+ s->sm->update(0);
+ driver_cam_auto_exposure(c, *(s->sm));
}
MessageBuilder msg;
auto framed = msg.initEvent().initDriverCameraState();
@@ -423,5 +421,5 @@ void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c,
if (env_send_driver) {
framed.setImage(get_frame_image(&c->buf));
}
- pm->send("driverCameraState", msg);
+ s->pm->send("driverCameraState", msg);
}
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index e994fc220..9394f9075 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -14,6 +14,7 @@
#include "selfdrive/common/queue.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/visionimg.h"
+#include "selfdrive/hardware/hw.h"
#define CAMERA_ID_IMX298 0
#define CAMERA_ID_IMX179 1
@@ -26,7 +27,8 @@
#define CAMERA_ID_AR0231 8
#define CAMERA_ID_MAX 9
-#define UI_BUF_COUNT 4
+const int UI_BUF_COUNT = 4;
+const int YUV_BUFFER_COUNT = Hardware::EON() ? 100 : 40;
enum CameraType {
RoadCam = 0,
@@ -49,23 +51,6 @@ typedef struct CameraInfo {
bool hdr;
} CameraInfo;
-typedef struct LogCameraInfo {
- CameraType type;
- const char* filename;
- const char* frame_packet_name;
- const char* encode_idx_name;
- VisionStreamType stream_type;
- int frame_width, frame_height;
- int fps;
- int bitrate;
- bool is_h265;
- bool downscale;
- bool has_qcamera;
- bool trigger_rotate;
- bool enable;
- bool record;
-} LogCameraInfo;
-
typedef struct FrameMetadata {
uint32_t frame_id;
unsigned int frame_length;
@@ -138,7 +123,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
kj::Array get_frame_image(const CameraBuf *b);
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
-void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt);
+void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt);
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx);
void cameras_open(MultiCameraState *s);
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc
deleted file mode 100644
index 8e6c69e36..000000000
--- a/selfdrive/camerad/cameras/camera_frame_stream.cc
+++ /dev/null
@@ -1,92 +0,0 @@
-#include "selfdrive/camerad/cameras/camera_frame_stream.h"
-
-#include
-#include
-
-#include
-
-#include "cereal/messaging/messaging.h"
-#include "selfdrive/common/util.h"
-
-#define FRAME_WIDTH 1164
-#define FRAME_HEIGHT 874
-
-extern ExitHandler do_exit;
-
-namespace {
-
-// TODO: make this more generic
-CameraInfo cameras_supported[CAMERA_ID_MAX] = {
- [CAMERA_ID_IMX298] = {
- .frame_width = FRAME_WIDTH,
- .frame_height = FRAME_HEIGHT,
- .frame_stride = FRAME_WIDTH*3,
- .bayer = false,
- .bayer_flip = false,
- },
- [CAMERA_ID_OV8865] = {
- .frame_width = 1632,
- .frame_height = 1224,
- .frame_stride = 2040, // seems right
- .bayer = false,
- .bayer_flip = 3,
- .hdr = false
- },
-};
-
-void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
- assert(camera_id < std::size(cameras_supported));
- s->ci = cameras_supported[camera_id];
- assert(s->ci.frame_width != 0);
-
- s->camera_num = camera_id;
- s->fps = fps;
- s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
-}
-
-void run_frame_stream(CameraState &camera, const char* frame_pkt) {
- SubMaster sm({frame_pkt});
-
- size_t buf_idx = 0;
- while (!do_exit) {
- sm.update(1000);
- if(sm.updated(frame_pkt)) {
- auto msg = static_cast(sm[frame_pkt]);
- auto frame = msg.get(frame_pkt).as();
- camera.buf.camera_bufs_metadata[buf_idx] = {
- .frame_id = frame.get("frameId").as(),
- .timestamp_eof = frame.get("timestampEof").as(),
- .timestamp_sof = frame.get("timestampSof").as(),
- };
-
- cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q;
- cl_mem yuv_cl = camera.buf.camera_bufs[buf_idx].buf_cl;
-
- auto image = frame.get("image").as();
- clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, image.size(), image.begin(), 0, NULL, NULL);
- camera.buf.queue(buf_idx);
- buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
- }
- }
-}
-
-} // namespace
-
-void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
- camera_init(v, &s->road_cam, CAMERA_ID_IMX298, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
- camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 10, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
-}
-
-void cameras_open(MultiCameraState *s) {}
-void cameras_close(MultiCameraState *s) {}
-void camera_autoexposure(CameraState *s, float grey_frac) {}
-void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {}
-
-void cameras_run(MultiCameraState *s) {
- std::thread t = start_process_thread(s, &s->road_cam, process_road_camera);
- set_thread_name("frame_streaming");
- run_frame_stream(s->road_cam, "roadCameraState");
- t.join();
-}
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h
deleted file mode 100644
index 0d4d8cfb4..000000000
--- a/selfdrive/camerad/cameras/camera_frame_stream.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#pragma once
-
-#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
-#ifdef __APPLE__
-#include
-#else
-#include
-#endif
-
-#include "camera_common.h"
-
-#define FRAME_BUF_COUNT 16
-
-typedef struct CameraState {
- int camera_num;
- CameraInfo ci;
-
- int fps;
- float digital_gain;
-
- CameraBuf buf;
-} CameraState;
-
-typedef struct MultiCameraState {
- CameraState road_cam;
- CameraState driver_cam;
-
- SubMaster *sm;
- PubMaster *pm;
-} MultiCameraState;
diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc
index 33b5d00c9..06d58f2b0 100644
--- a/selfdrive/camerad/cameras/camera_qcom.cc
+++ b/selfdrive/camerad/cameras/camera_qcom.cc
@@ -211,12 +211,12 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
/*fps*/ 20,
#endif
device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
+ VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
/*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
/*max_gain=*/510, 10, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
+ VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
s->sm = new SubMaster({"driverState"});
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
@@ -227,7 +227,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
s->stats_bufs[i].allocate(0xb80);
}
std::fill_n(s->lapres, std::size(s->lapres), 16160);
- s->lap_conv = new LapConv(device_id, ctx, s->road_cam.buf.rgb_width, s->road_cam.buf.rgb_height, 3);
+ s->lap_conv = new LapConv(device_id, ctx, s->road_cam.buf.rgb_width, s->road_cam.buf.rgb_height, s->road_cam.buf.rgb_stride, 3);
}
static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
@@ -1045,7 +1045,7 @@ static void ops_thread(MultiCameraState *s) {
CameraExpInfo road_cam_op;
CameraExpInfo driver_cam_op;
- set_thread_name("camera_settings");
+ util::set_thread_name("camera_settings");
SubMaster sm({"sensorEvents"});
while(!do_exit) {
road_cam_op = road_cam_exp.load();
@@ -1086,10 +1086,6 @@ static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t la
c->self_recover.store(self_recover);
}
-void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
- common_process_driver_camera(s->sm, s->pm, c, cnt);
-}
-
// called by processing_thread
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
@@ -1121,7 +1117,7 @@ void cameras_run(MultiCameraState *s) {
std::vector threads;
threads.push_back(std::thread(ops_thread, s));
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
- threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
+ threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera));
CameraState* cameras[2] = {&s->road_cam, &s->driver_cam};
diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc
index 03138e353..030bbe47e 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/selfdrive/camerad/cameras/camera_qcom2.cc
@@ -709,13 +709,13 @@ static void camera_open(CameraState *s) {
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); // swap left/right
+ VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
printf("road camera initted \n");
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
- VISION_STREAM_RGB_WIDE, VISION_STREAM_YUV_WIDE);
+ VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
printf("wide road camera initted \n");
camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
+ VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
printf("driver camera initted \n");
s->sm = new SubMaster({"driverState"});
@@ -987,11 +987,6 @@ void camera_autoexposure(CameraState *s, float grey_frac) {
set_camera_exposure(s, grey_frac);
}
-
-void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
- common_process_driver_camera(s->sm, s->pm, c, cnt);
-}
-
// called by processing_thread
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
@@ -1016,7 +1011,7 @@ void cameras_run(MultiCameraState *s) {
LOG("-- Starting threads");
std::vector threads;
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
- threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
+ threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera));
threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
// start devices
diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc
index 6bc53d8c9..b5b2e6ad2 100644
--- a/selfdrive/camerad/cameras/camera_replay.cc
+++ b/selfdrive/camerad/cameras/camera_replay.cc
@@ -23,7 +23,7 @@ std::string get_url(std::string route_name, const std::string &camera, int segme
}
void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, const std::string &url) {
- s->frame = new FrameReader(true);
+ s->frame = new FrameReader();
if (!s->frame->load(url)) {
printf("failed to load stream from %s", url.c_str());
assert(0);
@@ -67,12 +67,12 @@ void run_camera(CameraState *s) {
}
void road_camera_thread(CameraState *s) {
- set_thread_name("replay_road_camera_thread");
+ util::set_thread_name("replay_road_camera_thread");
run_camera(s);
}
// void driver_camera_thread(CameraState *s) {
-// set_thread_name("replay_driver_camera_thread");
+// util::set_thread_name("replay_driver_camera_thread");
// run_camera(s);
// }
@@ -98,9 +98,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK, get_url(road_camera_route, "fcamera", 0));
+ VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
// camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
- // VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT, get_url(driver_camera_route, "dcamera", 0));
+ // VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
}
diff --git a/selfdrive/camerad/imgproc/utils.cc b/selfdrive/camerad/imgproc/utils.cc
index ad6452c6a..a88b8f4bb 100644
--- a/selfdrive/camerad/imgproc/utils.cc
+++ b/selfdrive/camerad/imgproc/utils.cc
@@ -55,8 +55,8 @@ static cl_program build_conv_program(cl_device_id device_id, cl_context context,
return cl_program_from_file(context, device_id, "imgproc/conv.cl", args);
}
-LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int filter_size)
- : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y),
+LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size)
+ : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), rgb_stride(rgb_stride),
roi_buf(width * height * 3), result_buf(width * height) {
prg = build_conv_program(device_id, ctx, width, height, filter_size);
@@ -81,9 +81,9 @@ uint16_t LapConv::Update(cl_command_queue q, const uint8_t *rgb_buf, const int r
const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1);
const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1);
- const uint8_t *rgb_offset = rgb_buf + y_offset * height * FULL_STRIDE_X * 3 + x_offset * width * 3;
+ const uint8_t *rgb_offset = rgb_buf + y_offset * height * rgb_stride + x_offset * width * 3;
for (int i = 0; i < height; ++i) {
- memcpy(&roi_buf[i * width * 3], &rgb_offset[i * FULL_STRIDE_X * 3], width * 3);
+ memcpy(&roi_buf[i * width * 3], &rgb_offset[i * rgb_stride], width * 3);
}
constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t));
diff --git a/selfdrive/camerad/imgproc/utils.h b/selfdrive/camerad/imgproc/utils.h
index 72baa5d53..b735975b3 100644
--- a/selfdrive/camerad/imgproc/utils.h
+++ b/selfdrive/camerad/imgproc/utils.h
@@ -16,16 +16,11 @@
#define LM_THRESH 120
#define LM_PREC_THRESH 0.9 // 90 perc is blur
-
-// only apply to QCOM
-#define FULL_STRIDE_X 1280
-#define FULL_STRIDE_Y 896
-
#define CONV_LOCAL_WORKSIZE 16
class LapConv {
public:
- LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int filter_size);
+ LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size);
~LapConv();
uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id);
@@ -34,6 +29,7 @@ private:
cl_program prg;
cl_kernel krnl;
const int width, height;
+ const int rgb_stride;
std::vector roi_buf;
std::vector result_buf;
};
diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc
index 1db713044..f06bd341d 100644
--- a/selfdrive/camerad/main.cc
+++ b/selfdrive/camerad/main.cc
@@ -46,9 +46,9 @@ void party(cl_device_id device_id, cl_context context) {
int main(int argc, char *argv[]) {
if (!Hardware::PC()) {
int ret;
- ret = set_realtime_priority(53);
+ ret = util::set_realtime_priority(53);
assert(ret == 0);
- ret = set_core_affinity({Hardware::EON() ? 2 : 6});
+ ret = util::set_core_affinity({Hardware::EON() ? 2 : 6});
assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
}
diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py
index 889e1579b..506064de3 100755
--- a/selfdrive/camerad/snapshot/snapshot.py
+++ b/selfdrive/camerad/snapshot/snapshot.py
@@ -37,7 +37,7 @@ def extract_image(buf, w, h, stride):
def rois_in_focus(lapres: List[float]) -> float:
- return sum([1. / len(lapres) for sharpness in lapres if sharpness >= LM_THRESH])
+ return sum(1. / len(lapres) for sharpness in lapres if sharpness >= LM_THRESH)
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focus_perc_threshold=0.):
diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index ddb20fe54..408e0d075 100644
--- a/selfdrive/car/__init__.py
+++ b/selfdrive/car/__init__.py
@@ -99,7 +99,7 @@ def crc8_pedal(data):
return crc
-def create_gas_command(packer, gas_amount, idx):
+def create_gas_interceptor_command(packer, gas_amount, idx):
# Common gas pedal msg generator
enable = gas_amount > 0.001
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index 980b207fa..786b00fa2 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -1,7 +1,7 @@
import os
from common.params import Params
from common.basedir import BASEDIR
-from selfdrive.version import comma_remote, tested_branch
+from selfdrive.version import get_comma_remote, get_tested_branch
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car
@@ -14,7 +14,7 @@ EventName = car.CarEvent.EventName
def get_startup_event(car_recognized, controller_available, fw_seen):
- if comma_remote and tested_branch:
+ if get_comma_remote() and get_tested_branch():
event = EventName.startup
else:
event = EventName.startupMaster
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index ba85a89c3..ab3125b01 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -31,10 +31,13 @@ class CarState(CarStateBase):
ret.espDisabled = (cp.vl["TRACTION_BUTTON"]["TRACTION_OFF"] == 1)
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"]
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"]
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"]
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"]
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
+ unit=1,
+ )
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index 100d47240..a621ab16a 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -10,10 +10,14 @@ WHEEL_RADIUS = 0.33
class CarState(CarStateBase):
def update(self, cp):
ret = car.CarState.new_message()
- ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"] * WHEEL_RADIUS
- ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"] * WHEEL_RADIUS
- ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"] * WHEEL_RADIUS
- ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"] * WHEEL_RADIUS
+
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"],
+ cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"],
+ cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"],
+ cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"],
+ unit=WHEEL_RADIUS,
+ )
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index 315c398ab..7c6cc6d2d 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -9,12 +9,6 @@ MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause l
class CAR:
FUSION = "FORD FUSION 2018"
-FINGERPRINTS = {
- CAR.FUSION: [{
- 71: 8, 74: 8, 75: 8, 76: 8, 90: 8, 92: 8, 93: 8, 118: 8, 119: 8, 120: 8, 125: 8, 129: 8, 130: 8, 131: 8, 132: 8, 133: 8, 145: 8, 146: 8, 357: 8, 359: 8, 360: 8, 361: 8, 376: 8, 390: 8, 391: 8, 392: 8, 394: 8, 512: 8, 514: 8, 516: 8, 531: 8, 532: 8, 534: 8, 535: 8, 560: 8, 578: 8, 604: 8, 613: 8, 673: 8, 827: 8, 848: 8, 934: 8, 935: 8, 936: 8, 947: 8, 963: 8, 970: 8, 972: 8, 973: 8, 984: 8, 992: 8, 994: 8, 997: 8, 998: 8, 1003: 8, 1034: 8, 1045: 8, 1046: 8, 1053: 8, 1054: 8, 1058: 8, 1059: 8, 1068: 8, 1072: 8, 1073: 8, 1082: 8, 1107: 8, 1108: 8, 1109: 8, 1110: 8, 1200: 8, 1427: 8, 1430: 8, 1438: 8, 1459: 8
- }],
-}
-
DBC = {
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
}
diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py
index 2c451b7f4..5d982d6a8 100755
--- a/selfdrive/car/fw_versions.py
+++ b/selfdrive/car/fw_versions.py
@@ -47,7 +47,7 @@ HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER])
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
- p16(0xf100)
+ p16(0xf100)
HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
@@ -240,7 +240,7 @@ def match_fw_to_car_exact(fw_versions_dict):
addr = ecu[1:]
found_version = fw_versions_dict.get(addr, None)
ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
- if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None:
+ if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER, TOYOTA.SIENNA, TOYOTA.LEXUS_IS] and found_version is None:
continue
# On some Toyota models, the engine can show on two different addresses
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 00f25c5fd..e4d644834 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -1,6 +1,5 @@
from cereal import car
from common.numpy_fast import mean
-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
@@ -21,10 +20,12 @@ class CarState(CarStateBase):
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
- ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
+ pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
+ pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
+ pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
+ )
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 1e0a35fdf..e1488eda2 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -3,7 +3,7 @@ from cereal import car
from common.realtime import DT_CTRL
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 import create_gas_interceptor_command
from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
from opendbc.can.packer import CANPacker
@@ -236,7 +236,7 @@ class CarController():
apply_gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
else:
apply_gas = 0.0
- can_sends.append(create_gas_command(self.packer, apply_gas, idx))
+ can_sends.append(create_gas_interceptor_command(self.packer, apply_gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
@@ -244,6 +244,6 @@ class CarController():
# 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.openpilotLongitudinalControl, CS.stock_hud))
+ can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
return can_sends
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 8bf3ac58f..12a4ae4c2 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -5,7 +5,7 @@ from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
TransmissionType = car.CarParams.TransmissionType
@@ -213,16 +213,17 @@ class CarState(CarStateBase):
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
- speed_factor = SPEED_FACTOR.get(self.CP.carFingerprint, 1.)
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS * speed_factor
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS * speed_factor
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS * speed_factor
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS * speed_factor
- v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4.
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
+ )
+ v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0
# blend in transmission speed at low speed, since it has more low speed accuracy
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
- ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
+ ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 5bcf59c25..7fcafe67f 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -1,4 +1,4 @@
-from selfdrive.car.honda.values import HONDA_BOSCH, CAR, CarControllerParams
+from selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, CAR, CarControllerParams
from selfdrive.config import Conversions as CV
# CAN bus layout with relay
@@ -98,14 +98,14 @@ def create_bosch_supplemental_1(packer, car_fingerprint, idx):
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx)
-def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, openpilot_longitudinal_control, stock_hud):
+def create_ui_commands(packer, CP, pcm_speed, hud, is_metric, idx, stock_hud):
commands = []
- bus_pt = get_pt_bus(car_fingerprint)
- radar_disabled = car_fingerprint in HONDA_BOSCH and openpilot_longitudinal_control
- bus_lkas = get_lkas_cmd_bus(car_fingerprint, radar_disabled)
+ bus_pt = get_pt_bus(CP.carFingerprint)
+ radar_disabled = CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl
+ bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled)
- if openpilot_longitudinal_control:
- if car_fingerprint in HONDA_BOSCH:
+ if CP.openpilotLongitudinalControl:
+ if CP.carFingerprint in HONDA_BOSCH:
acc_hud_values = {
'CRUISE_SPEED': hud.v_cruise,
'ENABLE_MINI_CAR': 1,
@@ -142,16 +142,24 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
'SOLID_LANES': hud.lanes,
'BEEP': 0,
}
- commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
- if radar_disabled and car_fingerprint in HONDA_BOSCH:
+ if not (CP.flags & HondaFlags.BOSCH_EXT_HUD):
+ lkas_hud_values['SET_ME_X48'] = 0x48
+
+ if CP.flags & HondaFlags.BOSCH_EXT_HUD and not CP.openpilotLongitudinalControl:
+ commands.append(packer.make_can_msg('LKAS_HUD_A', bus_lkas, lkas_hud_values, idx))
+ commands.append(packer.make_can_msg('LKAS_HUD_B', bus_lkas, lkas_hud_values, idx))
+ else:
+ commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
+
+ if radar_disabled and CP.carFingerprint in HONDA_BOSCH:
radar_hud_values = {
'CMBS_OFF': 0x01,
'SET_TO_1': 0x01,
}
commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
- if car_fingerprint == CAR.CIVIC_BOSCH:
+ if CP.carFingerprint == CAR.CIVIC_BOSCH:
commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {}, idx))
return commands
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 4aa6ec381..03074b875 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -3,7 +3,7 @@ from cereal import car
from panda import Panda
from common.numpy_fast import interp
from common.params import Params
-from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.disable_ecu import disable_ecu
@@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
ret.carName = "honda"
if candidate in HONDA_BOSCH:
- ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBoschHarness)]
+ ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)]
ret.radarOffCan = True
# Disable the radar and let openpilot control longitudinal
@@ -52,6 +52,10 @@ class CarInterface(CarInterfaceBase):
if candidate == CAR.CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
+ # Detect Bosch cars with new HUD msgs
+ if any(0x33DA in f for f in fingerprint.values()):
+ ret.flags |= HondaFlags.BOSCH_EXT_HUD.value
+
# Accord 1.5T CVT has different gearbox message
if candidate == CAR.ACCORD and 0x191 in fingerprint[1]:
ret.transmissionType = TransmissionType.cvt
@@ -143,6 +147,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_5G:
stop_and_go = True
@@ -160,6 +165,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
tire_stiffness_factor = 0.677
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_HYBRID:
stop_and_go = True
@@ -170,6 +176,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.677
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.FIT:
stop_and_go = False
@@ -201,6 +208,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.ACURA_RDX:
stop_and_go = False
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 97e0ded66..083a52bb8 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -1,3 +1,5 @@
+from enum import IntFlag
+
from cereal import car
from selfdrive.car import dbc_dict
@@ -37,6 +39,11 @@ class CarControllerParams():
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
+class HondaFlags(IntFlag):
+ # Bosch models with alternate set of LKAS_HUD messages
+ BOSCH_EXT_HUD = 1
+
+
# Car button codes
class CruiseButtons:
RES_ACCEL = 4
@@ -84,6 +91,7 @@ class CAR:
FW_VERSIONS = {
CAR.ACCORD: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
+ b'37805-6A0-8720\x00\x00',
b'37805-6A0-9520\x00\x00',
b'37805-6A0-9620\x00\x00',
b'37805-6A0-9720\x00\x00',
@@ -95,7 +103,9 @@ FW_VERSIONS = {
b'37805-6A0-A750\x00\x00',
b'37805-6A0-A840\x00\x00',
b'37805-6A0-A850\x00\x00',
+ b'37805-6A0-AF30\x00\x00',
b'37805-6A0-AG30\x00\x00',
+ b'37805-6B2-C520\x00\x00',
b'37805-6A0-C540\x00\x00',
b'37805-6A1-H650\x00\x00',
b'37805-6B2-A550\x00\x00',
@@ -121,6 +131,7 @@ FW_VERSIONS = {
b'28101-6A7-A410\x00\x00',
b'28101-6A7-A510\x00\x00',
b'28101-6A7-A610\x00\x00',
+ b'28101-6A7-A710\x00\x00',
b'28101-6A9-H140\x00\x00',
b'28101-6A9-H420\x00\x00',
b'28102-6B8-A560\x00\x00',
@@ -150,6 +161,7 @@ FW_VERSIONS = {
b'57114-TVA-C050\x00\x00',
b'57114-TVA-C060\x00\x00',
b'57114-TVA-C530\x00\x00',
+ b'57114-TVA-E520\x00\x00',
b'57114-TVE-H250\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
@@ -165,6 +177,7 @@ FW_VERSIONS = {
],
(Ecu.unknown, 0x18da3af1, None): [
b'39390-TVA-A020\x00\x00',
+ b'39390-TVA-A120\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TBX-H230\x00\x00',
@@ -183,10 +196,12 @@ FW_VERSIONS = {
b'78109-TVA-A120\x00\x00',
b'78109-TVA-A210\x00\x00',
b'78109-TVA-A220\x00\x00',
+ b'78109-TVA-A230\x00\x00',
b'78109-TVA-A310\x00\x00',
b'78109-TVA-C010\x00\x00',
b'78109-TVA-L010\x00\x00',
b'78109-TVA-L210\x00\x00',
+ b'78109-TVA-R310\x00\x00',
b'78109-TVC-A010\x00\x00',
b'78109-TVC-A020\x00\x00',
b'78109-TVC-A030\x00\x00',
@@ -194,6 +209,7 @@ FW_VERSIONS = {
b'78109-TVC-A130\x00\x00',
b'78109-TVC-A210\x00\x00',
b'78109-TVC-A220\x00\x00',
+ b'78109-TVC-A230\x00\x00',
b'78109-TVC-C010\x00\x00',
b'78109-TVC-C110\x00\x00',
b'78109-TVC-L010\x00\x00',
@@ -205,6 +221,7 @@ FW_VERSIONS = {
],
(Ecu.hud, 0x18da61f1, None): [
b'78209-TVA-A010\x00\x00',
+ b'78209-TVA-A110\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36802-TBX-H140\x00\x00',
@@ -253,6 +270,7 @@ FW_VERSIONS = {
b'78109-TWA-A030\x00\x00',
b'78109-TWA-A110\x00\x00',
b'78109-TWA-A120\x00\x00',
+ b'78109-TWA-A130\x00\x00',
b'78109-TWA-A210\x00\x00',
b'78109-TWA-A220\x00\x00',
b'78109-TWA-A230\x00\x00',
@@ -271,8 +289,8 @@ FW_VERSIONS = {
b'36161-TWA-A330\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
- b'36802-TWA-A080\x00\x00',
b'36802-TWA-A070\x00\x00',
+ b'36802-TWA-A080\x00\x00',
b'36802-TWA-A330\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
@@ -382,6 +400,7 @@ FW_VERSIONS = {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
b'37805-5AA-A940\x00\x00',
b'37805-5AA-A950\x00\x00',
+ b'37805-5AA-C950\x00\x00',
b'37805-5AA-L940\x00\x00',
b'37805-5AA-L950\x00\x00',
b'37805-5AG-Z910\x00\x00',
@@ -397,7 +416,9 @@ FW_VERSIONS = {
b'37805-5AN-AG20\x00\x00',
b'37805-5AN-AH20\x00\x00',
b'37805-5AN-AJ30\x00\x00',
+ b'37805-5AN-AK10\x00\x00',
b'37805-5AN-AK20\x00\x00',
+ b'37805-5AN-AR10\x00\x00',
b'37805-5AN-AR20\x00\x00',
b'37805-5AN-CH20\x00\x00',
b'37805-5AN-E630\x00\x00',
@@ -492,7 +513,9 @@ FW_VERSIONS = {
b'78109-TBA-C340\x00\x00',
b'78109-TBA-C910\x00\x00',
b'78109-TBC-A740\x00\x00',
+ b'78109-TBC-C540\x00\x00',
b'78109-TBG-A110\x00\x00',
+ b'78109-TBH-A710\x00\x00',
b'78109-TEG-A720\x00\x00',
b'78109-TFJ-G020\x00\x00',
b'78109-TGG-9020\x00\x00',
@@ -686,6 +709,7 @@ FW_VERSIONS = {
b'78109-TLA-A120\x00\x00',
b'78109-TLA-A210\x00\x00',
b'78109-TLA-A220\x00\x00',
+ b'78109-TLA-C020\x00\x00',
b'78109-TLA-C110\x00\x00',
b'78109-TLA-C210\x00\x00',
b'78109-TLA-C310\x00\x00',
@@ -720,6 +744,7 @@ FW_VERSIONS = {
b'36161-TMC-Q040\x00\x00',
b'36161-TNY-A020\x00\x00',
b'36161-TNY-A030\x00\x00',
+ b'36161-TNY-A040\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TLA-A240\x00\x00',
@@ -1025,6 +1050,7 @@ FW_VERSIONS = {
b'36161-TG8-A830\x00\x00',
b'36161-TGS-A130\x00\x00',
b'36161-TGT-A030\x00\x00',
+ b'36161-TGT-A130\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TG7-A210\x00\x00',
@@ -1040,7 +1066,9 @@ FW_VERSIONS = {
b'78109-TG7-AP10\x00\x00',
b'78109-TG7-AP20\x00\x00',
b'78109-TG7-AS20\x00\x00',
+ b'78109-TG7-AT20\x00\x00',
b'78109-TG7-AU20\x00\x00',
+ b'78109-TG7-AX20\x00\x00',
b'78109-TG7-DJ10\x00\x00',
b'78109-TG7-YK20\x00\x00',
b'78109-TG8-AJ10\x00\x00',
@@ -1049,7 +1077,7 @@ FW_VERSIONS = {
b'78109-TGS-AK20\x00\x00',
b'78109-TGS-AP20\x00\x00',
b'78109-TGT-AJ20\x00\x00',
- b'78109-TG7-AT20\x00\x00',
+ b'78109-TGT-AK30\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TG7-A630\x00\x00',
@@ -1147,6 +1175,7 @@ FW_VERSIONS = {
b'28102-5YK-A711\x00\x00',
b'28102-5YL-A620\x00\x00',
b'28102-5YL-A700\x00\x00',
+ b'28102-5YL-A711\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TJB-A140\x00\x00',
@@ -1155,6 +1184,7 @@ FW_VERSIONS = {
b'78109-TJB-AB10\x00\x00',
b'78109-TJB-AD10\x00\x00',
b'78109-TJB-AF10\x00\x00',
+ b'78109-TJB-AR10\x00\x00',
b'78109-TJB-AS10\000\000',
b'78109-TJB-AU10\x00\x00',
b'78109-TJB-AW10\x00\x00',
@@ -1271,6 +1301,7 @@ FW_VERSIONS = {
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-THX-A110\x00\x00',
+ b'78109-THX-A120\x00\x00',
b'78109-THX-A210\x00\x00',
b'78109-THX-A220\x00\x00',
b'78109-THX-C220\x00\x00',
@@ -1354,18 +1385,9 @@ STEER_THRESHOLD = {
CAR.CRV_EU: 400,
}
-# TODO: is this real?
-SPEED_FACTOR = {
- # default is 1, overrides go here
- CAR.CRV: 1.025,
- CAR.CRV_5G: 1.025,
- CAR.CRV_EU: 1.025,
- CAR.CRV_HYBRID: 1.025,
- CAR.HRV: 1.025,
-}
-
HONDA_NIDEC_ALT_PCM_ACCEL = set([CAR.ODYSSEY])
HONDA_NIDEC_ALT_SCM_MESSAGES = set([CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE])
-HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E])
+HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
+ CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E])
HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 4db775bea..e889f24fc 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -28,10 +28,12 @@ class CarState(CarStateBase):
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
- ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]["WHL_SPD_FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]["WHL_SPD_RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]["WHL_SPD_RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
+ cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
+ cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
+ cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index c8130fd72..50031231b 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -102,7 +102,7 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
"CR_VSM_Alive": idx % 0xF,
}
scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2]
- scc12_values["CR_VSM_ChkSum"] = 0x10 - sum([sum(divmod(i, 16)) for i in scc12_dat]) % 0x10
+ scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
@@ -127,7 +127,7 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
"FCA_Status": 1, # AEB disabled
}
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
- fca11_values["CR_FCA_ChkSum"] = 0x10 - sum([sum(divmod(i, 16)) for i in fca11_dat]) % 0x10
+ fca11_values["CR_FCA_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in fca11_dat) % 0x10
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
return commands
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index ddcd00ba0..df2e10dbe 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -283,6 +283,7 @@ FW_VERSIONS = {
b'\xf1\x82DNBWN5TMDCXXXG2E',
b'\xf1\x82DNCVN5GMCCXXXF0A',
b'\xf1\x82DNCVN5GMCCXXXG2B',
+ b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DNDWN5TMDCXXXJ1A',
b'\xf1\x87391162M003',
b'\xf1\x87391162M013',
b'\xf1\x87391162M023',
@@ -327,6 +328,8 @@ FW_VERSIONS = {
b'\xf1\x87SAKFBA2926554GJ2VefVww\x87xwwwww\x88\x87xww\x87wTo\xfb\xffvUo\xff\x8d\x16\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SAKFBA3030524GJ2UVugww\x97yx\x88\x87\x88vw\x87gww\x87wto\xf9\xfffUo\xff\xa2\x0c\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SAKFBA3356084GJ2\x86fvgUUuWgw\x86www\x87wffvf\xb6\xcf\xfc\xffeUO\xff\x12\x19\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
+ b'\xf1\x87SAKFBA3474944GJ2ffvgwwwwg\x88\x86x\x88\x88\x98\x88ffvfeo\xfa\xff\x86fo\xff\t\xae\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
+ b'\xf1\x87SAKFBA3475714GJ2Vfvgvg\x96yx\x88\x97\x88ww\x87ww\x88\x87xs_\xfb\xffvUO\xff\x0f\xff\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALDBA3510954GJ3ww\x87xUUuWx\x88\x87\x88\x87w\x88wvfwfc_\xf9\xff\x98wO\xffl\xe0\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA3573534GJ3\x89\x98\x89\x88EUuWgwvwwwwww\x88\x87xTo\xfa\xff\x86f\x7f\xffo\x0e\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA3601464GJ3\x88\x88\x88\x88ffvggwvwvw\x87gww\x87wvo\xfb\xff\x98\x88\x7f\xffjJ\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
@@ -336,21 +339,27 @@ FW_VERSIONS = {
b'\xf1\x87SALDBA4525334GJ3\x89\x99\x99\x99fevWh\x88\x86\x88fwvgw\x88\x87xfo\xfa\xffuDo\xff\xd1>\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA4626804GJ3wwww\x88\x87\x88xx\x88\x87\x88wwgw\x88\x88\x98\x88\x95_\xf9\xffuDo\xff|\xe7\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA4803224GJ3wwwwwvwg\x88\x88\x98\x88wwww\x87\x88\x88xu\x9f\xfc\xff\x87f\x8f\xff\xea\xea\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
+ b'\xf1\x87SALDBA6212564GJ3\x87wwwUTuGg\x88\x86xx\x88\x87\x88\x87\x88\x98xu?\xf9\xff\x97f\x7f\xff\xb8\n\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA6347404GJ3wwwwff\x86hx\x88\x97\x88\x88\x88\x88\x88vfgf\x88?\xfc\xff\x86Uo\xff\xec/\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA6901634GJ3UUuWVeVUww\x87wwwwwvUge\x86/\xfb\xff\xbb\x99\x7f\xff]2\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA7077724GJ3\x98\x88\x88\x88ww\x97ygwvwww\x87ww\x88\x87x\x87_\xfd\xff\xba\x99o\xff\x99\x01\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALFBA3525114GJ2wvwgvfvggw\x86wffvffw\x86g\x85_\xf9\xff\xa8wo\xffv\xcd\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA3624024GJ2\x88\x88\x88\x88wv\x87hx\x88\x97\x88x\x88\x97\x88ww\x87w\x86o\xfa\xffvU\x7f\xff\xd1\xec\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA3960824GJ2wwwwff\x86hffvfffffvfwfg_\xf9\xff\xa9\x88\x8f\xffb\x99\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
+ b'\xf1\x87SALFBA4011074GJ2fgvwwv\x87hw\x88\x87xww\x87wwfgvu_\xfa\xffefo\xff\x87\xc0\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA4121304GJ2x\x87xwff\x86hwwwwww\x87wwwww\x84_\xfc\xff\x98\x88\x9f\xffi\xa6\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA4195874GJ2EVugvf\x86hgwvwww\x87wgw\x86wc_\xfb\xff\x98\x88\x8f\xff\xe23\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
+ b'\xf1\x87SALFBA4625294GJ2eVefeUeVx\x88\x97\x88wwwwwwww\xa7o\xfb\xffvw\x9f\xff\xee.\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
+ b'\xf1\x87SALFBA4728774GJ2vfvg\x87vwgww\x87ww\x88\x97xww\x87w\x86_\xfb\xffeD?\xffk0\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA5129064GJ2vfvgwv\x87hx\x88\x87\x88ww\x87www\x87wd_\xfa\xffvfo\xff\x1d\x00\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA5454914GJ2\x98\x88\x88\x88\x87vwgx\x88\x87\x88xww\x87ffvf\xa7\x7f\xf9\xff\xa8w\x7f\xff\x1b\x90\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
+ b'\xf1\x87SALFBA5987784GJ2UVugDDtGx\x88\x87\x88w\x88\x87xwwwwd/\xfb\xff\x97fO\xff\xb0h\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA5987864GJ2fgvwUUuWgwvw\x87wxwwwww\x84/\xfc\xff\x97w\x7f\xff\xdf\x1d\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA6337644GJ2vgvwwv\x87hgffvwwwwwwww\x85O\xfa\xff\xa7w\x7f\xff\xc5\xfc\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA6802004GJ2UUuWUUuWgw\x86www\x87www\x87w\x96?\xf9\xff\xa9\x88\x7f\xff\x9fK\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA6892284GJ233S5\x87w\x87xx\x88\x87\x88vwwgww\x87w\x84?\xfb\xff\x98\x88\x8f\xff*\x9e\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
b'\xf1\x87SALFBA7005534GJ2eUuWfg\x86xxww\x87x\x88\x87\x88\x88w\x88\x87\x87O\xfc\xffuUO\xff\xa3k\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1',
+ b'\xf1\x87SALFBA7152454GJ2gvwgFf\x86hx\x88\x87\x88vfWfffffd?\xfa\xff\xba\x88o\xff,\xcf\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1',
b'\xf1\x87SALFBA7485034GJ2ww\x87xww\x87xfwvgwwwwvfgf\xa5/\xfc\xff\xa9w_\xff40\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc',
b'\xf1\x87SAMDBA7743924GJ3wwwwww\x87xgwvw\x88\x88\x88\x88wwww\x85_\xfa\xff\x86f\x7f\xff0\x9d\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SAMDBA7817334GJ3Vgvwvfvgww\x87wwwwwwfgv\x97O\xfd\xff\x88\x88o\xff\x8e\xeb\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00',
@@ -529,6 +538,7 @@ FW_VERSIONS = {
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.00 99110-S8110 ',
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 ',
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ',
+ b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ',
],
(Ecu.esp, 0x7d1, None): [
b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360',
@@ -595,11 +605,18 @@ FW_VERSIONS = {
b'\xf1\x87LDLVBN757883KF37\x98\x87xw\x98\x87\x88xy\xaa\xb7\x9ag\x88\x96x\x89\x99\xa8\x99e\x7f\xf6\xff\xa9\x88o\xff5\x15\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6',
b'\xf1\x87LDMVBN778156KF37\x87vWe\xa9\x99\x99\x99y\x99\xb7\x99\x99\x99\x99\x99x\x99\x97\x89\xa8\x7f\xf8\xffwf\x7f\xff\x82_\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6',
b'\xf1\x87LDMVBN780576KF37\x98\x87hv\x97x\x97\x89x\x99\xa7\x89\x88\x99\x98\x89w\x88\x97x\x98\x7f\xf7\xff\xba\x88\x8f\xff\x1e0\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6',
+ b'\xf1\x87LDMVBN783485KF37\x87www\x87vwgy\x99\xa7\x99\x99\x99\xa9\x99Vw\x95g\x89_\xf6\xff\xa9w_\xff\xc5\xd6\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6',
b'\xf1\x87LDMVBN811844KF37\x87vwgvfffx\x99\xa7\x89Vw\x95gg\x88\xa6xe\x8f\xf6\xff\x97wO\xff\t\x80\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6',
b'\xf1\x87LDMVBN830601KF37\xa7www\xa8\x87xwx\x99\xa7\x89Uw\x85Ww\x88\x97x\x88o\xf6\xff\x8a\xaa\x7f\xff\xe2:\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6',
b'\xf1\x87LDMVBN848789KF37\x87w\x87x\x87w\x87xy\x99\xb7\x99\x87\x88\x98x\x88\x99\xa8\x89\x87\x7f\xf6\xfffUo\xff\xe3!\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
b'\xf1\x87LDMVBN851595KF37\x97wgvvfffx\x99\xb7\x89\x88\x99\x98\x89\x87\x88\x98x\x99\x7f\xf7\xff\x97w\x7f\xff@\xf3\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
+ b'\xf1\x87LDMVBN873175KF26\xa8\x88\x88\x88vfVex\x99\xb7\x89\x88\x99\x98\x89x\x88\x97\x88f\x7f\xf7\xff\xbb\xaa\x8f\xff,\x04\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
b'\xf1\x87LDMVBN879401KF26veVU\xa8\x88\x88\x88g\x88\xa6xVw\x95gx\x88\xa7\x88v\x8f\xf9\xff\xdd\xbb\xbf\xff\xb3\x99\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
+ b'\xf1\x87LDMVBN881314KF37\xa8\x88h\x86\x97www\x89\x99\xa8\x99w\x88\x97xx\x99\xa7\x89\xca\x7f\xf8\xff\xba\x99\x8f\xff\xd8v\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
+ b'\xf1\x87LDMVBN888651KF37\xa9\x99\x89\x98vfff\x88\x99\x98\x89w\x99\xa7y\x88\x88\x98\x88D\x8f\xf9\xff\xcb\x99\x8f\xff\xa5\x1e\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
+ b'\xf1\x87LDMVBN889419KF37\xa9\x99y\x97\x87w\x87xx\x88\x97\x88w\x88\x97x\x88\x99\x98\x89e\x9f\xf9\xffeUo\xff\x901\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
+ b'\xf1\x87LDMVBN895969KF37vefV\x87vgfx\x99\xa7\x89\x99\x99\xb9\x99f\x88\x96he_\xf7\xffxwo\xff\x14\xf9\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
+ b'\xf1\x87LDMVBN899222KF37\xa8\x88x\x87\x97www\x98\x99\x99\x89\x88\x99\x98\x89f\x88\x96hdo\xf7\xff\xbb\xaa\x9f\xff\xe2U\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89',
b"\xf1\x87LBLUFN622950KF36\xa8\x88\x88\x88\x87w\x87xh\x99\x96\x89\x88\x99\x98\x89\x88\x99\x98\x89\x87o\xf6\xff\x98\x88o\xffx'\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8",
],
},
@@ -672,6 +689,7 @@ FW_VERSIONS = {
CAR.KIA_FORTE: {
(Ecu.eps, 0x7D4, None): [
b'\xf1\x00BD MDPS C 1.00 1.02 56310-XX000 4BD2C102',
+ b'\xf1\x00BD MDPS C 1.00 1.08 56310/M6300 4BDDC108',
b'\xf1\x00BD MDPS C 1.00 1.08 56310M6300\x00 4BDDC108',
],
(Ecu.fwdCamera, 0x7C4, None): [
@@ -688,6 +706,7 @@ FW_VERSIONS = {
b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\x00\x00\x00\x00',
b"\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\xcf\x1e'\xc3",
],
},
@@ -696,28 +715,34 @@ FW_VERSIONS = {
b'\xf1\000DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ',
b'\xf1\x8799110L2000\xf1\000DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ',
b'\xf1\x8799110L2100\xf1\x00DL3_ SCC F-CUP 1.00 1.03 99110-L2100 ',
+ b'\xf1\x8799110L2100\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2100 ',
],
(Ecu.eps, 0x7D4, None): [
b'\xf1\x8756310-L3110\xf1\000DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101',
b'\xf1\x8756310-L3220\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101',
+ b'\xf1\x8757700-L3000\xf1\x00DL3 MDPS R 1.00 1.02 57700-L3000 4DLAP102',
],
(Ecu.fwdCamera, 0x7C4, None): [
b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915',
+ b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.04 99210-L3000 210208',
],
(Ecu.esp, 0x7D1, None): [
b'\xf1\000DL ESC \006 101 \004\002 58910-L3200',
b'\xf1\x8758910-L3200\xf1\000DL ESC \006 101 \004\002 58910-L3200',
b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800',
+ b'\xf1\x8758910-L3600\xf1\x00DL ESC \x03 100 \x08\x02 58910-L3600',
],
(Ecu.engine, 0x7E0, None): [
b'\xf1\x87391212MKT0',
b'\xf1\x87391212MKV0',
+ b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DLDWN5TMDCXXXJ1B',
],
(Ecu.transmission, 0x7E1, None): [
b'\xf1\000bcsh8p54 U913\000\000\000\000\000\000TDL2T16NB1ia\v\xb8',
b'\xf1\x87SALFEA5652514GK2UUeV\x88\x87\x88xxwg\x87ww\x87wwfwvd/\xfb\xffvU_\xff\x93\xd3\xf1\x81U913\000\000\000\000\000\000\xf1\000bcsh8p54 U913\000\000\000\000\000\000TDL2T16NB1ia\v\xb8',
b'\xf1\x87SALFEA6046104GK2wvwgeTeFg\x88\x96xwwwwffvfe?\xfd\xff\x86fo\xff\x97A\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB1ia\x0b\xb8',
b'\xf1\x87SCMSAA8572454GK1\x87x\x87\x88Vf\x86hgwvwvwwgvwwgT?\xfb\xff\x97fo\xffH\xb8\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18',
+ b'\xf1\x87954A02N300\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 WDL3T25XXX730NS2b\x1f\xb8%',
],
},
CAR.KONA_EV: {
@@ -750,6 +775,7 @@ FW_VERSIONS = {
CAR.KIA_NIRO_EV: {
(Ecu.fwdRadar, 0x7D0, None): [
b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ',
+ b'\xf1\x00DEev SCC F-CUP 1.00 1.02 96400-Q4100 ',
b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 ',
b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 ',
b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ',
@@ -842,13 +868,17 @@ FW_VERSIONS = {
b'\xf1\x89F1JF600AISEIU702\xf1\x82F1JF600AISEIU702',
],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409'],
- (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.02 95895-D5000 h31'],
+ (Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.00 95895-D5001 h32',
+ b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.02 95895-D5000 h31',
+ ],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW'],
},
CAR.ELANTRA_2021: {
(Ecu.fwdRadar, 0x7d0, None): [
- b'\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ',
b'\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ',
+ b'\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ',
+ b'\xf1\x8799110AA000\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106',
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 336c1ee6c..bef3456a7 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -14,9 +14,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
GearShifter = car.CarState.GearShifter
EventName = car.CarEvent.EventName
-# WARNING: this value was determined based on the model's training distribution,
-# model predictions above this speed can be unpredictable
-MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 135 + 4 = 86 mph
+MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
ACCEL_MAX = 2.0
ACCEL_MIN = -3.5
@@ -78,6 +76,7 @@ class CarInterfaceBase():
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.]
ret.minSteerSpeed = 0.
+ ret.wheelSpeedFactor = 1.0
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
@@ -210,6 +209,16 @@ class CarStateBase:
v_ego_x = self.v_ego_kf.update(v_ego_raw)
return float(v_ego_x[0]), float(v_ego_x[1])
+ def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS):
+ factor = unit * self.CP.wheelSpeedFactor
+
+ wheelSpeeds = car.CarState.WheelSpeeds.new_message()
+ wheelSpeeds.fl = fl * factor
+ wheelSpeeds.fr = fr * factor
+ wheelSpeeds.rl = rl * factor
+ wheelSpeeds.rr = rr * factor
+ return wheelSpeeds
+
def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
"""Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
iterations"""
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index 4140a4e07..7ebf60c96 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -20,10 +20,12 @@ class CarState(CarStateBase):
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["FL"],
+ cp.vl["WHEEL_SPEEDS"]["FR"],
+ cp.vl["WHEEL_SPEEDS"]["RL"],
+ cp.vl["WHEEL_SPEEDS"]["RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index ada3ea1dd..58e1dd703 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -21,7 +21,7 @@ class CAR:
CX9 = "MAZDA CX-9"
MAZDA3 = "MAZDA 3"
MAZDA6 = "MAZDA 6"
- CX9_2021 = "Mazda CX-9 2021" # No Steer Lockout
+ CX9_2021 = "MAZDA CX-9 2021" # No Steer Lockout
class LKAS_LIMITS:
STEER_THRESHOLD = 15
diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py
index a4019b538..05191feed 100644
--- a/selfdrive/car/nissan/carstate.py
+++ b/selfdrive/car/nissan/carstate.py
@@ -35,11 +35,12 @@ class CarState(CarStateBase):
elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]:
ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"])
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS
-
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index e32403776..1f56f09ff 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -23,10 +23,12 @@ class CarState(CarStateBase):
else:
ret.brakePressed = cp.vl["Brake_Status"]["Brake"] == 1
- ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]["FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]["FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]["RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]["RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["Wheel_Speeds"]["FL"],
+ cp.vl["Wheel_Speeds"]["FR"],
+ cp.vl["Wheel_Speeds"]["RL"],
+ cp.vl["Wheel_Speeds"]["RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index daf91a49d..1997e6c11 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,39 +1,22 @@
from cereal import car
from common.numpy_fast import clip, interp
-from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg
+from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
create_accel_command, create_acc_cancel_command, \
create_fcw_command, create_lta_steer_command
from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
- MIN_ACC_SPEED, PEDAL_HYST_GAP, PEDAL_SCALE, CarControllerParams
+ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
-def accel_hysteresis(accel, accel_steady, enabled):
-
- # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
- if not enabled:
- # send 0 when disabled, otherwise acc faults
- accel_steady = 0.
- elif accel > accel_steady + CarControllerParams.ACCEL_HYST_GAP:
- accel_steady = accel - CarControllerParams.ACCEL_HYST_GAP
- elif accel < accel_steady - CarControllerParams.ACCEL_HYST_GAP:
- accel_steady = accel + CarControllerParams.ACCEL_HYST_GAP
- accel = accel_steady
-
- return accel, accel_steady
-
-
class CarController():
def __init__(self, dbc_name, CP, VM):
self.last_steer = 0
- self.accel_steady = 0.
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.steer_rate_limited = False
- self.use_interceptor = False
self.packer = CANPacker(dbc_name)
@@ -43,25 +26,22 @@ class CarController():
# *** compute control surfaces ***
# gas and brake
- interceptor_gas_cmd = 0.
- pcm_accel_cmd = actuators.accel
-
- if CS.CP.enableGasInterceptor:
- # handle hysteresis when around the minimum acc speed
- if CS.out.vEgo < MIN_ACC_SPEED:
- self.use_interceptor = True
- elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
- self.use_interceptor = False
-
- if self.use_interceptor and active:
- # only send negative accel when using interceptor. gas handles acceleration
- # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
- MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5])
- interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS)
- pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
-
- pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled)
- pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+ if CS.CP.enableGasInterceptor and enabled:
+ MAX_INTERCEPTOR_GAS = 0.5
+ # RAV4 has very sensitive gas pedal
+ if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
+ PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
+ elif CS.CP.carFingerprint in [CAR.COROLLA]:
+ PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
+ else:
+ PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
+ # offset for creep and windbrake
+ pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
+ pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
+ interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
+ else:
+ interceptor_gas_cmd = 0.
+ pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -88,7 +68,6 @@ class CarController():
self.standstill_req = False
self.last_steer = apply_steer
- self.last_accel = pcm_accel_cmd
self.last_standstill = CS.out.standstill
can_sends = []
@@ -113,7 +92,7 @@ class CarController():
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
- if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
+ if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
@@ -123,7 +102,7 @@ class CarController():
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
- can_sends.append(create_gas_command(self.packer, interceptor_gas_cmd, frame // 2))
+ can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index cacd5b7be..7ce5907b9 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -41,10 +41,12 @@ class CarState(CarStateBase):
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
+ )
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
@@ -79,7 +81,7 @@ class CarState(CarStateBase):
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in [1, 5]
- if self.CP.carFingerprint == CAR.LEXUS_IS:
+ if self.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
else:
@@ -93,7 +95,7 @@ class CarState(CarStateBase):
# these cars are identified by an ACC_TYPE value of 2.
# TODO: it is possible to avoid the lockout and gain stop and go if you
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
- if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint != CAR.LEXUS_IS) or \
+ if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in [CAR.LEXUS_IS, CAR.LEXUS_RC]) or \
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
@@ -168,7 +170,7 @@ class CarState(CarStateBase):
("STEER_TORQUE_SENSOR", 50),
]
- if CP.carFingerprint == CAR.LEXUS_IS:
+ if CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
checks.append(("DSU_CRUISE", 5))
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index a1221ae8a..f9d6b586f 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -65,7 +65,6 @@ class CarInterface(CarInterfaceBase):
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_B)
-
elif candidate == CAR.LEXUS_RXH:
stop_and_go = True
ret.wheelbase = 2.79
@@ -81,6 +80,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.5533 # not optimized yet
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+ ret.wheelSpeedFactor = 1.035
elif candidate == CAR.LEXUS_RXH_TSS2:
stop_and_go = True
@@ -89,6 +89,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4481.0 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
set_lat_tune(ret.lateralTuning, LatTunes.PID_E)
+ ret.wheelSpeedFactor = 1.035
elif candidate in [CAR.CHR, CAR.CHRH]:
stop_and_go = True
@@ -186,6 +187,15 @@ class CarInterface(CarInterfaceBase):
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
+ elif candidate == CAR.LEXUS_RC:
+ ret.safetyConfigs[0].safetyParam = 77
+ stop_and_go = False
+ ret.wheelbase = 2.73050
+ ret.steerRatio = 13.3
+ tire_stiffness_factor = 0.444
+ ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
+
elif candidate == CAR.LEXUS_CTH:
ret.safetyConfigs[0].safetyParam = 100
stop_and_go = True
@@ -206,7 +216,7 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.PRIUS_TSS2:
stop_and_go = True
ret.wheelbase = 2.70002 # from toyota online sepc.
- ret.steerRatio = 13.4 # True steerRation from older prius
+ ret.steerRatio = 13.4 # True steerRatio from older prius
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_N)
@@ -259,7 +269,8 @@ class CarInterface(CarInterfaceBase):
if ret.enableGasInterceptor:
set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL)
- elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]:
+ elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2,
+ CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2]:
set_long_tune(ret.longitudinalTuning, LongTunes.TSS2)
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
ret.startingAccelRate = 6.0 # release brakes fast
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 3f210d48a..15c8bbfcc 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -1,6 +1,5 @@
#!/usr/bin/env python3
from enum import Enum
-from selfdrive.car.toyota.values import MIN_ACC_SPEED, PEDAL_HYST_GAP
class LongTunes(Enum):
@@ -29,15 +28,8 @@ class LatTunes(Enum):
###### LONG ######
def set_long_tune(tune, name):
- if name == LongTunes.PEDAL:
- tune.deadzoneBP = [0.]
- tune.deadzoneV = [0.]
- tune.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
- tune.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
- tune.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
- tune.kiV = [0.18, 0.165, 0.489, 0.36]
# Improved longitudinal tune
- elif name == LongTunes.TSS2:
+ if name == LongTunes.TSS2 or name == LongTunes.PEDAL:
tune.deadzoneBP = [0., 8.05]
tune.deadzoneV = [.0, .14]
tune.kpBP = [0., 5., 20.]
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 73198d925..692de379b 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -6,12 +6,10 @@ from selfdrive.config import Conversions as CV
Ecu = car.CarParams.Ecu
MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
+PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
-PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS
-PEDAL_SCALE = 3.0
class CarControllerParams:
- ACCEL_HYST_GAP = 0.06 # don't change accel command for small oscilalitons within this value
ACCEL_MAX = 1.5 # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
ACCEL_MIN = -3.5 # m/s2
@@ -58,6 +56,7 @@ class CAR:
LEXUS_NX = "LEXUS NX 2018"
LEXUS_NXH = "LEXUS NX HYBRID 2018"
LEXUS_NX_TSS2 = "LEXUS NX 2020"
+ LEXUS_RC = "LEXUS RC 2020"
LEXUS_RX = "LEXUS RX 2016"
LEXUS_RXH = "LEXUS RX HYBRID 2017"
LEXUS_RX_TSS2 = "LEXUS RX 2020"
@@ -347,10 +346,11 @@ FW_VERSIONS = {
b'\x018966306T3200\x00\x00\x00\x00',
b'\x018966306T4100\x00\x00\x00\x00',
],
- (Ecu.fwdRadar, 0x750, 15): [
+ (Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
],
- (Ecu.fwdCamera, 0x750, 109): [
+ (Ecu.fwdCamera, 0x750, 0x6d): [
+ b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
],
@@ -434,6 +434,7 @@ FW_VERSIONS = {
},
CAR.CHRH: {
(Ecu.engine, 0x700, None): [
+ b'\x0289663F405100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896631013200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F405000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F418000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
@@ -442,6 +443,7 @@ FW_VERSIONS = {
b'\x0189663F438000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
+ b'F152610012\x00\x00\x00\x00\x00\x00',
b'F152610013\x00\x00\x00\x00\x00\x00',
b'F152610014\x00\x00\x00\x00\x00\x00',
b'F152610040\x00\x00\x00\x00\x00\x00',
@@ -455,6 +457,7 @@ FW_VERSIONS = {
b'8821FF402400 ',
b'8821FF404000 ',
b'8821FF404100 ',
+ b'8821FF405000 ',
b'8821FF406000 ',
b'8821FF407100 ',
],
@@ -470,10 +473,12 @@ FW_VERSIONS = {
b'8821FF402400 ',
b'8821FF404000 ',
b'8821FF404100 ',
+ b'8821FF405000 ',
b'8821FF406000 ',
b'8821FF407100 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
+ b'8646FF401700 ',
b'8646FF402100 ',
b'8646FF404000 ',
b'8646FF406000 ',
@@ -542,6 +547,7 @@ FW_VERSIONS = {
b'\x018966312W9000\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
+ b'\x0230A10000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0230A11000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x03312K7000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
@@ -568,6 +574,7 @@ FW_VERSIONS = {
b'\x01F152602560\x00\x00\x00\x00\x00\x00',
b'\x01F152602590\x00\x00\x00\x00\x00\x00',
b'\x01F152602650\x00\x00\x00\x00\x00\x00',
+ b"\x01F15260A010\x00\x00\x00\x00\x00\x00",
b'\x01F15260A050\x00\x00\x00\x00\x00\x00',
b'\x01F152612641\x00\x00\x00\x00\x00\x00',
b'\x01F152612651\x00\x00\x00\x00\x00\x00',
@@ -666,6 +673,7 @@ FW_VERSIONS = {
b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
+ b"\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00",
b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F7603100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
@@ -801,13 +809,14 @@ FW_VERSIONS = {
},
CAR.LEXUS_IS: {
(Ecu.engine, 0x700, None): [
+ b'\x018966353M7000\x00\x00\x00\x00',
b'\x018966353M7100\x00\x00\x00\x00',
b'\x018966353Q2000\x00\x00\x00\x00',
b'\x018966353Q2300\x00\x00\x00\x00',
+ b'\x018966353Q4000\x00\x00\x00\x00',
b'\x018966353R1100\x00\x00\x00\x00',
b'\x018966353R7100\x00\x00\x00\x00',
b'\x018966353R8100\x00\x00\x00\x00',
- b'\x018966353Q4000\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\x0232480000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -815,6 +824,7 @@ FW_VERSIONS = {
b'\x02353P9000\x00\x00\x00\x00\x00\x00\x00\x00553C1000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
+ b'F152653300\x00\x00\x00\x00\x00\x00',
b'F152653301\x00\x00\x00\x00\x00\x00',
b'F152653310\x00\x00\x00\x00\x00\x00',
b'F152653330\x00\x00\x00\x00\x00\x00',
@@ -837,9 +847,10 @@ FW_VERSIONS = {
b'8821F4702100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
+ b'8646F5301101\x00\x00\x00\x00',
+ b'8646F5301200\x00\x00\x00\x00',
b'8646F5301300\x00\x00\x00\x00',
b'8646F5301400\x00\x00\x00\x00',
- b'8646F5301200\x00\x00\x00\x00',
],
},
CAR.PRIUS: {
@@ -1157,6 +1168,7 @@ FW_VERSIONS = {
b'\x01896630851000\x00\x00\x00\x00',
b'\x01896630851100\x00\x00\x00\x00',
b'\x01896630851200\x00\x00\x00\x00',
+ b'\x01896630852000\x00\x00\x00\x00',
b'\x01896630852100\x00\x00\x00\x00',
b'\x01896630859000\x00\x00\x00\x00',
b'\x01896630860000\x00\x00\x00\x00',
@@ -1204,15 +1216,18 @@ FW_VERSIONS = {
b'\x018966333T5000\x00\x00\x00\x00',
b'\x018966333T5100\x00\x00\x00\x00',
b'\x018966333X6000\x00\x00\x00\x00',
+ b'\x01896633T07000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152606281\x00\x00\x00\x00\x00\x00',
b'\x01F152606340\x00\x00\x00\x00\x00\x00',
+ b'\x01F152606461\x00\x00\x00\x00\x00\x00',
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B33252\x00\x00\x00\x00\x00\x00',
b'8965B33590\x00\x00\x00\x00\x00\x00',
+ b'8965B33690\x00\x00\x00\x00\x00\x00',
b'8965B48271\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
@@ -1224,6 +1239,7 @@ FW_VERSIONS = {
b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
+ b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
@@ -1288,6 +1304,7 @@ FW_VERSIONS = {
b'\x01896637851000\x00\x00\x00\x00',
b'\x01896637852000\x00\x00\x00\x00',
b'\x01896637854000\x00\x00\x00\x00',
+ b'\x01896637878000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152678130\x00\x00\x00\x00\x00\x00',
@@ -1295,6 +1312,7 @@ FW_VERSIONS = {
],
(Ecu.dsu, 0x791, None): [
b'881517803100\x00\x00\x00\x00',
+ b'881517803300\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B78060\x00\x00\x00\x00\x00\x00',
@@ -1306,6 +1324,7 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F7801100\x00\x00\x00\x00',
+ b'8646F7801300\x00\x00\x00\x00',
],
},
CAR.LEXUS_NX_TSS2: {
@@ -1358,6 +1377,26 @@ FW_VERSIONS = {
b'8646F7801100\x00\x00\x00\x00',
],
},
+ CAR.LEXUS_RC: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\x0232484000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.esp, 0x7b0, None): [
+ b'F152624221\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.dsu, 0x791, None): [
+ b'881512409100\x00\x00\x00\x00',
+ ],
+ (Ecu.eps, 0x7a1, None): [
+ b'8965B24081\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x750, 0xf): [
+ b'8821F4702300\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x750, 0x6d): [
+ b'8646F2402200\x00\x00\x00\x00',
+ ],
+ },
CAR.LEXUS_RX: {
(Ecu.engine, 0x700, None): [
b'\x01896630E36200\x00\x00\x00\x00',
@@ -1369,6 +1408,7 @@ FW_VERSIONS = {
b'\x01896630E41200\x00\x00\x00\x00',
b'\x01896630E41500\x00\x00\x00\x00',
b'\x01896630EA3100\x00\x00\x00\x00',
+ b'\x01896630EA3400\x00\x00\x00\x00',
b'\x01896630EA4100\x00\x00\x00\x00',
b'\x01896630EA4300\x00\x00\x00\x00',
b'\x01896630EA4400\x00\x00\x00\x00',
@@ -1558,6 +1598,7 @@ DBC = {
CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_adas'),
CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_adas'),
CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_adas'),
+ CAR.LEXUS_RC: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX: dbc_dict('lexus_rx_350_2016_pt_generated', 'toyota_adas'),
CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index 97f00008b..977818dbd 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -86,27 +86,28 @@ class CarController():
# FIXME: this entire section is in desperate need of refactoring
- if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
- if not enabled and CS.out.cruiseState.enabled:
- # Cancel ACC if it's engaged with OP disengaged.
- self.graButtonStatesToSend = BUTTON_STATES.copy()
- self.graButtonStatesToSend["cancel"] = True
- elif enabled and CS.out.standstill:
- # Blip the Resume button if we're engaged at standstill.
- # FIXME: This is a naive implementation, improve with visiond or radar input.
- self.graButtonStatesToSend = BUTTON_STATES.copy()
- self.graButtonStatesToSend["resumeCruise"] = True
+ if CS.CP.pcmCruise:
+ if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
+ if not enabled and CS.out.cruiseState.enabled:
+ # Cancel ACC if it's engaged with OP disengaged.
+ self.graButtonStatesToSend = BUTTON_STATES.copy()
+ self.graButtonStatesToSend["cancel"] = True
+ elif enabled and CS.esp_hold_confirmation:
+ # Blip the Resume button if we're engaged at standstill.
+ # FIXME: This is a naive implementation, improve with visiond or radar input.
+ self.graButtonStatesToSend = BUTTON_STATES.copy()
+ self.graButtonStatesToSend["resumeCruise"] = True
- if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
- self.graMsgBusCounterPrev = CS.graMsgBusCounter
- if self.graButtonStatesToSend is not None:
- if self.graMsgSentCount == 0:
- self.graMsgStartFramePrev = frame
- idx = (CS.graMsgBusCounter + 1) % 16
- can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx))
- self.graMsgSentCount += 1
- if self.graMsgSentCount >= P.GRA_VBP_COUNT:
- self.graButtonStatesToSend = None
- self.graMsgSentCount = 0
+ if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
+ self.graMsgBusCounterPrev = CS.graMsgBusCounter
+ if self.graButtonStatesToSend is not None:
+ if self.graMsgSentCount == 0:
+ self.graMsgStartFramePrev = frame
+ idx = (CS.graMsgBusCounter + 1) % 16
+ can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx))
+ self.graMsgSentCount += 1
+ if self.graMsgSentCount >= P.GRA_VBP_COUNT:
+ self.graButtonStatesToSend = None
+ self.graMsgSentCount = 0
return can_sends
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index f02129a61..97e3094fa 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -20,14 +20,16 @@ class CarState(CarStateBase):
def update(self, pt_cp, cam_cp, ext_cp, trans_type):
ret = car.CarState.new_message()
# Update vehicle speed and acceleration from ABS wheel speeds.
- ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"],
+ pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"],
+ pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"],
+ pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"],
+ )
ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
- ret.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"])
+ ret.standstill = ret.vEgo < 0.1
# Update steering angle, rate, yaw rate, and driver input torque. VW send
# the sign/direction in a separate signal so they must be recombined.
@@ -47,6 +49,7 @@ class CarState(CarStateBase):
ret.gasPressed = ret.gas > 0
ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
+ self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]
# Update gear and/or clutch position data.
if trans_type == TransmissionType.automatic:
@@ -94,13 +97,13 @@ class CarState(CarStateBase):
ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"])
# Update ACC radar status.
- accStatus = pt_cp.vl["TSK_06"]["TSK_Status"]
- if accStatus == 2:
+ self.tsk_status = pt_cp.vl["TSK_06"]["TSK_Status"]
+ if self.tsk_status == 2:
# ACC okay and enabled, but not currently engaged
ret.cruiseState.available = True
ret.cruiseState.enabled = False
- elif accStatus in [3, 4, 5]:
- # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5)
+ elif self.tsk_status in [3, 4, 5]:
+ # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5)
ret.cruiseState.available = True
ret.cruiseState.enabled = True
else:
@@ -110,9 +113,10 @@ class CarState(CarStateBase):
# Update ACC setpoint. When the setpoint is zero or there's an error, the
# radar sends a set-speed of ~90.69 m/s / 203mph.
- ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS
- if ret.cruiseState.speed > 90:
- ret.cruiseState.speed = 0
+ if self.CP.pcmCruise:
+ ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS
+ if ret.cruiseState.speed > 90:
+ ret.cruiseState.speed = 0
# Update control button states for turn signals and ACC controls.
self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"])
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index b8c9c5d67..560e64ce2 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -43,7 +43,7 @@ class CarInterface(CarInterfaceBase):
else:
ret.networkLocation = NetworkLocation.fwdCamera
- # Global tuning defaults, can be overridden per-vehicle
+ # Global lateral tuning defaults, can be overridden per-vehicle
ret.steerActuatorDelay = 0.05
ret.steerRateCost = 1.0
@@ -115,6 +115,10 @@ class CarInterface(CarInterfaceBase):
ret.mass = 1205 + STD_CARGO_KG
ret.wheelbase = 2.61
+ elif candidate == CAR.AUDI_Q3_MK2:
+ ret.mass = 1623 + STD_CARGO_KG
+ ret.wheelbase = 2.68
+
elif candidate == CAR.SEAT_ATECA_MK1:
ret.mass = 1900 + STD_CARGO_KG
ret.wheelbase = 2.64
@@ -190,6 +194,8 @@ class CarInterface(CarInterfaceBase):
# Vehicle health and operation safety checks
if self.CS.parkingBrakeSet:
events.add(EventName.parkBrake)
+ if self.CS.tsk_status in [6, 7]:
+ events.add(EventName.accFaulted)
# Low speed steer alert hysteresis logic
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 7b3160e62..822326acd 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -79,6 +79,7 @@ class CAR:
TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW VW T-Roc and variants
AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants
AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only)
+ AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants
SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca
SEAT_LEON_MK3 = "SEAT LEON 3RD GEN" # Chassis 5F, Mk3 SEAT Leon and variants
SKODA_KAMIQ_MK1 = "SKODA KAMIQ 1ST GEN" # Chassis NW, Mk1 Skoda Kamiq
@@ -121,6 +122,7 @@ FW_VERSIONS = {
b'\xf1\x8703H906026F \xf1\x896696',
b'\xf1\x8703H906026F \xf1\x899970',
b'\xf1\x8703H906026J \xf1\x896026',
+ b'\xf1\x8703H906026J \xf1\x899971',
b'\xf1\x8703H906026S \xf1\x896693',
b'\xf1\x8703H906026S \xf1\x899970',
],
@@ -147,6 +149,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704E906016A \xf1\x897697',
b'\xf1\x8704E906016AD\xf1\x895758',
+ b'\xf1\x8704E906016CE\xf1\x899096',
b'\xf1\x8704E906023AG\xf1\x891726',
b'\xf1\x8704E906023BN\xf1\x894518',
b'\xf1\x8704E906024K \xf1\x896811',
@@ -187,6 +190,7 @@ FW_VERSIONS = {
b'\xf1\x870CW300042F \xf1\x891604',
b'\xf1\x870CW300043B \xf1\x891601',
b'\xf1\x870CW300044S \xf1\x894530',
+ b'\xf1\x870CW300044T \xf1\x895245',
b'\xf1\x870CW300045 \xf1\x894531',
b'\xf1\x870CW300047D \xf1\x895261',
b'\xf1\x870CW300048J \xf1\x890611',
@@ -269,6 +273,7 @@ FW_VERSIONS = {
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x875Q0907567G \xf1\x890390\xf1\x82\00101',
+ b'\xf1\x875Q0907567J \xf1\x890396\xf1\x82\x0101',
b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\00101',
b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101',
b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\00101',
@@ -552,6 +557,29 @@ FW_VERSIONS = {
b'\xf1\x872Q0907572M \xf1\x890233',
],
},
+ CAR.AUDI_Q3_MK2: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8705E906018N \xf1\x899970',
+ b'\xf1\x8783A906259 \xf1\x890001',
+ b'\xf1\x8783A906259 \xf1\x890005',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x8709G927158CN\xf1\x893608',
+ b'\xf1\x870GC300046F \xf1\x892701',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655BF\xf1\x890403\xf1\x82\x1321211111211200311121232152219321422111',
+ b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300',
+ b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000800',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572R \xf1\x890372',
+ b'\xf1\x872Q0907572T \xf1\x890383',
+ ],
+ },
CAR.SEAT_ATECA_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704E906027KA\xf1\x893749',
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index 7aec15e7b..9a9414cfa 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,6 +1,8 @@
#pragma once
#include
+#include "selfdrive/common/mat.h"
+#include "selfdrive/hardware/hw.h"
const int TRAJECTORY_SIZE = 33;
const int LAT_MPC_N = 16;
@@ -36,10 +38,14 @@ const std::array X_IDXS = {
168.75 , 180.1875, 192.};
const auto X_IDXS_FLOAT = convert_array_to_type(X_IDXS);
-#ifdef __cplusplus
+const int TICI_CAM_WIDTH = 1928;
+
+namespace tici_dm_crop {
+ const int x_offset = -72;
+ const int y_offset = -144;
+ const int width = 954;
+};
-#include "selfdrive/common/mat.h"
-#include "selfdrive/hardware/hw.h"
const mat3 fcam_intrinsic_matrix =
Hardware::EON() ? (mat3){{910., 0., 1164.0 / 2,
0., 910., 874.0 / 2,
@@ -62,5 +68,3 @@ static inline mat3 get_model_yuv_transform(bool bayer = true) {
}};
return bayer ? transform_scale_buffer(transform, db_s) : transform;
}
-
-#endif
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 01bf9a8f4..a405d2160 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -130,11 +130,13 @@ std::unordered_map keys = {
{"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
{"LastGPSPosition", PERSISTENT},
+ {"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
{"LastUpdateException", PERSISTENT},
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"NavSettingTime24h", PERSISTENT},
+ {"NavdRender", PERSISTENT},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"Passive", PERSISTENT},
@@ -151,7 +153,6 @@ std::unordered_map keys = {
{"TrainingVersion", PERSISTENT},
{"UpdateAvailable", CLEAR_ON_MANAGER_START},
{"UpdateFailedCount", CLEAR_ON_MANAGER_START},
- {"UploadRaw", PERSISTENT},
{"Version", PERSISTENT},
{"VisionRadarToggle", PERSISTENT},
{"ApiCache_Device", PERSISTENT},
diff --git a/selfdrive/common/util.cc b/selfdrive/common/util.cc
index f115b13dc..534a7f445 100644
--- a/selfdrive/common/util.cc
+++ b/selfdrive/common/util.cc
@@ -20,6 +20,8 @@
#include
#endif // __linux__
+namespace util {
+
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
@@ -56,8 +58,6 @@ int set_core_affinity(std::vector cores) {
#endif
}
-namespace util {
-
std::string read_file(const std::string& fn) {
std::ifstream f(fn, std::ios::binary | std::ios::in);
if (f.is_open()) {
diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h
index a47cce68f..9a6a4d9bd 100644
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@@ -37,13 +37,12 @@ const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE;
const double METER_TO_MILE = KM_TO_MILE / 1000.0;
const double METER_TO_FOOT = 3.28084;
-void set_thread_name(const char* name);
+namespace util {
+void set_thread_name(const char* name);
int set_realtime_priority(int level);
int set_core_affinity(std::vector cores);
-namespace util {
-
// ***** Time helpers *****
struct tm get_time();
bool time_valid(struct tm sys_time);
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 9f00fac4f..aa786903c 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.8.11"
+#define COMMA_VERSION "0.8.12"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index bbd84d458..19fe328db 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -28,6 +28,7 @@ from selfdrive.locationd.calibrationd import Calibration
from selfdrive.hardware import HARDWARE, TICI, EON
from selfdrive.manager.process_config import managed_processes
+SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
@@ -300,7 +301,7 @@ class Controls:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
- if self.cruise_mismatch_counter > int(1. / DT_CTRL):
+ if self.cruise_mismatch_counter > int(3. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# Check for FCW
@@ -430,7 +431,7 @@ class Controls:
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
- self.soft_disable_timer = int(3 / DT_CTRL)
+ self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
@@ -540,8 +541,9 @@ class Controls:
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
- left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1
- right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1
+ # TODO use desired vs actual curvature
+ left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.20
+ right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
@@ -612,8 +614,8 @@ class Controls:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
- alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric])
- self.AM.add_many(self.sm.frame, alerts, self.enabled)
+ alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer])
+ self.AM.add_many(self.sm.frame, alerts)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index 42d9f457b..c8e702c5c 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -8,7 +8,6 @@ from typing import List, Dict, 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
@@ -33,14 +32,17 @@ class AlertEntry:
start_frame: int = -1
end_frame: int = -1
+ def active(self, frame: int) -> bool:
+ return frame <= self.end_frame
class AlertManager:
def __init__(self):
self.reset()
- self.activealerts: Dict[str, AlertEntry] = defaultdict(AlertEntry)
+ self.alerts: Dict[str, AlertEntry] = defaultdict(AlertEntry)
def reset(self) -> None:
+ self.alert: Optional[Alert] = None
self.alert_type: str = ""
self.alert_text_1: str = ""
self.alert_text_2: str = ""
@@ -50,37 +52,39 @@ class AlertManager:
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:
+ def add_many(self, frame: int, alerts: List[Alert]) -> None:
for alert in alerts:
- self.activealerts[alert.alert_type].alert = alert
- self.activealerts[alert.alert_type].start_frame = frame
- self.activealerts[alert.alert_type].end_frame = frame + int(alert.duration / DT_CTRL)
+ key = alert.alert_type
+ self.alerts[key].alert = alert
+ if not self.alerts[key].active(frame):
+ self.alerts[key].start_frame = frame
+ min_end_frame = self.alerts[key].start_frame + alert.duration
+ self.alerts[key].end_frame = max(frame + 1, min_end_frame)
def process_alerts(self, frame: int, clear_event_type=None) -> None:
current_alert = AlertEntry()
- for k, v in self.activealerts.items():
+ for k, v in self.alerts.items():
if v.alert is None:
continue
- if v.alert.event_type == clear_event_type:
- self.activealerts[k].end_frame = -1
+ if clear_event_type is not None and v.alert.event_type == clear_event_type:
+ self.alerts[k].end_frame = -1
# sort by priority first and then by start_frame
- active = self.activealerts[k].end_frame > frame
greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame)
- if active and greater:
+ if v.active(frame) and greater:
current_alert = v
# clear current alert
self.reset()
- a = current_alert.alert
- if a is not None:
- self.alert_type = a.alert_type
- self.audible_alert = a.audible_alert
- self.visual_alert = a.visual_alert
- self.alert_text_1 = a.alert_text_1
- self.alert_text_2 = a.alert_text_2
- self.alert_status = a.alert_status
- self.alert_size = a.alert_size
- self.alert_rate = a.alert_rate
+ self.alert = current_alert.alert
+ if self.alert is not None:
+ self.alert_type = self.alert.alert_type
+ self.audible_alert = self.alert.audible_alert
+ self.visual_alert = self.alert.visual_alert
+ self.alert_text_1 = self.alert.alert_text_1
+ self.alert_text_2 = self.alert.alert_text_2
+ self.alert_status = self.alert.alert_status
+ self.alert_size = self.alert.alert_size
+ self.alert_rate = self.alert.alert_rate
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index e9ceb7fd7..f173f5fd9 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -5,10 +5,11 @@ from common.realtime import DT_MDL
from selfdrive.config import Conversions as CV
from selfdrive.modeld.constants import T_IDXS
-# kph
-V_CRUISE_MAX = 135
-V_CRUISE_MIN = 8
-V_CRUISE_ENABLE_MIN = 40
+# WARNING: this value was determined based on the model's training distribution,
+# model predictions above this speed can be unpredictable
+V_CRUISE_MAX = 145 # kph
+V_CRUISE_MIN = 8 # kph
+V_CRUISE_ENABLE_MIN = 40 # kph
LAT_MPC_N = 16
LON_MPC_N = 32
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index bd728159b..0048ca661 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -1,5 +1,5 @@
from enum import IntEnum
-from typing import Dict, Union, Callable, Any
+from typing import Dict, Union, Callable
from cereal import log, car
import cereal.messaging as messaging
@@ -123,7 +123,7 @@ class Alert:
self.visual_alert = visual_alert
self.audible_alert = audible_alert
- self.duration = duration
+ self.duration = int(duration / DT_CTRL)
self.alert_rate = alert_rate
self.creation_delay = creation_delay
@@ -139,11 +139,10 @@ class Alert:
class NoEntryAlert(Alert):
- def __init__(self, alert_text_2, audible_alert=AudibleAlert.chimeError,
- visual_alert=VisualAlert.none):
+ def __init__(self, alert_text_2, visual_alert=VisualAlert.none):
super().__init__("openpilot Unavailable", alert_text_2, AlertStatus.normal,
AlertSize.mid, Priority.LOW, visual_alert,
- audible_alert, 3.)
+ AudibleAlert.refuse, 3.)
class SoftDisableAlert(Alert):
@@ -151,7 +150,14 @@ class SoftDisableAlert(Alert):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.userPrompt, AlertSize.full,
Priority.MID, VisualAlert.steerRequired,
- AudibleAlert.chimeWarningRepeatInfinite, 2.),
+ AudibleAlert.warningSoft, 2.),
+
+
+# less harsh version of SoftDisable, where the condition is user-triggered
+class UserSoftDisableAlert(SoftDisableAlert):
+ def __init__(self, alert_text_2):
+ super().__init__(alert_text_2),
+ self.alert_text_1 = "openpilot will disengage"
class ImmediateDisableAlert(Alert):
@@ -159,7 +165,7 @@ class ImmediateDisableAlert(Alert):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.steerRequired,
- AudibleAlert.chimeWarningRepeatInfinite, 4.),
+ AudibleAlert.warningImmediate, 4.),
class EngagementAlert(Alert):
@@ -192,19 +198,39 @@ def get_display_speed(speed_ms: float, metric: bool) -> str:
# ********** alert callback functions **********
-def below_engage_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
+
+AlertCallbackType = Callable[[car.CarParams, messaging.SubMaster, bool, int], Alert]
+
+
+def soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
+ def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ if soft_disable_time < int(0.5 / DT_CTRL):
+ return ImmediateDisableAlert(alert_text_2)
+ return SoftDisableAlert(alert_text_2)
+ return func
+
+
+def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
+ def func(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ if soft_disable_time < int(0.5 / DT_CTRL):
+ return ImmediateDisableAlert(alert_text_2)
+ return UserSoftDisableAlert(alert_text_2)
+ return func
+
+
+def below_engage_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return NoEntryAlert(f"Speed Below {get_display_speed(CP.minEnableSpeed, metric)}")
-def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
+def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return Alert(
f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
"",
AlertStatus.userPrompt, AlertSize.small,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 0.4)
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
-def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
+def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
return Alert(
"Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,
f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}",
@@ -212,7 +238,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
-def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
+def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
gps_integrated = sm['peripheralState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos]
return Alert(
"Poor GPS reception",
@@ -221,21 +247,22 @@ def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Al
Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.)
-def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
+def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
text = "Cruise Mode Disabled"
if CP.carName == "honda":
text = "Main Switch Off"
return NoEntryAlert(text)
-def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
+def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
axes = sm['testJoystick'].axes
gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
return NormalPermanentAlert("Joystick Mode", vals)
-EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = {
+
+EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# ********** events with no alerts **********
EventName.stockFcw: {},
@@ -248,7 +275,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
},
EventName.controlsInitializing: {
- ET.NO_ENTRY: NoEntryAlert("Controls Initializing"),
+ ET.NO_ENTRY: NoEntryAlert("System Initializing"),
},
EventName.startup: {
@@ -282,7 +309,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
},
EventName.invalidLkasSetting: {
- ET.PERMANENT: NormalPermanentAlert("Stock LKAS is turned on",
+ ET.PERMANENT: NormalPermanentAlert("Stock LKAS is on",
"Turn off stock LKAS to engage"),
},
@@ -294,8 +321,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# detects the use of a community feature it switches to dashcam mode
# until these features are allowed using a toggle in settings.
EventName.communityFeatureDisallowed: {
- ET.PERMANENT: NormalPermanentAlert("openpilot Not Available",
- "Enable Community Features in Settings to Engage"),
+ ET.PERMANENT: NormalPermanentAlert("openpilot Unavailable",
+ "Enable Community Features in Settings"),
},
# openpilot doesn't recognize the car. This switches openpilot into a
@@ -321,22 +348,22 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"BRAKE!",
"Risk of Collision",
AlertStatus.critical, AlertSize.full,
- Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.chimeWarningRepeatInfinite, 2.),
+ Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.),
},
EventName.ldw: {
ET.PERMANENT: Alert(
- "TAKE CONTROL",
"Lane Departure Detected",
- AlertStatus.userPrompt, AlertSize.mid,
- Priority.LOW, VisualAlert.ldw, AudibleAlert.chimePrompt, 3.),
+ "",
+ AlertStatus.userPrompt, AlertSize.small,
+ Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.),
},
# ********** events only containing alerts that display while engaged **********
EventName.gasPressed: {
ET.PRE_ENABLE: Alert(
- "openpilot will not brake while gas pressed",
+ "Release Gas Pedal to Engage",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.),
@@ -352,7 +379,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
EventName.vehicleModelInvalid: {
ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"),
- ET.SOFT_DISABLE: SoftDisableAlert("Vehicle Parameter Identification Failed"),
+ ET.SOFT_DISABLE: soft_disable_alert("Vehicle Parameter Identification Failed"),
},
EventName.steerTempUnavailableSilent: {
@@ -360,12 +387,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"Steering Temporarily Unavailable",
"",
AlertStatus.userPrompt, AlertSize.small,
- Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1.),
+ Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.),
},
EventName.preDriverDistracted: {
ET.WARNING: Alert(
- "KEEP EYES ON ROAD: Driver Distracted",
+ "Pay Attention",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
@@ -373,10 +400,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.promptDriverDistracted: {
ET.WARNING: Alert(
- "KEEP EYES ON ROAD",
+ "Pay Attention",
"Driver Distracted",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, .1),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverDistracted: {
@@ -384,12 +411,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"DISENGAGE IMMEDIATELY",
"Driver Distracted",
AlertStatus.critical, AlertSize.full,
- Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeatInfinite, .1),
+ Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.preDriverUnresponsive: {
ET.WARNING: Alert(
- "TOUCH STEERING WHEEL: No Face Detected",
+ "Touch Steering Wheel: No Face Detected",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1, alert_rate=0.75),
@@ -397,10 +424,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.promptDriverUnresponsive: {
ET.WARNING: Alert(
- "TOUCH STEERING WHEEL",
+ "Touch Steering Wheel",
"Driver Unresponsive",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, .1),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverUnresponsive: {
@@ -408,7 +435,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"DISENGAGE IMMEDIATELY",
"Driver Unresponsive",
AlertStatus.critical, AlertSize.full,
- Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeatInfinite, .1),
+ Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.manualRestart: {
@@ -422,7 +449,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.resumeRequired: {
ET.WARNING: Alert(
"STOPPED",
- "Press Resume to Move",
+ "Press Resume to Go",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
},
@@ -452,7 +479,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"Car Detected in Blindspot",
"",
AlertStatus.userPrompt, AlertSize.small,
- Priority.LOW, VisualAlert.none, AudibleAlert.chimePrompt, .1),
+ Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
},
EventName.laneChange: {
@@ -465,10 +492,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.steerSaturated: {
ET.WARNING: Alert(
- "TAKE CONTROL",
+ "Take Control",
"Turn Exceeds Steering Limit",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, 1.),
+ Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 1.),
},
# Thrown when the fan is driven at >50% but is not rotating
@@ -496,49 +523,49 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# ********** events that affect controls state transitions **********
EventName.pcmEnable: {
- ET.ENABLE: EngagementAlert(AudibleAlert.chimeEngage),
+ ET.ENABLE: EngagementAlert(AudibleAlert.engage),
},
EventName.buttonEnable: {
- ET.ENABLE: EngagementAlert(AudibleAlert.chimeEngage),
+ ET.ENABLE: EngagementAlert(AudibleAlert.engage),
},
EventName.pcmDisable: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
},
EventName.buttonCancel: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
},
EventName.brakeHold: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
},
EventName.parkBrake: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
- ET.NO_ENTRY: NoEntryAlert("Park Brake Engaged"),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+ ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
},
EventName.pedalPressed: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
- ET.NO_ENTRY: NoEntryAlert("Pedal Pressed During Attempt",
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+ ET.NO_ENTRY: NoEntryAlert("Pedal Pressed",
visual_alert=VisualAlert.brakePressed),
},
EventName.wrongCarMode: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: wrong_car_mode_alert,
},
EventName.wrongCruiseMode: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
- ET.NO_ENTRY: NoEntryAlert("Enable Adaptive Cruise"),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+ ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"),
},
EventName.steerTempUnavailable: {
- ET.SOFT_DISABLE: SoftDisableAlert("Steering Temporarily Unavailable"),
+ ET.SOFT_DISABLE: soft_disable_alert("Steering Temporarily Unavailable"),
ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"),
},
@@ -575,12 +602,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.overheat: {
ET.PERMANENT: NormalPermanentAlert("System Overheated"),
- ET.SOFT_DISABLE: SoftDisableAlert("System Overheated"),
+ ET.SOFT_DISABLE: soft_disable_alert("System Overheated"),
ET.NO_ENTRY: NoEntryAlert("System Overheated"),
},
EventName.wrongGear: {
- ET.SOFT_DISABLE: SoftDisableAlert("Gear not D"),
+ ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"),
ET.NO_ENTRY: NoEntryAlert("Gear not D"),
},
@@ -591,33 +618,33 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# See https://comma.ai/setup for more information
EventName.calibrationInvalid: {
ET.PERMANENT: NormalPermanentAlert("Calibration Invalid", "Remount Device and Recalibrate"),
- ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Remount Device & Recalibrate"),
+ ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"),
ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"),
},
EventName.calibrationIncomplete: {
ET.PERMANENT: calibration_incomplete_alert,
- ET.SOFT_DISABLE: SoftDisableAlert("Calibration in Progress"),
+ ET.SOFT_DISABLE: soft_disable_alert("Calibration in Progress"),
ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
},
EventName.doorOpen: {
- ET.SOFT_DISABLE: SoftDisableAlert("Door Open"),
+ ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"),
ET.NO_ENTRY: NoEntryAlert("Door Open"),
},
EventName.seatbeltNotLatched: {
- ET.SOFT_DISABLE: SoftDisableAlert("Seatbelt Unlatched"),
+ ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"),
ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
},
EventName.espDisabled: {
- ET.SOFT_DISABLE: SoftDisableAlert("ESP Off"),
+ ET.SOFT_DISABLE: soft_disable_alert("ESP Off"),
ET.NO_ENTRY: NoEntryAlert("ESP Off"),
},
EventName.lowBattery: {
- ET.SOFT_DISABLE: SoftDisableAlert("Low Battery"),
+ ET.SOFT_DISABLE: soft_disable_alert("Low Battery"),
ET.NO_ENTRY: NoEntryAlert("Low Battery"),
},
@@ -626,19 +653,17 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# is thrown. This can mean a service crashed, did not broadcast a message for
# ten times the regular interval, or the average interval is more than 10% too high.
EventName.commIssue: {
- ET.SOFT_DISABLE: SoftDisableAlert("Communication Issue between Processes"),
- ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.SOFT_DISABLE: soft_disable_alert("Communication Issue between Processes"),
+ ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes"),
},
# Thrown when manager detects a service exited unexpectedly while driving
EventName.processNotRunning: {
- ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"),
},
EventName.radarFault: {
- ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"),
+ ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"),
ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"),
},
@@ -646,7 +671,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# is not processing frames fast enough they have to be dropped. This alert is
# thrown when over 20% of frames are dropped.
EventName.modeldLagging: {
- ET.SOFT_DISABLE: SoftDisableAlert("Driving model lagging"),
+ ET.SOFT_DISABLE: soft_disable_alert("Driving model lagging"),
ET.NO_ENTRY: NoEntryAlert("Driving model lagging"),
},
@@ -656,29 +681,27 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# usually means the model has trouble understanding the scene. This is used
# as a heuristic to warn the driver.
EventName.posenetInvalid: {
- ET.SOFT_DISABLE: SoftDisableAlert("Model Output Uncertain"),
+ ET.SOFT_DISABLE: soft_disable_alert("Model Output Uncertain"),
ET.NO_ENTRY: NoEntryAlert("Model Output Uncertain"),
},
# When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we
# alert the driver the device might have fallen from the windshield.
EventName.deviceFalling: {
- ET.SOFT_DISABLE: SoftDisableAlert("Device Fell Off Mount"),
+ ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"),
ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"),
},
EventName.lowMemory: {
- ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
+ ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"),
ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"),
- ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"),
},
EventName.highCpuUsage: {
- #ET.SOFT_DISABLE: SoftDisableAlert("System Malfunction: Reboot Your Device"),
+ #ET.SOFT_DISABLE: soft_disable_alert("System Malfunction: Reboot Your Device"),
#ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"),
- ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"),
},
EventName.accFaulted: {
@@ -712,7 +735,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# Sometimes the USB stack on the device can get into a bad state
# causing the connection to the panda to be lost
EventName.usbError: {
- ET.SOFT_DISABLE: SoftDisableAlert("USB Error: Reboot Your Device"),
+ ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"),
ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device", ""),
ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"),
},
@@ -782,7 +805,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"openpilot Canceled",
"No close lead car",
AlertStatus.normal, AlertSize.mid,
- Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, 3.),
+ Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.),
ET.NO_ENTRY: NoEntryAlert("No Close Lead Car"),
},
@@ -791,16 +814,16 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"openpilot Canceled",
"Speed too low",
AlertStatus.normal, AlertSize.mid,
- Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, 3.),
+ Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.),
},
- # When the car is driving faster than most cars in the training data the model outputs can be unpredictable
+ # When the car is driving faster than most cars in the training data, the model outputs can be unpredictable.
EventName.speedTooHigh: {
ET.WARNING: Alert(
"Speed Too High",
"Model uncertain at this speed",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, 4.),
+ Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.),
ET.NO_ENTRY: NoEntryAlert("Slow down to engage"),
},
diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py
index a641687b4..8fcb9ae7b 100644
--- a/selfdrive/controls/lib/latcontrol_angle.py
+++ b/selfdrive/controls/lib/latcontrol_angle.py
@@ -1,4 +1,5 @@
import math
+
from cereal import log
@@ -21,5 +22,7 @@ class LatControlAngle():
angle_steers_des += params.angleOffsetDeg
angle_log.saturated = False
- angle_log.steeringAngleDeg = angle_steers_des
+ angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+ angle_log.steeringAngleDesiredDeg = angle_steers_des
+
return 0, float(angle_steers_des), angle_log
diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py
index cee391d45..50a8e22b3 100644
--- a/selfdrive/controls/lib/latcontrol_indi.py
+++ b/selfdrive/controls/lib/latcontrol_indi.py
@@ -95,14 +95,16 @@ class LatControlINDI():
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
steers_des += math.radians(params.angleOffsetDeg)
+ indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
+
+ rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
+ indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
+
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.steer_filter.x = 0.0
else:
-
- rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
-
# Expected actuator value
self.steer_filter.update_alpha(self.RC)
self.steer_filter.update(self.output_steer)
diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py
index e445fb3f8..16fffac27 100644
--- a/selfdrive/controls/lib/latcontrol_lqr.py
+++ b/selfdrive/controls/lib/latcontrol_lqr.py
@@ -57,6 +57,7 @@ class LatControlLQR():
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
desired_angle += instant_offset # Only add offset that originates from vehicle model errors
+ lqr_log.steeringAngleDesiredDeg = desired_angle
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
@@ -93,7 +94,7 @@ class LatControlLQR():
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(output_steer, check_saturation, steers_max)
- lqr_log.steeringAngleDeg = angle_steers_k + params.angleOffsetAverageDeg
+ lqr_log.steeringAngleDeg = angle_steers_k
lqr_log.i = self.i_lqr
lqr_log.output = output_steer
lqr_log.lqrOutput = lqr_output
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index 5062df36d..c7730d011 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -24,6 +24,7 @@ class LatControlPID():
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+ pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 74b27f595..0aa2359ae 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -38,7 +38,7 @@ DESIRES = {
}
-class LateralPlanner():
+class LateralPlanner:
def __init__(self, CP, use_lanelines=True, wide_camera=False):
self.use_lanelines = use_lanelines
self.LP = LanePlanner(wide_camera)
@@ -55,8 +55,8 @@ class LateralPlanner():
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
- self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
- self.path_xyz_stds = np.ones((TRAJECTORY_SIZE,3))
+ self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
+ self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
@@ -67,12 +67,8 @@ class LateralPlanner():
def reset_mpc(self, x0=np.zeros(6)):
self.x0 = x0
self.lat_mpc.reset(x0=self.x0)
- self.desired_curvature = 0.0
- self.safe_desired_curvature = 0.0
- self.desired_curvature_rate = 0.0
- self.safe_desired_curvature_rate = 0.0
- def update(self, sm, CP):
+ def update(self, sm):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
measured_curvature = sm['controlsState'].curvature
@@ -110,7 +106,7 @@ class LateralPlanner():
self.lane_change_direction = LaneChangeDirection.none
torque_applied = sm['carState'].steeringPressed and \
- ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
+ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
@@ -124,7 +120,7 @@ class LateralPlanner():
# LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
- self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
+ self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
@@ -167,14 +163,14 @@ class LateralPlanner():
self.LP.rll_prob *= self.lane_change_ll_prob
if self.use_lanelines:
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
- self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
+ self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
else:
d_path_xyz = self.path_xyz
- path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 1.5) * MPC_COST_LAT.PATH
+ path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
- self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
- y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
+ self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
+ y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
self.y_pts = y_pts
@@ -187,11 +183,10 @@ class LateralPlanner():
y_pts,
heading_pts)
# init state for next
- self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:,3])
+ self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
-
- # Check for infeasable MPC solution
- mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:,3])
+ # Check for infeasible MPC solution
+ mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3])
t = sec_since_boot()
if mpc_nans or self.lat_mpc.solution_status != 0:
self.reset_mpc()
@@ -212,8 +207,8 @@ class LateralPlanner():
plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
plan_send.lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]]
- plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N,3]]
- plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N-1]] +[0.0]
+ plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 3]]
+ plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
plan_send.lateralPlan.lProb = float(self.LP.lll_prob)
plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
plan_send.lateralPlan.dProb = float(self.LP.d_prob)
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index f59a6cc0d..beacc518d 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -10,20 +10,20 @@ LongCtrlState = car.CarControl.Actuators.LongControlState
STOPPING_TARGET_SPEED_OFFSET = 0.01
# As per ISO 15622:2018 for all speeds
-ACCEL_MIN_ISO = -3.5 # m/s^2
-ACCEL_MAX_ISO = 2.0 # m/s^2
+ACCEL_MIN_ISO = -3.5 # m/s^2
+ACCEL_MAX_ISO = 2.0 # m/s^2
-def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_pid,
+def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future, v_pid,
output_accel, brake_pressed, cruise_standstill, min_speed_can):
"""Update longitudinal control state machine"""
stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET
stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
(v_ego < CP.vEgoStopping and
- ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or
+ ((v_pid < stopping_target_speed and v_target_future < stopping_target_speed) or
brake_pressed))
- starting_condition = v_target > CP.vEgoStarting and not cruise_standstill
+ starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill
if not active:
long_control_state = LongCtrlState.off
@@ -75,13 +75,10 @@ class LongControl():
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds)
a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
-
- v_target = min(v_target_lower, v_target_upper)
a_target = min(a_target_lower, a_target_upper)
v_target_future = long_plan.speeds[-1]
else:
- v_target = 0.0
v_target_future = 0.0
a_target = 0.0
@@ -103,11 +100,11 @@ class LongControl():
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
- self.v_pid = v_target
+ self.v_pid = long_plan.speeds[0]
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
- prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
+ prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index 577651f8d..e0e0208d6 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -49,15 +49,15 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N
T_IDXS = np.array(T_IDXS_LST)
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
MIN_ACCEL = -3.5
-T_REACT = 1.8
-MAX_BRAKE = 9.81
-
+T_FOLLOW = 1.45
+COMFORT_BRAKE = 2.5
+STOP_DISTANCE = 6.0
def get_stopped_equivalence_factor(v_lead):
- return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE)
+ return (v_lead**2) / (2 * COMFORT_BRAKE)
def get_safe_obstacle_distance(v_ego):
- return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0
+ return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE
def desired_follow_distance(v_ego, v_lead):
return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
@@ -203,7 +203,7 @@ class LongitudinalMpc():
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
self.v_solution = [0.0 for i in range(N+1)]
self.a_solution = [0.0 for i in range(N+1)]
- self.prev_a = self.a_solution
+ self.prev_a = np.array(self.a_solution)
self.j_solution = [0.0 for i in range(N)]
self.yref = np.zeros((N+1, COST_DIM))
for i in range(N):
@@ -255,7 +255,7 @@ class LongitudinalMpc():
self.solver.cost_set(i, 'Zl', Zl)
def set_cur_state(self, v, a):
- if abs(self.x0[1] - v) > 1.:
+ if abs(self.x0[1] - v) > 2.:
self.x0[1] = v
self.x0[2] = a
for i in range(0, N+1):
@@ -298,8 +298,9 @@ class LongitudinalMpc():
self.cruise_min_a = min_a
self.cruise_max_a = max_a
- def update(self, carstate, radarstate, v_cruise):
+ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
v_ego = self.x0[1]
+ a_ego = self.x0[2]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
@@ -317,17 +318,20 @@ class LongitudinalMpc():
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
- cruise_lower_bound = v_ego + (3/4) * self.cruise_min_a * T_IDXS
- cruise_upper_bound = v_ego + (3/4) * self.cruise_max_a * T_IDXS
+ v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
+ v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
- cruise_lower_bound,
- cruise_upper_bound)
- cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped)
+ v_lower,
+ v_upper)
+ cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
self.params[:,2] = np.min(x_obstacles, axis=1)
- self.params[:,3] = np.copy(self.prev_a)
+ if prev_accel_constraint:
+ self.params[:,3] = np.copy(self.prev_a)
+ else:
+ self.params[:,3] = a_ego
self.run()
if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
@@ -348,7 +352,7 @@ class LongitudinalMpc():
x_obstacle = 1e5*np.ones((N+1))
self.params = np.concatenate([self.accel_limit_arr,
x_obstacle[:,None],
- self.prev_a], axis=1)
+ self.prev_a[:,None]], axis=1)
self.run()
@@ -367,7 +371,7 @@ class LongitudinalMpc():
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]
- self.prev_a = interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
+ self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
t = sec_since_boot()
if self.solution_status != 0:
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 051a68a74..41bae4c47 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -14,7 +14,7 @@ from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
from selfdrive.swaglog import cloudlog
LON_MPC_STEP = 0.2 # first step is 0.2s
-AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
+AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 15., 25., 40.]
@@ -35,13 +35,13 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
"""
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
- a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
- a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
+ a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
+ a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
return [a_target[0], min(a_target[1], a_x_allowed)]
-class Planner():
+class Planner:
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
@@ -50,14 +50,13 @@ class Planner():
self.v_desired = init_v
self.a_desired = init_a
- self.alpha = np.exp(-DT_MDL/2.0)
+ self.alpha = np.exp(-DT_MDL / 2.0)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
-
- def update(self, sm, CP):
+ def update(self, sm):
v_ego = sm['carState'].vEgo
a_ego = sm['carState'].aEgo
@@ -68,10 +67,12 @@ class Planner():
long_control_state = sm['controlsState'].longControlState
force_slow_decel = sm['controlsState'].forceDecel
- enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
- if not enabled or sm['carState'].gasPressed:
+ prev_accel_constraint = True
+ if long_control_state == LongCtrlState.off or sm['carState'].gasPressed:
self.v_desired = v_ego
self.a_desired = a_ego
+ # Smoothly changing between accel trajectory is only relevant when OP is driving
+ prev_accel_constraint = False
# Prevent divergence, smooth in current v_ego
self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
@@ -88,12 +89,12 @@ class Planner():
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired, self.a_desired)
- self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
+ self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint)
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
- #TODO counter is only needed because radar is glitchy, remove once radar is gone
+ # TODO counter is only needed because radar is glitchy, remove once radar is gone
self.fcw = self.mpc.crash_cnt > 5
if self.fcw:
cloudlog.info("FCW triggered")
@@ -101,7 +102,7 @@ class Planner():
# Interpolate 0.05 seconds and save as starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
- self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev)/2.0
+ self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 88734b3c1..02f1c19a7 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -36,9 +36,9 @@ def plannerd_thread(sm=None, pm=None):
sm.update()
if sm.updated['modelV2']:
- lateral_planner.update(sm, CP)
+ lateral_planner.update(sm)
lateral_planner.publish(sm, pm)
- longitudinal_planner.update(sm, CP)
+ longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)
diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index f85d7c0e6..e42b7532b 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -1,6 +1,6 @@
"""Install exception handler for process crash."""
from selfdrive.swaglog import cloudlog
-from selfdrive.version import version
+from selfdrive.version import get_version
import sentry_sdk
from sentry_sdk.integrations.threading import ThreadingIntegration
@@ -24,4 +24,4 @@ def bind_extra(**kwargs) -> None:
def init() -> None:
sentry_sdk.init("https://a8dc76b5bfb34908a601d67e2aa8bcf9@o33823.ingest.sentry.io/77924",
default_integrations=False, integrations=[ThreadingIntegration(propagate_hub=True)],
- release=version)
+ release=get_version())
diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py
index 5931fda64..aafc696b4 100755
--- a/selfdrive/debug/cpu_usage_stat.py
+++ b/selfdrive/debug/cpu_usage_stat.py
@@ -66,7 +66,7 @@ if __name__ == "__main__":
for p in psutil.process_iter():
if p == psutil.Process():
continue
- matched = any([l for l in p.cmdline() if any([pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)])])
+ matched = any(l for l in p.cmdline() if any(pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)))
if matched:
k = ' '.join(p.cmdline())
print('Add monitored proc:', k)
@@ -119,5 +119,5 @@ if __name__ == "__main__":
for x in l:
print(x[2])
print('avg sum: {0:.2%} over {1} samples {2} seconds\n'.format(
- sum([stat['avg']['total'] for k, stat in stats.items()]), i, i * SLEEP_INTERVAL
+ sum(stat['avg']['total'] for k, stat in stats.items()), i, i * SLEEP_INTERVAL
))
diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py
index 8b01a73b0..f28d5373f 100755
--- a/selfdrive/debug/cycle_alerts.py
+++ b/selfdrive/debug/cycle_alerts.py
@@ -7,18 +7,31 @@ import time
from cereal import car, log
import cereal.messaging as messaging
+from common.realtime import DT_CTRL
from selfdrive.car.honda.interface import CarInterface
from selfdrive.controls.lib.events import ET, EVENTS, Events
from selfdrive.controls.lib.alertmanager import AlertManager
EventName = car.CarEvent.EventName
-def cycle_alerts(duration=2000, is_metric=False):
- alerts = list(EVENTS.keys())
- print(alerts)
+def cycle_alerts(duration=200, is_metric=False):
+ # all alerts
+ #alerts = list(EVENTS.keys())
- alerts = [EventName.preDriverDistracted, EventName.promptDriverDistracted, EventName.driverDistracted]
- #alerts = [EventName.preLaneChangeLeft, EventName.preLaneChangeRight]
+ # this plays each type of audible alert
+ alerts = [
+ (EventName.buttonEnable, ET.ENABLE),
+ (EventName.buttonCancel, ET.USER_DISABLE),
+ (EventName.wrongGear, ET.NO_ENTRY),
+
+ (EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
+ (EventName.accFaulted, ET.IMMEDIATE_DISABLE),
+
+ # DM sequence
+ (EventName.preDriverDistracted, ET.WARNING),
+ (EventName.promptDriverDistracted, ET.WARNING),
+ (EventName.driverDistracted, ET.WARNING),
+ ]
CP = CarInterface.get_params("HONDA CIVIC 2016")
sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration',
@@ -30,43 +43,45 @@ def cycle_alerts(duration=2000, is_metric=False):
AM = AlertManager()
frame = 0
- idx, last_alert_millis = 0, 0
- while 1:
- if frame % duration == 0:
- idx = (idx + 1) % len(alerts)
- events.clear()
- events.add(alerts[idx])
-
-
+ while True:
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.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
- pm.send('controlsState', dat)
+ for alert, et in alerts:
+ events.clear()
+ events.add(alert)
- dat = messaging.new_message()
- dat.init('deviceState')
- dat.deviceState.started = True
- pm.send('deviceState', dat)
+ a = events.create_alerts([et, ], [CP, sm, is_metric, 0])
+ AM.add_many(frame, a)
+ AM.process_alerts(frame)
+ print(AM.alert)
+ for _ in range(duration):
+ dat = messaging.new_message()
+ dat.init('controlsState')
+ dat.controlsState.enabled = True
- dat = messaging.new_message('pandaStates', 1)
- dat.pandaStates[0].ignitionLine = True
- dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
- pm.send('pandaStates', dat)
+ 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
+ pm.send('controlsState', dat)
- time.sleep(0.01)
+ dat = messaging.new_message()
+ dat.init('deviceState')
+ dat.deviceState.started = True
+ pm.send('deviceState', dat)
+
+ dat = messaging.new_message('pandaStates', 1)
+ dat.pandaStates[0].ignitionLine = True
+ dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
+ pm.send('pandaStates', dat)
+
+ frame += 1
+ time.sleep(DT_CTRL)
if __name__ == '__main__':
cycle_alerts()
diff --git a/selfdrive/hardware/eon/androidd.py b/selfdrive/hardware/eon/androidd.py
index 6de00d8e7..b836eb012 100755
--- a/selfdrive/hardware/eon/androidd.py
+++ b/selfdrive/hardware/eon/androidd.py
@@ -52,25 +52,26 @@ def main():
cloudlog.event("android service pid changed", proc=p, cur=cur[p], prev=procs[p])
procs.update(cur)
- # check modem state
- state = get_modem_state()
- if state != modem_state and not modem_killed:
- cloudlog.event("modem state changed", state=state)
- modem_state = state
+ if os.path.exists(MODEM_PATH):
+ # check modem state
+ state = get_modem_state()
+ if state != modem_state and not modem_killed:
+ cloudlog.event("modem state changed", state=state)
+ modem_state = state
- # check modem crashes
- cnt = get_modem_crash_count()
- if cnt is not None:
- if cnt > crash_count:
- cloudlog.event("modem crash", count=cnt)
- crash_count = cnt
+ # check modem crashes
+ cnt = get_modem_crash_count()
+ if cnt is not None:
+ if cnt > crash_count:
+ cloudlog.event("modem crash", count=cnt)
+ crash_count = cnt
- # handle excessive modem crashes
- if crash_count > MAX_MODEM_CRASHES and not modem_killed:
- cloudlog.event("killing modem")
- with open("/sys/kernel/debug/msm_subsys/modem", "w") as f:
- f.write("put")
- modem_killed = True
+ # handle excessive modem crashes
+ if crash_count > MAX_MODEM_CRASHES and not modem_killed:
+ cloudlog.event("killing modem")
+ with open("/sys/kernel/debug/msm_subsys/modem", "w") as f:
+ f.write("put")
+ modem_killed = True
time.sleep(1)
diff --git a/selfdrive/hardware/tici/agnos.json b/selfdrive/hardware/tici/agnos.json
index 14f5567c7..f22575a33 100644
--- a/selfdrive/hardware/tici/agnos.json
+++ b/selfdrive/hardware/tici/agnos.json
@@ -1,10 +1,10 @@
[
{
"name": "boot",
- "url": "https://commadist.azureedge.net/agnosupdate/boot-ab4b6f64a90617ddbebe250f977616d70a25864f82c9c6ea9d88ebc5fe80e37c.img.xz",
- "hash": "ab4b6f64a90617ddbebe250f977616d70a25864f82c9c6ea9d88ebc5fe80e37c",
- "hash_raw": "ab4b6f64a90617ddbebe250f977616d70a25864f82c9c6ea9d88ebc5fe80e37c",
- "size": 14772224,
+ "url": "https://commadist.azureedge.net/agnosupdate/boot-5ad783213b7de18400f5fd3609fe75677fec80780ae31cbdf5a8ee7106675d7c.img.xz",
+ "hash": "5ad783213b7de18400f5fd3609fe75677fec80780ae31cbdf5a8ee7106675d7c",
+ "hash_raw": "5ad783213b7de18400f5fd3609fe75677fec80780ae31cbdf5a8ee7106675d7c",
+ "size": 14768128,
"sparse": false,
"full_check": true,
"has_ab": true
@@ -41,9 +41,9 @@
},
{
"name": "system",
- "url": "https://commadist.azureedge.net/agnosupdate/system-a778d523851d88a78ad7440ab602a80e09decdd1877f9f31ea36a7d7f15970dd.img.xz",
- "hash": "540ee7184cc6d8c14f94e652a062027dcc7559e47f4b347b6f8abac570521ec6",
- "hash_raw": "a778d523851d88a78ad7440ab602a80e09decdd1877f9f31ea36a7d7f15970dd",
+ "url": "https://commadist.azureedge.net/agnosupdate/system-0fee88a42385d067756e9b25d57a80228835310deb7b5eef7b7bed5c22c45515.img.xz",
+ "hash": "a043cba1ae08ca6d17704a8a0978b1e27e5bc79abb85b97efd35203ae26ae1ea",
+ "hash_raw": "0fee88a42385d067756e9b25d57a80228835310deb7b5eef7b7bed5c22c45515",
"size": 10737418240,
"sparse": true,
"full_check": false,
diff --git a/selfdrive/hardware/tici/agnos.py b/selfdrive/hardware/tici/agnos.py
index d62d4dbc5..a28b13ac4 100755
--- a/selfdrive/hardware/tici/agnos.py
+++ b/selfdrive/hardware/tici/agnos.py
@@ -5,6 +5,7 @@ import hashlib
import requests
import struct
import subprocess
+import time
import os
from typing import Generator
@@ -224,7 +225,6 @@ def verify_agnos_update(manifest_path: str, target_slot_number: int) -> bool:
if __name__ == "__main__":
import logging
- import time
import argparse
parser = argparse.ArgumentParser(description="Flash and verify AGNOS update",
diff --git a/selfdrive/hardware/tici/amplifier.py b/selfdrive/hardware/tici/amplifier.py
index 99b6d0598..a8b279863 100755
--- a/selfdrive/hardware/tici/amplifier.py
+++ b/selfdrive/hardware/tici/amplifier.py
@@ -31,17 +31,18 @@ BASE_CONFIG = [
AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000),
AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100),
AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100),
- AmpConfig("Right speaker output volume", 0x1a, 0x3E, 0, 0b00011111),
+ AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000),
AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000),
AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111),
AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111),
AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001),
AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000),
- AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000),
+ AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000),
AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000),
+ AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000),
AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001),
- AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010),
+ AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000),
AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111),
AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000),
@@ -62,11 +63,11 @@ BASE_CONFIG = [
AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000),
]
-BASE_CONFIG += configs_from_eq_params(0x84, EQParams(0x65C4, 0xC07C, 0x3D66, 0x07D9, 0x120F))
+BASE_CONFIG += configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656))
BASE_CONFIG += configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF))
-BASE_CONFIG += configs_from_eq_params(0x98, EQParams(0x2822, 0xC1C7, 0x3B50, 0x0EF8, 0x180A))
-BASE_CONFIG += configs_from_eq_params(0xA2, EQParams(0x1009, 0xC5C2, 0x271F, 0x1A87, 0x32A6))
-BASE_CONFIG += configs_from_eq_params(0xAC, EQParams(0x2000, 0xCA1E, 0x4000, 0x2287, 0x0000))
+BASE_CONFIG += configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42))
+BASE_CONFIG += configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807))
+BASE_CONFIG += configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B))
class Amplifier:
AMP_I2C_BUS = 0
diff --git a/selfdrive/hardware/tici/hardware.h b/selfdrive/hardware/tici/hardware.h
index abd7e9297..c37dbb0a3 100644
--- a/selfdrive/hardware/tici/hardware.h
+++ b/selfdrive/hardware/tici/hardware.h
@@ -9,8 +9,8 @@
class HardwareTici : public HardwareNone {
public:
- static constexpr float MAX_VOLUME = 1.0;
- static constexpr float MIN_VOLUME = 0.4;
+ static constexpr float MAX_VOLUME = 0.9;
+ static constexpr float MIN_VOLUME = 0.2;
static bool TICI() { return true; }
static std::string get_os_version() {
return "AGNOS " + util::read_file("/VERSION");
diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py
index 5b7596df2..855eee908 100644
--- a/selfdrive/hardware/tici/hardware.py
+++ b/selfdrive/hardware/tici/hardware.py
@@ -303,6 +303,13 @@ class Tici(HardwareBase):
val = "0" if powersave_enabled else "1"
os.system(f"sudo su -c 'echo {val} > /sys/devices/system/cpu/cpu{i}/online'")
+ for n in ('0', '4'):
+ gov = 'userspace' if powersave_enabled else 'performance'
+ os.system(f"sudo su -c 'echo {gov} > /sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor'")
+
+ if powersave_enabled:
+ os.system(f"sudo su -c 'echo 979200 > /sys/devices/system/cpu/cpufreq/policy{n}/scaling_setspeed'")
+
def get_gpu_usage_percent(self):
try:
used, total = open('/sys/class/kgsl/kgsl-3d0/gpubusy').read().strip().split()
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index 8bdc30a8d..0350625a9 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -18,6 +18,7 @@ const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0;
+const double SANE_GPS_UNCERTAINTY = 1500.0; // m
static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) {
VectorXd res(floatlist.size());
@@ -130,16 +131,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
Vector3d nans = Vector3d(NAN, NAN, NAN);
// write measurements to msg
- init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->last_gps_fix > 0);
- init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->last_gps_fix > 0);
- init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->last_gps_fix > 0);
- init_measurement(fix.initVelocityNED(), ned_vel, nans, this->last_gps_fix > 0);
+ init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
+ init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
+ init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
+ init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
- init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->last_gps_fix > 0);
- init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->last_gps_fix > 0);
- init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->last_gps_fix > 0);
- init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->last_gps_fix > 0);
+ init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
+ init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
+ init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->gps_mode);
+ init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
@@ -243,27 +244,39 @@ void Localizer::handle_sensors(double current_time, const capnp::List observe current obs with reasonable STD
+ this->kf->predict(current_time);
+
+ VectorXd current_x = this->kf->get_x();
+ VectorXd ecef_pos = current_x.segment(STATE_ECEF_POS_START);
+ VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START);
+ MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov();
+ MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov();
+
+ this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+ this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
// ignore the message if the fix is invalid
- if (log.getFlags() % 2 == 0) {
- return;
- }
- // Sanity checks
- if ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)) {
- return;
- }
+ bool gps_invalid_flag = (log.getFlags() % 2 == 0);
+ bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
+ bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
+ bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
+ bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
- if ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) {
- return;
- }
-
- if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK) {
+ if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){
+ this->determine_gps_mode(current_time);
return;
}
// Process message
this->last_gps_fix = current_time;
+ this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique(geodetic);
@@ -273,7 +286,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
this->unix_timestamp_millis = log.getTimestamp();
- double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm();
+ double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
@@ -290,11 +303,11 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
- this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
+ this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 100.0) {
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
- this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
+ this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
}
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
@@ -358,7 +371,8 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra
void Localizer::reset_kalman(double current_time) {
VectorXd init_x = this->kf->get_initial_x();
- this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
+ MatrixXdr init_P = this->kf->get_initial_P();
+ this->reset_kalman(current_time, init_x, init_P);
}
void Localizer::finite_check(double current_time) {
@@ -390,13 +404,27 @@ void Localizer::update_reset_tracker() {
}
}
-void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) {
+void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R) {
// too nonlinear to init on completely wrong
- VectorXd init_x = this->kf->get_initial_x();
+ VectorXd current_x = this->kf->get_x();
+ MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
- init_x.segment<4>(3) = init_orient;
- init_x.head(3) = init_pos;
+ MatrixXdr reset_orientation_P = this->kf->get_reset_orientation_P();
+ int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
+ current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient;
+ current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel;
+ current_x.segment(STATE_ECEF_POS_START) = init_pos;
+
+ init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
+ init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
+ init_P.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
+ init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
+
+ this->reset_kalman(current_time, current_x, init_P);
+}
+
+void Localizer::reset_kalman(double current_time, VectorXd init_x, MatrixXdr init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
@@ -447,14 +475,28 @@ bool Localizer::isGpsOK() {
return this->kf->get_filter_time() - this->last_gps_fix < 1.0;
}
+void Localizer::determine_gps_mode(double current_time) {
+ // 1. If the pos_std is greater than what's not acceptible and localizer is in gps-mode, reset to no-gps-mode
+ // 2. If the pos_std is greater than what's not acceptible and localizer is in no-gps-mode, fake obs
+ // 3. If the pos_std is smaller than what's not acceptible, let gps-mode be whatever it is
+ VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
+ if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
+ if (this->gps_mode){
+ this->gps_mode = false;
+ this->reset_kalman(current_time);
+ }
+ else{
+ this->input_fake_gps_observations(current_time);
+ }
+ }
+}
+
int Localizer::locationd_thread() {
const std::initializer_list service_list =
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
PubMaster pm({ "liveLocationKalman" });
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
- Params params;
-
while (!do_exit) {
sm.update();
for (const char* service : service_list) {
@@ -479,8 +521,8 @@ int Localizer::locationd_thread() {
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
- std::thread([¶ms] (const std::string gpsjson) {
- params.put("LastGPSPosition", gpsjson);
+ std::thread([] (const std::string gpsjson) {
+ Params().put("LastGPSPosition", gpsjson);
}, lastGPSPosJSON).detach();
}
}
@@ -489,7 +531,7 @@ int Localizer::locationd_thread() {
}
int main() {
- set_realtime_priority(5);
+ util::set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();
diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h
index 60fed112c..9bc864bf6 100755
--- a/selfdrive/locationd/locationd.h
+++ b/selfdrive/locationd/locationd.h
@@ -27,11 +27,13 @@ public:
int locationd_thread();
void reset_kalman(double current_time = NAN);
- void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
+ void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R);
+ void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P);
void finite_check(double current_time = NAN);
void time_check(double current_time = NAN);
void update_reset_tracker();
bool isGpsOK();
+ void determine_gps_mode(double current_time);
kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
bool inputsOK, bool sensorsOK, bool gpsOK);
@@ -49,6 +51,8 @@ public:
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
+ void input_fake_gps_observations(double current_time);
+
private:
std::unique_ptr kf;
@@ -67,4 +71,5 @@ private:
double last_gps_fix = 0;
double reset_tracker = 0.0;
bool device_fell = false;
+ bool gps_mode = false;
};
diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc
index 541c0f773..5ff0f2699 100755
--- a/selfdrive/locationd/models/live_kf.cc
+++ b/selfdrive/locationd/models/live_kf.cc
@@ -28,11 +28,14 @@ std::vector> get_vec_mapmat(std::vector& mat_ve
}
LiveKalman::LiveKalman() {
- this->dim_state = 26;
- this->dim_state_err = 25;
+ this->dim_state = live_initial_x.rows();
+ this->dim_state_err = live_initial_P_diag.rows();
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
+ this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
+ this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
+ this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
this->Q = live_Q_diag.asDiagonal();
for (auto& pair : live_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
@@ -87,6 +90,10 @@ std::optional LiveKalman::predict_and_observe(double t, int kind, std:
return r;
}
+void LiveKalman::predict(double t) {
+ this->filter->predict(t);
+}
+
Eigen::VectorXd LiveKalman::get_initial_x() {
return this->initial_x;
}
@@ -95,6 +102,18 @@ MatrixXdr LiveKalman::get_initial_P() {
return this->initial_P;
}
+MatrixXdr LiveKalman::get_fake_gps_pos_cov() {
+ return this->fake_gps_pos_cov;
+}
+
+MatrixXdr LiveKalman::get_fake_gps_vel_cov() {
+ return this->fake_gps_vel_cov;
+}
+
+MatrixXdr LiveKalman::get_reset_orientation_P() {
+ return this->reset_orientation_P;
+}
+
MatrixXdr LiveKalman::H(VectorXd in) {
assert(in.size() == 6);
Matrix res;
diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h
index 4cd4756c9..06ec3854c 100755
--- a/selfdrive/locationd/models/live_kf.h
+++ b/selfdrive/locationd/models/live_kf.h
@@ -36,9 +36,13 @@ public:
std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind);
std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind);
std::optional predict_and_update_odo_rot(std::vector rot, double t, int kind);
+ void predict(double t);
Eigen::VectorXd get_initial_x();
MatrixXdr get_initial_P();
+ MatrixXdr get_fake_gps_pos_cov();
+ MatrixXdr get_fake_gps_vel_cov();
+ MatrixXdr get_reset_orientation_P();
MatrixXdr H(Eigen::VectorXd in);
@@ -52,6 +56,9 @@ private:
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
+ MatrixXdr fake_gps_pos_cov;
+ MatrixXdr fake_gps_vel_cov;
+ MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map obs_noise;
};
diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py
index 75415a284..fa5294593 100755
--- a/selfdrive/locationd/models/live_kf.py
+++ b/selfdrive/locationd/models/live_kf.py
@@ -26,10 +26,8 @@ class States():
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
- ODO_SCALE = slice(16, 17) # odometer scale
- ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2
- IMU_OFFSET = slice(20, 23) # imu offset angles in radians
- ACC_BIAS = slice(23, 26)
+ ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
+ ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
@@ -37,10 +35,8 @@ class States():
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
- ODO_SCALE_ERR = slice(15, 16)
- ACCELERATION_ERR = slice(16, 19)
- IMU_OFFSET_ERR = slice(19, 22)
- ACC_BIAS_ERR = slice(22, 25)
+ ACCELERATION_ERR = slice(15, 18)
+ ACC_BIAS_ERR = slice(18, 21)
class LiveKalman():
@@ -51,38 +47,37 @@ class LiveKalman():
0, 0, 0,
0, 0, 0,
0, 0, 0,
- 1,
- 0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
- initial_P_diag = np.array([1e3**2, 1e3**2, 1e3**2,
- 0.5**2, 0.5**2, 0.5**2,
+ initial_P_diag = np.array([10**2, 10**2, 10**2,
+ 0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
- 0.02**2,
100**2, 100**2, 100**2,
- 0.01**2, 0.01**2, 0.01**2,
0.01**2, 0.01**2, 0.01**2])
+ # state covariance when resetting midway in a segment
+ reset_orientation_diag = np.array([1**2, 1**2, 1**2])
+
+ # fake observation covariance, to ensure the uncertainty estimate of the filter is under control
+ fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
+ fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
+
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
- (0.02 / 100)**2,
3**2, 3**2, 3**2,
- (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
0.005**2, 0.005**2, 0.005**2])
- obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]),
- ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
+ obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
- ObservationKind.IMU_FRAME: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
@@ -105,7 +100,6 @@ class LiveKalman():
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
- imu_angles = state[States.IMU_OFFSET, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
@@ -140,7 +134,6 @@ class LiveKalman():
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
-
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
@@ -183,7 +176,6 @@ class LiveKalman():
#
# Observation functions
#
- # imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
@@ -194,19 +186,12 @@ class LiveKalman():
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
-
- speed = sp.sqrt(vx**2 + vy**2 + vz**2 + 1e-6)
- h_speed_sym = sp.Matrix([speed])
-
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
- h_imu_frame_sym = sp.Matrix(imu_angles)
-
h_relative_motion = sp.Matrix(quat_rot.T * v)
- obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
- [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+ obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
@@ -214,12 +199,11 @@ class LiveKalman():
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
- [h_imu_frame_sym, ObservationKind.IMU_FRAME, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
- h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T*(sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
+ h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
@@ -241,6 +225,9 @@ class LiveKalman():
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
+ live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
+ live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
+ live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
live_kf_header += "static const std::unordered_map> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():
diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc
index 52c3906f1..9e32c7b07 100644
--- a/selfdrive/locationd/ublox_msg.cc
+++ b/selfdrive/locationd/ublox_msg.cc
@@ -37,11 +37,11 @@ inline bool UbloxMsgParser::valid_cheksum() {
ck_b = (ck_b + ck_a) & 0xFF;
}
if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
- LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]);
+ LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
return false;
}
if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
- LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]);
+ LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
return false;
}
return true;
diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc
index bcf33b3f7..ae07284c8 100644
--- a/selfdrive/locationd/ubloxd.cc
+++ b/selfdrive/locationd/ubloxd.cc
@@ -17,14 +17,14 @@ int main() {
PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
- Context * context = Context::create();
- SubSocket * subscriber = SubSocket::create(context, "ubloxRaw");
+ std::unique_ptr context(Context::create());
+ std::unique_ptr subscriber(SubSocket::create(context.get(), "ubloxRaw"));
assert(subscriber != NULL);
subscriber->setTimeout(100);
while (!do_exit) {
- Message * msg = subscriber->receive();
+ std::unique_ptr msg(subscriber->receive());
if (!msg) {
if (errno == EINTR) {
do_exit = true;
@@ -32,7 +32,7 @@ int main() {
continue;
}
- capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg));
+ capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot();
auto ubloxRaw = event.getUbloxRaw();
@@ -58,11 +58,7 @@ int main() {
}
bytes_consumed += bytes_consumed_this_time;
}
- delete msg;
}
- delete subscriber;
- delete context;
-
return 0;
}
diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript
index 7e41c9d3e..2adcfb846 100644
--- a/selfdrive/loggerd/SConscript
+++ b/selfdrive/loggerd/SConscript
@@ -24,8 +24,8 @@ if arch == "Darwin":
del libs[libs.index('OpenCL')]
env['FRAMEWORKS'] = ['OpenCL']
-env.Program(src, LIBS=libs)
+env.Program('loggerd', ['main.cc'] + src, LIBS=libs)
env.Program('bootlog.cc', LIBS=libs)
if GetOption('test'):
- env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc', env.Object('logger_util', '#/selfdrive/ui/replay/util.cc')], LIBS=[libs] + ['curl', 'crypto'])
+ env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_loggerd.cc', 'tests/test_logger.cc', env.Object('logger_util', '#/selfdrive/ui/replay/util.cc')] + src, LIBS=[libs] + ['curl', 'crypto', 'bz2'])
diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc
index 481b3ee47..520995837 100644
--- a/selfdrive/loggerd/bootlog.cc
+++ b/selfdrive/loggerd/bootlog.cc
@@ -56,6 +56,7 @@ static kj::Array build_boot_log() {
}
int main(int argc, char** argv) {
+ clear_locks(LOG_ROOT);
const std::string path = LOG_ROOT + "/boot/" + logger_get_route_name() + ".bz2";
LOGW("bootlog to %s", path.c_str());
diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc
index a73fefb8c..81cfd131f 100644
--- a/selfdrive/loggerd/logger.cc
+++ b/selfdrive/loggerd/logger.cc
@@ -2,6 +2,7 @@
#include
#include
+#include
#include
#include
@@ -266,3 +267,15 @@ void lh_close(LoggerHandle* h) {
}
pthread_mutex_unlock(&h->lock);
}
+
+int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) {
+ const char* dot = strrchr(fpath, '.');
+ if (dot && strcmp(dot, ".lock") == 0) {
+ unlink(fpath);
+ }
+ return 0;
+}
+
+void clear_locks(const std::string log_root) {
+ ftw(log_root.c_str(), clear_locks_fn, 16);
+}
diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h
index bdda9d691..e85d7810e 100644
--- a/selfdrive/loggerd/logger.h
+++ b/selfdrive/loggerd/logger.h
@@ -96,3 +96,4 @@ void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog);
void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog);
void lh_close(LoggerHandle* h);
+void clear_locks(const std::string log_root);
diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc
index f6d89b809..37f03ef4e 100644
--- a/selfdrive/loggerd/loggerd.cc
+++ b/selfdrive/loggerd/loggerd.cc
@@ -1,153 +1,44 @@
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "cereal/messaging/messaging.h"
-#include "cereal/services.h"
-#include "cereal/visionipc/visionipc.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "selfdrive/camerad/cameras/camera_common.h"
-#include "selfdrive/common/params.h"
-#include "selfdrive/common/swaglog.h"
-#include "selfdrive/common/timing.h"
-#include "selfdrive/common/util.h"
-#include "selfdrive/hardware/hw.h"
-
-#include "selfdrive/loggerd/encoder.h"
-#include "selfdrive/loggerd/logger.h"
-#if defined(QCOM) || defined(QCOM2)
-#include "selfdrive/loggerd/omx_encoder.h"
-#define Encoder OmxEncoder
-#else
-#include "selfdrive/loggerd/raw_logger.h"
-#define Encoder RawLogger
-#endif
-
-namespace {
-
-constexpr int MAIN_FPS = 20;
-const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000;
-const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000;
-
-#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead
-
-const bool LOGGERD_TEST = getenv("LOGGERD_TEST");
-const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60;
+#include "selfdrive/loggerd/loggerd.h"
ExitHandler do_exit;
-const LogCameraInfo cameras_logged[] = {
- {
- .type = RoadCam,
- .stream_type = VISION_STREAM_YUV_BACK,
- .filename = "fcamera.hevc",
- .frame_packet_name = "roadCameraState",
- .fps = MAIN_FPS,
- .bitrate = MAIN_BITRATE,
- .is_h265 = true,
- .downscale = false,
- .has_qcamera = true,
- .trigger_rotate = true,
- .enable = true,
- .record = true,
- },
- {
- .type = DriverCam,
- .stream_type = VISION_STREAM_YUV_FRONT,
- .filename = "dcamera.hevc",
- .frame_packet_name = "driverCameraState",
- .fps = MAIN_FPS, // on EONs, more compressed this way
- .bitrate = DCAM_BITRATE,
- .is_h265 = true,
- .downscale = false,
- .has_qcamera = false,
- .trigger_rotate = Hardware::TICI(),
- .enable = !Hardware::PC(),
- .record = Params().getBool("RecordFront"),
- },
- {
- .type = WideRoadCam,
- .stream_type = VISION_STREAM_YUV_WIDE,
- .filename = "ecamera.hevc",
- .frame_packet_name = "wideRoadCameraState",
- .fps = MAIN_FPS,
- .bitrate = MAIN_BITRATE,
- .is_h265 = true,
- .downscale = false,
- .has_qcamera = false,
- .trigger_rotate = true,
- .enable = Hardware::TICI(),
- .record = Hardware::TICI(),
- },
-};
-const LogCameraInfo qcam_info = {
- .filename = "qcamera.ts",
- .fps = MAIN_FPS,
- .bitrate = 256000,
- .is_h265 = false,
- .downscale = true,
- .frame_width = Hardware::TICI() ? 526 : 480,
- .frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same?
-};
-
-struct LoggerdState {
- Context *ctx;
- LoggerState logger = {};
- char segment_path[4096];
- std::mutex rotate_lock;
- std::condition_variable rotate_cv;
- std::atomic rotate_segment;
- std::atomic last_camera_seen_tms;
- std::atomic ready_to_rotate; // count of encoders ready to rotate
- int max_waiting = 0;
- double last_rotate_tms = 0.; // last rotate time in ms
-
- // Sync logic for startup
- std::atomic encoders_ready = 0;
- std::atomic start_frame_id = 0;
- bool camera_ready[WideRoadCam + 1] = {};
- bool camera_synced[WideRoadCam + 1] = {};
-};
-LoggerdState s;
-
// Handle initial encoder syncing by waiting for all encoders to reach the same frame id
-bool sync_encoders(LoggerdState *state, CameraType cam_type, uint32_t frame_id) {
- if (state->camera_synced[cam_type]) return true;
+bool sync_encoders(LoggerdState *s, CameraType cam_type, uint32_t frame_id) {
+ if (s->camera_synced[cam_type]) return true;
- if (state->max_waiting > 1 && state->encoders_ready != state->max_waiting) {
+ if (s->max_waiting > 1 && s->encoders_ready != s->max_waiting) {
// add a small margin to the start frame id in case one of the encoders already dropped the next frame
- update_max_atomic(state->start_frame_id, frame_id + 2);
- if (std::exchange(state->camera_ready[cam_type], true) == false) {
- ++state->encoders_ready;
+ update_max_atomic(s->start_frame_id, frame_id + 2);
+ if (std::exchange(s->camera_ready[cam_type], true) == false) {
+ ++s->encoders_ready;
LOGE("camera %d encoder ready", cam_type);
}
return false;
} else {
- if (state->max_waiting == 1) update_max_atomic(state->start_frame_id, frame_id);
- bool synced = frame_id >= state->start_frame_id;
- state->camera_synced[cam_type] = synced;
- if (!synced) LOGE("camera %d waiting for frame %d, cur %d", cam_type, (int)state->start_frame_id, frame_id);
+ if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id);
+ bool synced = frame_id >= s->start_frame_id;
+ s->camera_synced[cam_type] = synced;
+ if (!synced) LOGE("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id);
return synced;
}
}
-void encoder_thread(const LogCameraInfo &cam_info) {
- set_thread_name(cam_info.filename);
+bool trigger_rotate_if_needed(LoggerdState *s, int cur_seg, uint32_t frame_id) {
+ const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS;
+ if (cur_seg >= 0 && frame_id >= ((cur_seg + 1) * frames_per_seg) + s->start_frame_id) {
+ // trigger rotate and wait until the main logger has rotated to the new segment
+ ++s->ready_to_rotate;
+ std::unique_lock lk(s->rotate_lock);
+ s->rotate_cv.wait(lk, [&] {
+ return s->rotate_segment > cur_seg || do_exit;
+ });
+ return !do_exit;
+ }
+ return false;
+}
+
+void encoder_thread(LoggerdState *s, const LogCameraInfo &cam_info) {
+ util::set_thread_name(cam_info.filename);
int cur_seg = -1;
int encode_idx = 0;
@@ -183,37 +74,29 @@ void encoder_thread(const LogCameraInfo &cam_info) {
if (buf == nullptr) continue;
if (cam_info.trigger_rotate) {
- s.last_camera_seen_tms = millis_since_boot();
- if (!sync_encoders(&s, cam_info.type, extra.frame_id)) {
+ s->last_camera_seen_tms = millis_since_boot();
+ if (!sync_encoders(s, cam_info.type, extra.frame_id)) {
continue;
}
// check if we're ready to rotate
- const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS;
- if (cur_seg >= 0 && extra.frame_id >= ((cur_seg+1) * frames_per_seg) + s.start_frame_id) {
- // trigger rotate and wait until the main logger has rotated to the new segment
- ++s.ready_to_rotate;
- std::unique_lock lk(s.rotate_lock);
- s.rotate_cv.wait(lk, [&] {
- return s.rotate_segment > cur_seg || do_exit;
- });
- if (do_exit) break;
- }
+ trigger_rotate_if_needed(s, cur_seg, extra.frame_id);
+ if (do_exit) break;
}
// rotate the encoder if the logger is on a newer segment
- if (s.rotate_segment > cur_seg) {
- cur_seg = s.rotate_segment;
+ if (s->rotate_segment > cur_seg) {
+ cur_seg = s->rotate_segment;
- LOGW("camera %d rotate encoder to %s", cam_info.type, s.segment_path);
+ LOGW("camera %d rotate encoder to %s", cam_info.type, s->segment_path);
for (auto &e : encoders) {
e->encoder_close();
- e->encoder_open(s.segment_path);
+ e->encoder_open(s->segment_path);
}
if (lh) {
lh_close(lh);
}
- lh = logger_get_handle(&s.logger);
+ lh = logger_get_handle(&s->logger);
}
// encode a frame
@@ -266,84 +149,63 @@ void encoder_thread(const LogCameraInfo &cam_info) {
}
}
-int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) {
- const char* dot = strrchr(fpath, '.');
- if (dot && strcmp(dot, ".lock") == 0) {
- unlink(fpath);
- }
- return 0;
-}
-
-void clear_locks() {
- ftw(LOG_ROOT.c_str(), clear_locks_fn, 16);
-}
-
-void logger_rotate() {
+void logger_rotate(LoggerdState *s) {
{
- std::unique_lock lk(s.rotate_lock);
+ std::unique_lock lk(s->rotate_lock);
int segment = -1;
- int err = logger_next(&s.logger, LOG_ROOT.c_str(), s.segment_path, sizeof(s.segment_path), &segment);
+ int err = logger_next(&s->logger, LOG_ROOT.c_str(), s->segment_path, sizeof(s->segment_path), &segment);
assert(err == 0);
- s.rotate_segment = segment;
- s.ready_to_rotate = 0;
- s.last_rotate_tms = millis_since_boot();
+ s->rotate_segment = segment;
+ s->ready_to_rotate = 0;
+ s->last_rotate_tms = millis_since_boot();
}
- s.rotate_cv.notify_all();
- LOGW((s.logger.part == 0) ? "logging to %s" : "rotated to %s", s.segment_path);
+ s->rotate_cv.notify_all();
+ LOGW((s->logger.part == 0) ? "logging to %s" : "rotated to %s", s->segment_path);
}
-void rotate_if_needed() {
- if (s.ready_to_rotate == s.max_waiting) {
- logger_rotate();
+void rotate_if_needed(LoggerdState *s) {
+ if (s->ready_to_rotate == s->max_waiting) {
+ logger_rotate(s);
}
double tms = millis_since_boot();
- if ((tms - s.last_rotate_tms) > SEGMENT_LENGTH * 1000 &&
- (tms - s.last_camera_seen_tms) > NO_CAMERA_PATIENCE &&
+ if ((tms - s->last_rotate_tms) > SEGMENT_LENGTH * 1000 &&
+ (tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE &&
!LOGGERD_TEST) {
LOGW("no camera packet seen. auto rotating");
- logger_rotate();
+ logger_rotate(s);
}
}
-} // namespace
-
-int main(int argc, char** argv) {
- if (Hardware::EON()) {
- setpriority(PRIO_PROCESS, 0, -20);
- } else if (Hardware::TICI()) {
- int ret;
- ret = set_core_affinity({0, 1, 2, 3});
- assert(ret == 0);
- // TODO: why does this impact camerad timings?
- //ret = set_realtime_priority(1);
- //assert(ret == 0);
- }
-
- clear_locks();
-
+void loggerd_thread() {
// setup messaging
typedef struct QlogState {
+ std::string name;
int counter, freq;
} QlogState;
std::unordered_map qlog_states;
- s.ctx = Context::create();
- Poller * poller = Poller::create();
+ std::unique_ptr ctx(Context::create());
+ std::unique_ptr poller(Poller::create());
// subscribe to all socks
for (const auto& it : services) {
if (!it.should_log) continue;
- SubSocket * sock = SubSocket::create(s.ctx, it.name);
+ SubSocket * sock = SubSocket::create(ctx.get(), it.name);
assert(sock != NULL);
poller->registerSocket(sock);
- qlog_states[sock] = {.counter = 0, .freq = it.decimation};
+ qlog_states[sock] = {
+ .name = it.name,
+ .counter = 0,
+ .freq = it.decimation,
+ };
}
+ LoggerdState s;
// init logger
logger_init(&s.logger, "rlog", true);
- logger_rotate();
+ logger_rotate(&s);
Params().put("CurrentRoute", s.logger.route_name);
// init encoders
@@ -351,7 +213,7 @@ int main(int argc, char** argv) {
std::vector encoder_threads;
for (const auto &cam : cameras_logged) {
if (cam.enable) {
- encoder_threads.push_back(std::thread(encoder_thread, cam));
+ encoder_threads.push_back(std::thread(encoder_thread, &s, cam));
if (cam.trigger_rotate) s.max_waiting++;
}
}
@@ -361,7 +223,10 @@ int main(int argc, char** argv) {
while (!do_exit) {
// poll for new messages on all sockets
for (auto sock : poller->poll(1000)) {
+ if (do_exit) break;
+
// drain socket
+ int count = 0;
QlogState &qs = qlog_states[sock];
Message *msg = nullptr;
while (!do_exit && (msg = sock->receive(true))) {
@@ -370,12 +235,18 @@ int main(int argc, char** argv) {
bytes_count += msg->getSize();
delete msg;
- rotate_if_needed();
+ rotate_if_needed(&s);
if ((++msg_count % 1000) == 0) {
double seconds = (millis_since_boot() - start_ts) / 1000.0;
LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds);
}
+
+ count++;
+ if (count >= 200) {
+ LOGE("large volume of '%s' messages", qs.name.c_str());
+ break;
+ }
}
}
}
@@ -395,8 +266,4 @@ int main(int argc, char** argv) {
// messaging cleanup
for (auto &[sock, qs] : qlog_states) delete sock;
- delete poller;
- delete s.ctx;
-
- return 0;
}
diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h
new file mode 100644
index 000000000..bdf5ef8f9
--- /dev/null
+++ b/selfdrive/loggerd/loggerd.h
@@ -0,0 +1,131 @@
+#pragma once
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "cereal/messaging/messaging.h"
+#include "cereal/services.h"
+#include "cereal/visionipc/visionipc.h"
+#include "cereal/visionipc/visionipc_client.h"
+#include "selfdrive/camerad/cameras/camera_common.h"
+#include "selfdrive/common/params.h"
+#include "selfdrive/common/swaglog.h"
+#include "selfdrive/common/timing.h"
+#include "selfdrive/common/util.h"
+#include "selfdrive/hardware/hw.h"
+
+#include "selfdrive/loggerd/encoder.h"
+#include "selfdrive/loggerd/logger.h"
+#if defined(QCOM) || defined(QCOM2)
+#include "selfdrive/loggerd/omx_encoder.h"
+#define Encoder OmxEncoder
+#else
+#include "selfdrive/loggerd/raw_logger.h"
+#define Encoder RawLogger
+#endif
+
+constexpr int MAIN_FPS = 20;
+const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000;
+const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000;
+
+#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead
+
+const bool LOGGERD_TEST = getenv("LOGGERD_TEST");
+const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60;
+
+struct LogCameraInfo {
+ CameraType type;
+ const char *filename;
+ VisionStreamType stream_type;
+ int frame_width, frame_height;
+ int fps;
+ int bitrate;
+ bool is_h265;
+ bool downscale;
+ bool has_qcamera;
+ bool trigger_rotate;
+ bool enable;
+ bool record;
+};
+
+const LogCameraInfo cameras_logged[] = {
+ {
+ .type = RoadCam,
+ .stream_type = VISION_STREAM_ROAD,
+ .filename = "fcamera.hevc",
+ .fps = MAIN_FPS,
+ .bitrate = MAIN_BITRATE,
+ .is_h265 = true,
+ .downscale = false,
+ .has_qcamera = true,
+ .trigger_rotate = true,
+ .enable = true,
+ .record = true,
+ },
+ {
+ .type = DriverCam,
+ .stream_type = VISION_STREAM_DRIVER,
+ .filename = "dcamera.hevc",
+ .fps = MAIN_FPS, // on EONs, more compressed this way
+ .bitrate = DCAM_BITRATE,
+ .is_h265 = true,
+ .downscale = false,
+ .has_qcamera = false,
+ .trigger_rotate = Hardware::TICI(),
+ .enable = !Hardware::PC(),
+ .record = Params().getBool("RecordFront"),
+ },
+ {
+ .type = WideRoadCam,
+ .stream_type = VISION_STREAM_WIDE_ROAD,
+ .filename = "ecamera.hevc",
+ .fps = MAIN_FPS,
+ .bitrate = MAIN_BITRATE,
+ .is_h265 = true,
+ .downscale = false,
+ .has_qcamera = false,
+ .trigger_rotate = true,
+ .enable = Hardware::TICI(),
+ .record = Hardware::TICI(),
+ },
+};
+const LogCameraInfo qcam_info = {
+ .filename = "qcamera.ts",
+ .fps = MAIN_FPS,
+ .bitrate = 256000,
+ .is_h265 = false,
+ .downscale = true,
+ .frame_width = Hardware::TICI() ? 526 : 480,
+ .frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same?
+};
+
+struct LoggerdState {
+ LoggerState logger = {};
+ char segment_path[4096];
+ std::mutex rotate_lock;
+ std::condition_variable rotate_cv;
+ std::atomic rotate_segment;
+ std::atomic last_camera_seen_tms;
+ std::atomic ready_to_rotate; // count of encoders ready to rotate
+ int max_waiting = 0;
+ double last_rotate_tms = 0.; // last rotate time in ms
+
+ // Sync logic for startup
+ std::atomic encoders_ready = 0;
+ std::atomic start_frame_id = 0;
+ bool camera_ready[WideRoadCam + 1] = {};
+ bool camera_synced[WideRoadCam + 1] = {};
+};
+
+bool sync_encoders(LoggerdState *s, CameraType cam_type, uint32_t frame_id);
+bool trigger_rotate_if_needed(LoggerdState *s, int cur_seg, uint32_t frame_id);
+void rotate_if_needed(LoggerdState *s);
+void loggerd_thread();
diff --git a/selfdrive/loggerd/main.cc b/selfdrive/loggerd/main.cc
new file mode 100644
index 000000000..7069aa706
--- /dev/null
+++ b/selfdrive/loggerd/main.cc
@@ -0,0 +1,20 @@
+#include "selfdrive/loggerd/loggerd.h"
+
+#include
+
+int main(int argc, char** argv) {
+ if (Hardware::EON()) {
+ setpriority(PRIO_PROCESS, 0, -20);
+ } else if (Hardware::TICI()) {
+ int ret;
+ ret = util::set_core_affinity({0, 1, 2, 3});
+ assert(ret == 0);
+ // TODO: why does this impact camerad timings?
+ //ret = util::set_realtime_priority(1);
+ //assert(ret == 0);
+ }
+
+ loggerd_thread();
+
+ return 0;
+}
diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc
index c490a0813..b0f9f7a9c 100644
--- a/selfdrive/loggerd/raw_logger.cc
+++ b/selfdrive/loggerd/raw_logger.cc
@@ -127,7 +127,7 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui
frame->data[0] = (uint8_t*)y_ptr;
frame->data[1] = (uint8_t*)u_ptr;
frame->data[2] = (uint8_t*)v_ptr;
- frame->pts = ts;
+ frame->pts = counter;
int ret = counter;
diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py
index e99725f0c..10bf218b0 100644
--- a/selfdrive/loggerd/uploader.py
+++ b/selfdrive/loggerd/uploader.py
@@ -60,8 +60,6 @@ class Uploader():
self.last_resp = None
self.last_exc = None
- self.raw_size = 0
- self.raw_count = 0
self.immediate_size = 0
self.immediate_count = 0
@@ -72,21 +70,16 @@ class Uploader():
self.immediate_folders = ["crash/", "boot/"]
self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1}
- self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2, "ecamera.hevc": 3}
def get_upload_sort(self, name):
if name in self.immediate_priority:
return self.immediate_priority[name]
- if name in self.high_priority:
- return self.high_priority[name] + 100
return 1000
def list_upload_files(self):
if not os.path.isdir(self.root):
return
- self.raw_size = 0
- self.raw_count = 0
self.immediate_size = 0
self.immediate_count = 0
@@ -116,38 +109,27 @@ class Uploader():
if name in self.immediate_priority:
self.immediate_count += 1
self.immediate_size += os.path.getsize(fn)
- else:
- self.raw_count += 1
- self.raw_size += os.path.getsize(fn)
except OSError:
pass
yield (name, key, fn)
- def next_file_to_upload(self, with_raw):
+ def next_file_to_upload(self):
upload_files = list(self.list_upload_files())
- # try to upload qlog files first
for name, key, fn in upload_files:
- if name in self.immediate_priority or any(f in fn for f in self.immediate_folders):
+ if any(f in fn for f in self.immediate_folders):
return (key, fn)
- if with_raw:
- # then upload the full log files, rear and front camera files
- for name, key, fn in upload_files:
- if name in self.high_priority:
- return (key, fn)
-
- # then upload other files
- for name, key, fn in upload_files:
- if not name.endswith('.lock') and not name.endswith(".tmp"):
- return (key, fn)
+ for name, key, fn in upload_files:
+ if name in self.immediate_priority:
+ return (key, fn)
return None
def do_upload(self, key, fn):
try:
- url_resp = self.api.get("v1.4/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
+ url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
if url_resp.status_code == 412:
self.last_resp = url_resp
return
@@ -226,8 +208,6 @@ class Uploader():
def get_msg(self):
msg = messaging.new_message("uploaderState")
us = msg.uploaderState
- us.rawQueueSize = int(self.raw_size / 1e6)
- us.rawQueueCount = self.raw_count
us.immediateQueueSize = int(self.immediate_size / 1e6)
us.immediateQueueCount = self.immediate_count
us.lastTime = self.last_time
@@ -260,10 +240,7 @@ def uploader_fn(exit_event):
time.sleep(60 if offroad else 5)
continue
- good_internet = network_type in [NetworkType.wifi, NetworkType.ethernet]
- allow_raw_upload = params.get_bool("UploadRaw")
-
- d = uploader.next_file_to_upload(with_raw=allow_raw_upload and good_internet and offroad)
+ d = uploader.next_file_to_upload()
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py
index 66f836ce6..078f18fd2 100755
--- a/selfdrive/manager/build.py
+++ b/selfdrive/manager/build.py
@@ -12,9 +12,9 @@ from common.spinner import Spinner
from common.text_window import TextWindow
from selfdrive.hardware import TICI
from selfdrive.swaglog import cloudlog, add_file_handler
-from selfdrive.version import dirty
+from selfdrive.version import get_dirty
-MAX_CACHE_SIZE = 2e9
+MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9
CACHE_DIR = Path("/data/scons_cache" if TICI else "/tmp/scons_cache")
TOTAL_SCONS_NODES = 2405
@@ -69,7 +69,7 @@ def build(spinner, dirty=False):
else:
# Build failed log errors
errors = [line.decode('utf8', 'replace') for line in compile_output
- if any([err in line for err in [b'error: ', b'not found, needed by target']])]
+ if any(err in line for err in [b'error: ', b'not found, needed by target'])]
error_s = "\n".join(errors)
add_file_handler(cloudlog)
cloudlog.error("scons build failed\n" + error_s)
@@ -77,7 +77,7 @@ def build(spinner, dirty=False):
# Show TextWindow
spinner.close()
if not os.getenv("CI"):
- error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors])
+ error_s = "\n \n".join("\n".join(textwrap.wrap(e, 65)) for e in errors)
with TextWindow("openpilot failed to build\n \n" + error_s) as t:
t.wait_for_exit()
exit(1)
@@ -98,4 +98,4 @@ def build(spinner, dirty=False):
if __name__ == "__main__" and not PREBUILT:
spinner = Spinner()
spinner.update_progress(0, 100)
- build(spinner, dirty)
+ build(spinner, get_dirty())
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index 6c71c2a37..386876b69 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -18,17 +18,20 @@ from selfdrive.manager.process import ensure_running
from selfdrive.manager.process_config import managed_processes
from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID
from selfdrive.swaglog import cloudlog, add_file_handler
-from selfdrive.version import dirty, get_git_commit, version, origin, branch, commit, \
- terms_version, training_version, comma_remote, \
- get_git_branch, get_git_remote
+from selfdrive.version import get_dirty, get_commit, get_version, get_origin, get_short_branch, \
+ terms_version, training_version, get_comma_remote
+
sys.path.append(os.path.join(BASEDIR, "pyextra"))
-def manager_init():
+def manager_init():
# update system time from panda
set_time(cloudlog)
+ # save boot log
+ subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
+
params = Params()
params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START)
@@ -67,12 +70,12 @@ def manager_init():
print("WARNING: failed to make /dev/shm")
# set version params
- params.put("Version", version)
+ params.put("Version", get_version())
params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
- params.put("GitCommit", get_git_commit(default=""))
- params.put("GitBranch", get_git_branch(default=""))
- params.put("GitRemote", get_git_remote(default=""))
+ params.put("GitCommit", get_commit(default=""))
+ params.put("GitBranch", get_short_branch(default=""))
+ params.put("GitRemote", get_origin(default=""))
# set dongle id
reg_res = register(show_spinner=True)
@@ -83,16 +86,16 @@ def manager_init():
raise Exception(f"Registration failed for device {serial}")
os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog
- if not dirty:
+ if not get_dirty():
os.environ['CLEAN'] = '1'
- cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty,
+ cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=get_dirty(),
device=HARDWARE.get_device_type())
- if comma_remote and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC):
+ if get_comma_remote() and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC):
crash.init()
crash.bind_user(id=dongle_id)
- crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit,
+ crash.bind_extra(dirty=get_dirty(), origin=get_origin(), branch=get_short_branch(), commit=get_commit(),
device=HARDWARE.get_device_type())
@@ -102,8 +105,13 @@ def manager_prepare():
def manager_cleanup():
+ # send signals to kill all procs
for p in managed_processes.values():
- p.stop()
+ p.stop(block=False)
+
+ # ensure all are killed
+ for p in managed_processes.values():
+ p.stop(block=True)
cloudlog.info("everything is dead")
@@ -112,9 +120,6 @@ def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
- # save boot log
- subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
-
params = Params()
ignore = []
@@ -149,8 +154,8 @@ def manager_thread():
started_prev = started
- running = ' '.join(["%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
- for p in managed_processes.values() if p.proc])
+ running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
+ for p in managed_processes.values() if p.proc)
print(running)
cloudlog.debug(running)
diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py
index 6acda36a2..d16a14503 100755
--- a/selfdrive/manager/test/test_manager.py
+++ b/selfdrive/manager/test/test_manager.py
@@ -19,6 +19,7 @@ ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not D
class TestManager(unittest.TestCase):
def setUp(self):
os.environ['PASSIVE'] = '0'
+ HARDWARE.set_power_save(False)
def tearDown(self):
manager.manager_cleanup()
diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript
index 8e4f2569e..ceb3c560b 100644
--- a/selfdrive/modeld/SConscript
+++ b/selfdrive/modeld/SConscript
@@ -65,7 +65,7 @@ if use_thneed and arch in ("aarch64", "larch64"):
compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs)
cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} ../../models/supercombo.dlc ../../models/supercombo.thneed --binary"
- lib_paths = ':'.join([Dir(p).abspath for p in lenv["LIBPATH"]])
+ lib_paths = ':'.join(Dir(p).abspath for p in lenv["LIBPATH"])
cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}"})
cenv.Command("../../models/supercombo.thneed", ["../../models/supercombo.dlc", compiler], cmd)
diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc
index 6ae6e780c..c85c05c9d 100644
--- a/selfdrive/modeld/dmonitoringmodeld.cc
+++ b/selfdrive/modeld/dmonitoringmodeld.cc
@@ -39,7 +39,7 @@ int main(int argc, char **argv) {
DMonitoringModelState model;
dmonitoring_init(&model);
- VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true);
+ VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_DRIVER, true);
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
}
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 3d3e43590..9e1750eea 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -20,8 +20,8 @@ mat3 cur_transform;
std::mutex transform_lock;
void calibration_thread(bool wide_camera) {
- set_thread_name("calibration");
- set_realtime_priority(50);
+ util::set_thread_name("calibration");
+ util::set_realtime_priority(50);
SubMaster sm({"liveCalibration"});
@@ -104,7 +104,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
}
double mt1 = millis_since_boot();
- ModelDataRaw model_buf = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
+ ModelOutput *model_output = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
model_transform, vec_desire);
double mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0;
@@ -119,9 +119,9 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
- model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time,
+ model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, *model_output, extra.timestamp_eof, model_execution_time,
kj::ArrayPtr(model.output.data(), model.output.size()));
- posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
+ posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof);
//printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;
@@ -133,9 +133,9 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
int main(int argc, char **argv) {
if (!Hardware::PC()) {
int ret;
- ret = set_realtime_priority(54);
+ ret = util::set_realtime_priority(54);
assert(ret == 0);
- set_core_affinity({Hardware::EON() ? 2 : 7});
+ util::set_core_affinity({Hardware::EON() ? 2 : 7});
assert(ret == 0);
}
@@ -153,7 +153,7 @@ int main(int argc, char **argv) {
model_init(&model, device_id, context);
LOGW("models loaded, modeld starting");
- VisionIpcClient vipc_client = VisionIpcClient("camerad", wide_camera ? VISION_STREAM_YUV_WIDE : VISION_STREAM_YUV_BACK, true, device_id, context);
+ VisionIpcClient vipc_client = VisionIpcClient("camerad", wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context);
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
}
diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h
index d7904489b..19af75eef 100644
--- a/selfdrive/modeld/models/commonmodel.h
+++ b/selfdrive/modeld/models/commonmodel.h
@@ -16,10 +16,6 @@
#include "selfdrive/modeld/transforms/loadyuv.h"
#include "selfdrive/modeld/transforms/transform.h"
-constexpr int MODEL_WIDTH = 512;
-constexpr int MODEL_HEIGHT = 256;
-constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
-
const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
void softmax(const float* input, float* output, size_t len);
@@ -27,14 +23,17 @@ float softplus(float input);
float sigmoid(float input);
class ModelFrame {
- public:
+public:
ModelFrame(cl_device_id device_id, cl_context context);
~ModelFrame();
float* prepare(cl_mem yuv_cl, int width, int height, const mat3& transform, cl_mem *output);
+ const int MODEL_WIDTH = 512;
+ const int MODEL_HEIGHT = 256;
+ const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
const int buf_size = MODEL_FRAME_SIZE * 2;
- private:
+private:
Transform transform;
LoadYUVState loadyuv;
cl_command_queue q;
diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc
index 9d87e320a..2acdf7d2b 100644
--- a/selfdrive/modeld/models/dmonitoring.cc
+++ b/selfdrive/modeld/models/dmonitoring.cc
@@ -3,15 +3,15 @@
#include "libyuv.h"
#include "selfdrive/common/mat.h"
+#include "selfdrive/common/modeldata.h"
#include "selfdrive/common/params.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/hardware/hw.h"
#include "selfdrive/modeld/models/dmonitoring.h"
-#define MODEL_WIDTH 320
-#define MODEL_HEIGHT 640
-#define FULL_W 852 // should get these numbers from camerad
+constexpr int MODEL_WIDTH = 320;
+constexpr int MODEL_HEIGHT = 640;
template
static inline T *get_buffer(std::vector &buf, const size_t size) {
@@ -67,21 +67,15 @@ void crop_yuv(uint8_t *raw, int width, int height, uint8_t *y, uint8_t *u, uint8
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) {
Rect crop_rect;
- if (Hardware::TICI()) {
- const int full_width_tici = 1928;
- const int full_height_tici = 1208;
- const int adapt_width_tici = 954;
- const int x_offset_tici = -72;
- const int y_offset_tici = -144;
- const int cropped_height = adapt_width_tici / 1.33;
- crop_rect = {full_width_tici / 2 - adapt_width_tici / 2 + x_offset_tici,
- full_height_tici / 2 - cropped_height / 2 + y_offset_tici,
+ if (width == TICI_CAM_WIDTH) {
+ const int cropped_height = tici_dm_crop::width / 1.33;
+ crop_rect = {width / 2 - tici_dm_crop::width / 2 + tici_dm_crop::x_offset,
+ height / 2 - cropped_height / 2 + tici_dm_crop::y_offset,
cropped_height / 2,
cropped_height};
if (!s->is_rhd) {
- crop_rect.x += adapt_width_tici - crop_rect.w;
+ crop_rect.x += tici_dm_crop::width - crop_rect.w;
}
-
} else {
const int adapt_width = 372;
crop_rect = {0, 0, adapt_width, height};
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index a2ff9e811..629e1d7ed 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -16,8 +16,8 @@ constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
-float prev_brake_5ms2_probs[5] = {0,0,0,0,0};
-float prev_brake_3ms2_probs[3] = {0,0,0};
+std::array prev_brake_5ms2_probs = {0,0,0,0,0};
+std::array prev_brake_3ms2_probs = {0,0,0};
// #define DUMP_YUV
@@ -52,7 +52,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
#endif
}
-ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
+ModelOutput* model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
const mat3 &transform, float *desire_in) {
#ifdef DESIRE
if (desire_in != NULL) {
@@ -69,35 +69,18 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
}
#endif
- //for (int i = 0; i < NET_OUTPUT_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
-
// if getInputBuf is not NULL, net_input_buf will be
auto net_input_buf = s->frame->prepare(yuv_cl, width, height, transform, static_cast(s->m->getInputBuf()));
s->m->execute(net_input_buf, s->frame->buf_size);
- // net outputs
- ModelDataRaw net_outputs {
- .plans = (ModelDataRawPlans*)&s->output[PLAN_IDX],
- .lane_lines = (ModelDataRawLaneLines*)&s->output[LL_IDX],
- .road_edges = (ModelDataRawRoadEdges*)&s->output[RE_IDX],
- .leads = (ModelDataRawLeads*)&s->output[LEAD_IDX],
- .meta = &s->output[DESIRE_STATE_IDX],
- .pose = (ModelDataRawPose*)&s->output[POSE_IDX],
- };
- return net_outputs;
+ return (ModelOutput*)&s->output;
}
void model_free(ModelState* s) {
delete s->frame;
}
-void fill_sigmoid(const float *input, float *output, int len, int stride) {
- for (int i=0; i lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
const auto &best_prediction = leads.get_best_prediction(t_idx);
lead.setProb(sigmoid(leads.prob[t_idx]));
@@ -125,56 +108,54 @@ void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelDataRaw
lead.setAStd(to_kj_array_ptr(lead_a_std));
}
-void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
- float desire_state_softmax[DESIRE_LEN];
- float desire_pred_softmax[4*DESIRE_LEN];
- softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
- for (int i=0; i<4; i++) {
- softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
- &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
+void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data) {
+ std::array desire_state_softmax;
+ softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN);
+
+ std::array desire_pred_softmax;
+ for (int i=0; i lat_long_t = {2,4,6,8,10};
+ std::array gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid,
+ brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid;
+ for (int i=0; i threshold;
}
- for (int i=0; i<3; i++) {
+ for (int i=0; i FCW_THRESHOLD_3MS2;
}
auto disengage = meta.initDisengagePredictions();
- disengage.setT({2,4,6,8,10});
- disengage.setGasDisengageProbs(gas_disengage_sigmoid);
- disengage.setBrakeDisengageProbs(brake_disengage_sigmoid);
- disengage.setSteerOverrideProbs(steer_override_sigmoid);
- disengage.setBrake3MetersPerSecondSquaredProbs(brake_3ms2_sigmoid);
- disengage.setBrake4MetersPerSecondSquaredProbs(brake_4ms2_sigmoid);
- disengage.setBrake5MetersPerSecondSquaredProbs(brake_5ms2_sigmoid);
+ disengage.setT(to_kj_array_ptr(lat_long_t));
+ disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid));
+ disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid));
+ disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid));
+ disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid));
+ disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid));
+ disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid));
- meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
- meta.setDesirePrediction(desire_pred_softmax);
- meta.setDesireState(desire_state_softmax);
+ meta.setEngagedProb(sigmoid(meta_data.engaged_prob));
+ meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax));
+ meta.setDesireState(to_kj_array_ptr(desire_state_softmax));
meta.setHardBrakePredicted(above_fcw_threshold);
}
@@ -197,7 +178,7 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array pos_x, pos_y, pos_z;
std::array pos_x_std, pos_y_std, pos_z_std;
std::array vel_x, vel_y, vel_z;
@@ -229,7 +210,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelDataRawPlanPredi
}
void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array &plan_t,
- const ModelDataRawLaneLines &lanes) {
+ const ModelOutputLaneLines &lanes) {
std::array left_far_y, left_far_z;
std::array left_near_y, left_near_z;
std::array right_near_y, right_near_z;
@@ -267,7 +248,7 @@ void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array &plan_t,
- const ModelDataRawRoadEdges &edges) {
+ const ModelOutputRoadEdges &edges) {
std::array left_y, left_z;
std::array right_y, right_z;
for (int j=0; jget_best_prediction();
+void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs) {
+ const auto &best_plan = net_outputs.plans.get_best_prediction();
std::array plan_t;
std::fill_n(plan_t.data(), plan_t.size(), NAN);
plan_t[0] = 0.0;
@@ -311,8 +292,8 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
}
fill_plan(framed, best_plan);
- fill_lane_lines(framed, plan_t, *net_outputs.lane_lines);
- fill_road_edges(framed, plan_t, *net_outputs.road_edges);
+ fill_lane_lines(framed, plan_t, net_outputs.lane_lines);
+ fill_road_edges(framed, plan_t, net_outputs.road_edges);
// meta
fill_meta(framed.initMeta(), net_outputs.meta);
@@ -321,12 +302,12 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
std::array t_offsets = {0.0, 2.0, 4.0};
for (int i=0; i raw_pred) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
MessageBuilder msg;
@@ -344,12 +325,12 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, flo
}
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
- const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
+ const ModelOutput &net_outputs, uint64_t timestamp_eof) {
MessageBuilder msg;
- auto v_mean = net_outputs.pose->velocity_mean;
- auto r_mean = net_outputs.pose->rotation_mean;
- auto v_std = net_outputs.pose->velocity_std;
- auto r_std = net_outputs.pose->rotation_std;
+ auto v_mean = net_outputs.pose.velocity_mean;
+ auto r_mean = net_outputs.pose.rotation_mean;
+ auto v_std = net_outputs.pose.velocity_std;
+ auto r_std = net_outputs.pose.rotation_std;
auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry();
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index c6d3e00ac..5749fb5ec 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -16,78 +16,54 @@
#include "selfdrive/modeld/runners/run.h"
constexpr int DESIRE_LEN = 8;
+constexpr int DESIRE_PRED_LEN = 4;
constexpr int TRAFFIC_CONVENTION_LEN = 2;
constexpr int MODEL_FREQ = 20;
-constexpr int DESIRE_PRED_SIZE = 32;
-constexpr int OTHER_META_SIZE = 48;
-constexpr int NUM_META_INTERVALS = 5;
+constexpr int DISENGAGE_LEN = 5;
+constexpr int BLINKER_LEN = 6;
constexpr int META_STRIDE = 7;
constexpr int PLAN_MHP_N = 5;
-constexpr int PLAN_MHP_VALS = 15*33;
-constexpr int PLAN_MHP_SELECTION = 1;
-constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
constexpr int LEAD_MHP_N = 2;
constexpr int LEAD_TRAJ_LEN = 6;
constexpr int LEAD_PRED_DIM = 4;
-constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
constexpr int LEAD_MHP_SELECTION = 3;
-constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
-constexpr int POSE_SIZE = 12;
-
-constexpr int PLAN_IDX = 0;
-constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
-constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
-constexpr int RE_IDX = LL_PROB_IDX + 8;
-constexpr int LEAD_IDX = RE_IDX + 2*2*2*33;
-constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE);
-constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3;
-constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN;
-constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE;
-constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
-#ifdef TEMPORAL
- constexpr int TEMPORAL_SIZE = 512;
-#else
- constexpr int TEMPORAL_SIZE = 0;
-#endif
-constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE;
-
-struct ModelDataRawXYZ {
+struct ModelOutputXYZ {
float x;
float y;
float z;
};
-static_assert(sizeof(ModelDataRawXYZ) == sizeof(float)*3);
+static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3);
-struct ModelDataRawYZ {
+struct ModelOutputYZ {
float y;
float z;
};
-static_assert(sizeof(ModelDataRawYZ) == sizeof(float)*2);
+static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2);
-struct ModelDataRawPlanElement {
- ModelDataRawXYZ position;
- ModelDataRawXYZ velocity;
- ModelDataRawXYZ acceleration;
- ModelDataRawXYZ rotation;
- ModelDataRawXYZ rotation_rate;
+struct ModelOutputPlanElement {
+ ModelOutputXYZ position;
+ ModelOutputXYZ velocity;
+ ModelOutputXYZ acceleration;
+ ModelOutputXYZ rotation;
+ ModelOutputXYZ rotation_rate;
};
-static_assert(sizeof(ModelDataRawPlanElement) == sizeof(ModelDataRawXYZ)*5);
+static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5);
-struct ModelDataRawPlanPrediction {
- std::array mean;
- std::array std;
+struct ModelOutputPlanPrediction {
+ std::array mean;
+ std::array std;
float prob;
};
-static_assert(sizeof(ModelDataRawPlanPrediction) == (sizeof(ModelDataRawPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
+static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
-struct ModelDataRawPlans {
- std::array prediction;
+struct ModelOutputPlans {
+ std::array prediction;
- constexpr const ModelDataRawPlanPrediction &get_best_prediction() const {
+ constexpr const ModelOutputPlanPrediction &get_best_prediction() const {
int max_idx = 0;
for (int i = 1; i < prediction.size(); i++) {
if (prediction[i].prob > prediction[max_idx].prob) {
@@ -97,69 +73,69 @@ struct ModelDataRawPlans {
return prediction[max_idx];
}
};
-static_assert(sizeof(ModelDataRawPlans) == sizeof(ModelDataRawPlanPrediction)*PLAN_MHP_N);
+static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N);
-struct ModelDataRawLinesXY {
- std::array left_far;
- std::array left_near;
- std::array right_near;
- std::array right_far;
+struct ModelOutputLinesXY {
+ std::array left_far;
+ std::array left_near;
+ std::array right_near;
+ std::array right_far;
};
-static_assert(sizeof(ModelDataRawLinesXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*4);
+static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4);
-struct ModelDataRawLineProbVal {
+struct ModelOutputLineProbVal {
float val_deprecated;
float val;
};
-static_assert(sizeof(ModelDataRawLineProbVal) == sizeof(float)*2);
+static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2);
-struct ModelDataRawLinesProb {
- ModelDataRawLineProbVal left_far;
- ModelDataRawLineProbVal left_near;
- ModelDataRawLineProbVal right_near;
- ModelDataRawLineProbVal right_far;
+struct ModelOutputLinesProb {
+ ModelOutputLineProbVal left_far;
+ ModelOutputLineProbVal left_near;
+ ModelOutputLineProbVal right_near;
+ ModelOutputLineProbVal right_far;
};
-static_assert(sizeof(ModelDataRawLinesProb) == sizeof(ModelDataRawLineProbVal)*4);
+static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4);
-struct ModelDataRawLaneLines {
- ModelDataRawLinesXY mean;
- ModelDataRawLinesXY std;
- ModelDataRawLinesProb prob;
+struct ModelOutputLaneLines {
+ ModelOutputLinesXY mean;
+ ModelOutputLinesXY std;
+ ModelOutputLinesProb prob;
};
-static_assert(sizeof(ModelDataRawLaneLines) == (sizeof(ModelDataRawLinesXY)*2) + sizeof(ModelDataRawLinesProb));
+static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb));
-struct ModelDataRawEdgessXY {
- std::array left;
- std::array right;
+struct ModelOutputEdgessXY {
+ std::array left;
+ std::array right;
};
-static_assert(sizeof(ModelDataRawEdgessXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*2);
+static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2);
-struct ModelDataRawRoadEdges {
- ModelDataRawEdgessXY mean;
- ModelDataRawEdgessXY std;
+struct ModelOutputRoadEdges {
+ ModelOutputEdgessXY mean;
+ ModelOutputEdgessXY std;
};
-static_assert(sizeof(ModelDataRawRoadEdges) == (sizeof(ModelDataRawEdgessXY)*2));
+static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2));
-struct ModelDataRawLeadElement {
+struct ModelOutputLeadElement {
float x;
float y;
float velocity;
float acceleration;
};
-static_assert(sizeof(ModelDataRawLeadElement) == sizeof(float)*4);
+static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4);
-struct ModelDataRawLeadPrediction {
- std::array mean;
- std::array std;
+struct ModelOutputLeadPrediction {
+ std::array mean;
+ std::array std;
std::array prob;
};
-static_assert(sizeof(ModelDataRawLeadPrediction) == (sizeof(ModelDataRawLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
+static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
-struct ModelDataRawLeads {
- std::array prediction;
+struct ModelOutputLeads {
+ std::array prediction;
std::array prob;
- constexpr const ModelDataRawLeadPrediction &get_best_prediction(int t_idx) const {
+ constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const {
int max_idx = 0;
for (int i = 1; i < prediction.size(); i++) {
if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) {
@@ -169,26 +145,77 @@ struct ModelDataRawLeads {
return prediction[max_idx];
}
};
-static_assert(sizeof(ModelDataRawLeads) == (sizeof(ModelDataRawLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
+static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
-struct ModelDataRawPose {
- ModelDataRawXYZ velocity_mean;
- ModelDataRawXYZ rotation_mean;
- ModelDataRawXYZ velocity_std;
- ModelDataRawXYZ rotation_std;
+struct ModelOutputPose {
+ ModelOutputXYZ velocity_mean;
+ ModelOutputXYZ rotation_mean;
+ ModelOutputXYZ velocity_std;
+ ModelOutputXYZ rotation_std;
};
-static_assert(sizeof(ModelDataRawPose) == sizeof(ModelDataRawXYZ)*4);
+static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4);
-struct ModelDataRaw {
- const ModelDataRawPlans *const plans;
- const ModelDataRawLaneLines *const lane_lines;
- const ModelDataRawRoadEdges *const road_edges;
- const ModelDataRawLeads *const leads;
- const float *const desire_state;
- const float *const meta;
- const float *const desire_pred;
- const ModelDataRawPose *const pose;
+struct ModelOutputDisengageProb {
+ float gas_disengage;
+ float brake_disengage;
+ float steer_override;
+ float brake_3ms2;
+ float brake_4ms2;
+ float brake_5ms2;
+ float gas_pressed;
};
+static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7);
+
+struct ModelOutputBlinkerProb {
+ float left;
+ float right;
+};
+static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2);
+
+struct ModelOutputDesireProb {
+ union {
+ struct {
+ float none;
+ float turn_left;
+ float turn_right;
+ float lane_change_left;
+ float lane_change_right;
+ float keep_left;
+ float keep_right;
+ float null;
+ };
+ struct {
+ std::array array;
+ };
+ };
+};
+static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN);
+
+struct ModelOutputMeta {
+ ModelOutputDesireProb desire_state_prob;
+ float engaged_prob;
+ std::array disengage_prob;
+ std::array blinker_prob;
+ std::array desire_pred_prob;
+};
+static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN));
+
+struct ModelOutput {
+ const ModelOutputPlans plans;
+ const ModelOutputLaneLines lane_lines;
+ const ModelOutputRoadEdges road_edges;
+ const ModelOutputLeads leads;
+ const ModelOutputMeta meta;
+ const ModelOutputPose pose;
+};
+
+constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
+#ifdef TEMPORAL
+ constexpr int TEMPORAL_SIZE = 512;
+#else
+ constexpr int TEMPORAL_SIZE = 0;
+#endif
+constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE;
// TODO: convert remaining arrays to std::array and update model runners
struct ModelState {
@@ -205,12 +232,12 @@ struct ModelState {
};
void model_init(ModelState* s, cl_device_id device_id, cl_context context);
-ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
+ModelOutput *model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
const mat3 &transform, float *desire_in);
void model_free(ModelState* s);
void poly_fit(float *in_pts, float *in_stds, float *out);
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
- const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
+ const ModelOutput &net_outputs, uint64_t timestamp_eof,
float model_execution_time, kj::ArrayPtr raw_pred);
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
- const ModelDataRaw &net_outputs, uint64_t timestamp_eof);
+ const ModelOutput &net_outputs, uint64_t timestamp_eof);
diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h
index 0893a4acc..948048f5d 100644
--- a/selfdrive/modeld/runners/runmodel.h
+++ b/selfdrive/modeld/runners/runmodel.h
@@ -1,6 +1,7 @@
#pragma once
class RunModel {
public:
+ virtual ~RunModel() {}
virtual void addRecurrent(float *state, int state_size) {}
virtual void addDesire(float *state, int state_size) {}
virtual void addTrafficConvention(float *state, int state_size) {}
diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh
index 53c7b3909..260a1b448 100755
--- a/selfdrive/test/setup_device_ci.sh
+++ b/selfdrive/test/setup_device_ci.sh
@@ -1,4 +1,6 @@
-#!/usr/bin/bash -e
+#!/usr/bin/bash
+
+set -e
if [ -z "$SOURCE_DIR" ]; then
echo "SOURCE_DIR must be set"
@@ -24,8 +26,35 @@ if [ -f "/EON" ]; then
rm -rf /data/safe_staging
fi
+export KEYS_PATH="/usr/comma/setup_keys"
+export CONTINUE_PATH="/data/continue.sh"
+if [ -f "/EON" ]; then
+ export KEYS_PATH="/data/data/com.termux/files/home/setup_keys"
+ export CONTINUE_PATH="/data/data/com.termux/files/continue.sh"
+fi
+tee $CONTINUE_PATH << EOF
+#!/usr/bin/bash
+
+PARAMS_ROOT="/data/params/d"
+
+while true; do
+ mkdir -p \$PARAMS_ROOT
+ cp $KEYS_PATH \$PARAMS_ROOT/GithubSshKeys
+ echo -n 1 > \$PARAMS_ROOT/SshEnabled
+ sleep 1m
+done
+
+sleep infinity
+EOF
+chmod +x $CONTINUE_PATH
+
# set up environment
+if [ ! -d "$SOURCE_DIR" ]; then
+ git clone https://github.com/commaai/openpilot.git $SOURCE_DIR
+fi
cd $SOURCE_DIR
+
+rm -f .git/index.lock
git reset --hard
git fetch
find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \;
diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py
index f1aad012e..bc44aa3ea 100755
--- a/selfdrive/test/test_onroad.py
+++ b/selfdrive/test/test_onroad.py
@@ -26,7 +26,7 @@ PROCS = {
"./loggerd": 45.0,
"./locationd": 9.1,
"selfdrive.controls.plannerd": 22.6,
- "./_ui": 15.0,
+ "./_ui": 20.0,
"selfdrive.locationd.paramsd": 9.1,
"./camerad": 7.07,
"./_sensord": 6.17,
@@ -138,21 +138,18 @@ class TestOnroad(unittest.TestCase):
cls.lr = list(LogReader(os.path.join(segs[-1], "rlog.bz2")))
return
+ # setup env
os.environ['REPLAY'] = "1"
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019"
+
+ params = Params()
+ params.clear_all()
set_params_enabled()
# Make sure athena isn't running
- Params().delete("DongleId")
- Params().delete("AthenadPid")
os.system("pkill -9 -f athena")
- logger_root = Path(ROOT)
- initial_segments = set()
- if logger_root.exists():
- initial_segments = set(Path(ROOT).iterdir())
-
# start manager and run openpilot for a minute
try:
manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py")
@@ -164,15 +161,22 @@ class TestOnroad(unittest.TestCase):
sm.update(1000)
# make sure we get at least two full segments
+ route = None
cls.segments = []
with Timeout(300, "timed out waiting for logs"):
+ while route is None:
+ route = params.get("CurrentRoute", encoding="utf-8")
+ time.sleep(0.1)
+
while len(cls.segments) < 3:
- new_paths = set()
- if logger_root.exists():
- new_paths = set(logger_root.iterdir()) - initial_segments
- segs = [p for p in new_paths if "--" in str(p)]
+ segs = set()
+ if Path(ROOT).exists():
+ segs = set(Path(ROOT).glob(f"{route}--*"))
cls.segments = sorted(segs, key=lambda s: int(str(s).rsplit('--')[-1]))
- time.sleep(5)
+ time.sleep(2)
+
+ # chop off last, incomplete segment
+ cls.segments = cls.segments[:-1]
finally:
proc.terminate()
@@ -190,7 +194,7 @@ class TestOnroad(unittest.TestCase):
total_size = sum(len(m.as_builder().to_bytes()) for m in msgs)
self.assertLess(total_size, 3.5e5)
- cnt = Counter([json.loads(m.logMessage)['filename'] for m in msgs])
+ cnt = Counter(json.loads(m.logMessage)['filename'] for m in msgs)
big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.]
self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}")
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index 114110891..1b467f962 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -22,15 +22,13 @@ from selfdrive.hardware import EON, TICI, PC, HARDWARE
from selfdrive.loggerd.config import get_available_percent
from selfdrive.swaglog import cloudlog
from selfdrive.thermald.power_monitoring import PowerMonitoring
-from selfdrive.version import tested_branch, terms_version, training_version
+from selfdrive.version import terms_version, training_version
ThermalStatus = log.DeviceState.ThermalStatus
NetworkType = log.DeviceState.NetworkType
NetworkStrength = log.DeviceState.NetworkStrength
CURRENT_TAU = 15. # 15s time constant
TEMP_TAU = 5. # 5s time constant
-DAYS_NO_CONNECTIVITY_MAX = 14 # do not allow to engage after this many days
-DAYS_NO_CONNECTIVITY_PROMPT = 10 # send an offroad prompt after this many days
DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert
ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp'])
@@ -138,7 +136,7 @@ def handle_fan_tici(controller, max_cpu_temp, fan_speed, ignition):
controller.reset()
fan_pwr_out = -int(controller.update(
- setpoint=(75 if ignition else (OFFROAD_DANGER_TEMP - 2)),
+ setpoint=75,
measurement=max_cpu_temp,
feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
))
@@ -165,10 +163,11 @@ def thermald_thread():
fan_speed = 0
count = 0
- startup_conditions = {
+ onroad_conditions = {
"ignition": False,
}
- startup_conditions_prev = startup_conditions.copy()
+ startup_conditions = {}
+ startup_conditions_prev = {}
off_ts = None
started_ts = None
@@ -222,12 +221,12 @@ def thermald_thread():
if pandaState.pandaType == log.PandaState.PandaType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
- if startup_conditions["ignition"]:
+ if onroad_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
- startup_conditions["ignition"] = False
+ onroad_conditions["ignition"] = False
else:
no_panda_cnt = 0
- startup_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan
+ onroad_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client
@@ -306,7 +305,7 @@ def thermald_thread():
)
if handle_fan is not None:
- fan_speed = handle_fan(controller, max_comp_temp, fan_speed, startup_conditions["ignition"])
+ fan_speed = handle_fan(controller, max_comp_temp, fan_speed, onroad_conditions["ignition"])
msg.deviceState.fanSpeedPercentDesired = fan_speed
is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5))
@@ -324,47 +323,11 @@ def thermald_thread():
# **** starting logic ****
- # Check for last update time and display alerts if needed
+ # Ensure date/time are valid
now = datetime.datetime.utcnow()
-
- # show invalid date/time alert
startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"]))
- # Show update prompt
- try:
- last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
- except (TypeError, ValueError):
- last_update = now
- dt = now - last_update
-
- update_failed_count = params.get("UpdateFailedCount")
- update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
- last_update_exception = params.get("LastUpdateException", encoding='utf8')
-
- if update_failed_count > 15 and last_update_exception is not None:
- if tested_branch:
- extra_text = "Ensure the software is correctly installed"
- else:
- extra_text = last_update_exception
-
- set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
- set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
- set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text)
- elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
- set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
- set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
- set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
- elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
- remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1)
- set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
- set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
- set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.")
- else:
- set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
- set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
- set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
-
startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
@@ -377,14 +340,17 @@ def thermald_thread():
startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot")
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
- startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
- set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"]))
+ onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
+ set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not onroad_conditions["device_temp_good"]))
if TICI:
set_offroad_alert_if_changed("Offroad_StorageMissing", (not Path("/data/media").is_mount()))
# Handle offroad/onroad transition
- should_start = all(startup_conditions.values())
+ should_start = all(onroad_conditions.values())
+ if started_ts is None:
+ should_start = should_start and all(startup_conditions.values())
+
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOnroad", should_start)
params.put_bool("IsOffroad", not should_start)
@@ -396,25 +362,25 @@ def thermald_thread():
started_ts = sec_since_boot()
started_seen = True
else:
- if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
- cloudlog.event("Startup blocked", startup_conditions=startup_conditions)
+ if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
+ cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
- power_monitor.calculate(peripheralState, startup_conditions["ignition"])
+ power_monitor.calculate(peripheralState, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
current_power_draw = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none
msg.deviceState.powerDrawW = current_power_draw if current_power_draw is not None else 0
# Check if we need to disable charging (handled by boardd)
- msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(startup_conditions["ignition"], in_car, off_ts)
+ msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(onroad_conditions["ignition"], in_car, off_ts)
# Check if we need to shut down
- if power_monitor.should_shutdown(peripheralState, startup_conditions["ignition"], in_car, off_ts, started_seen):
+ if power_monitor.should_shutdown(peripheralState, onroad_conditions["ignition"], in_car, off_ts, started_seen):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py
index fb7a1a2c9..a301725ba 100755
--- a/selfdrive/tombstoned.py
+++ b/selfdrive/tombstoned.py
@@ -15,7 +15,7 @@ from common.file_helpers import mkdirs_exists_ok
from selfdrive.hardware import TICI, HARDWARE
from selfdrive.loggerd.config import ROOT
from selfdrive.swaglog import cloudlog
-from selfdrive.version import branch, commit, dirty, origin, version
+from selfdrive.version import get_branch, get_commit, get_dirty, get_origin, get_version
MAX_SIZE = 100000 * 10 # mal size is 40-100k, allow up to 1M
if TICI:
@@ -109,7 +109,7 @@ def report_tombstone_android(fn):
clean_path = executable.replace('./', '').replace('/', '_')
date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S")
- new_fn = f"{date}_{commit[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
+ new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
crashlog_dir = os.path.join(ROOT, "crash")
mkdirs_exists_ok(crashlog_dir)
@@ -183,7 +183,7 @@ def report_tombstone_apport(fn):
clean_path = path.replace('/', '_')
date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S")
- new_fn = f"{date}_{commit[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
+ new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
crashlog_dir = os.path.join(ROOT, "crash")
mkdirs_exists_ok(crashlog_dir)
@@ -203,14 +203,14 @@ def main():
sentry_sdk.utils.MAX_STRING_LENGTH = 8192
sentry_sdk.init("https://a40f22e13cbc4261873333c125fc9d38@o33823.ingest.sentry.io/157615",
- default_integrations=False, release=version)
+ default_integrations=False, release=get_version())
dongle_id = Params().get("DongleId", encoding='utf-8')
sentry_sdk.set_user({"id": dongle_id})
- sentry_sdk.set_tag("dirty", dirty)
- sentry_sdk.set_tag("origin", origin)
- sentry_sdk.set_tag("branch", branch)
- sentry_sdk.set_tag("commit", commit)
+ sentry_sdk.set_tag("dirty", get_dirty())
+ sentry_sdk.set_tag("origin", get_origin())
+ sentry_sdk.set_tag("branch", get_branch())
+ sentry_sdk.set_tag("commit", get_commit())
sentry_sdk.set_tag("device", HARDWARE.get_device_type())
while True:
diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript
index bc21022dd..52e4e0c42 100644
--- a/selfdrive/ui/SConscript
+++ b/selfdrive/ui/SConscript
@@ -56,10 +56,9 @@ qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs)
qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs)
# build main UI
-qt_src = ["main.cc", "ui.cc", "paint.cc", "qt/sidebar.cc", "qt/onroad.cc",
+qt_src = ["main.cc", "ui.cc", "qt/sidebar.cc", "qt/onroad.cc",
"qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc",
- "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc",
- "#third_party/nanovg/nanovg.c"]
+ "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc"]
qt_env.Program("_ui", qt_src + [asset_obj], LIBS=qt_libs)
@@ -116,7 +115,7 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
replay_lib_src = ["replay/replay.cc", "replay/camera.cc", "replay/filereader.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"]
replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs)
- replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'swscale', 'yuv'] + qt_libs
+ replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs
qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs)
qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11'])
@@ -126,5 +125,8 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
# navd
if maps:
- navd_src = ["navd/main.cc", "navd/route_engine.cc"]
+ navd_src = ["navd/main.cc", "navd/route_engine.cc", "navd/map_renderer.cc"]
qt_env.Program("navd/_navd", navd_src, LIBS=qt_libs + ['common', 'json11'])
+
+ if GetOption('extras'):
+ qt_env.SharedLibrary("navd/map_renderer", ["navd/map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging'])
diff --git a/selfdrive/ui/navd/main.cc b/selfdrive/ui/navd/main.cc
index cc8d9c339..7ae0393a4 100644
--- a/selfdrive/ui/navd/main.cc
+++ b/selfdrive/ui/navd/main.cc
@@ -5,9 +5,11 @@
#include
#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/navd/route_engine.h"
-
-RouteEngine* route_engine = nullptr;
+#include "selfdrive/ui/navd/map_renderer.h"
+#include "selfdrive/hardware/hw.h"
+#include "selfdrive/common/params.h"
void sigHandler(int s) {
qInfo() << "Shutting down";
@@ -30,7 +32,14 @@ int main(int argc, char *argv[]) {
parser.process(app);
const QStringList args = parser.positionalArguments();
- route_engine = new RouteEngine();
+
+ RouteEngine* route_engine = new RouteEngine();
+
+ if (Params().getBool("NavdRender")) {
+ MapRenderer * m = new MapRenderer(get_mapbox_settings());
+ QObject::connect(route_engine, &RouteEngine::positionUpdated, m, &MapRenderer::updatePosition);
+ QObject::connect(route_engine, &RouteEngine::routeUpdated, m, &MapRenderer::updateRoute);
+ }
return app.exec();
}
diff --git a/selfdrive/ui/navd/map_renderer.cc b/selfdrive/ui/navd/map_renderer.cc
new file mode 100644
index 000000000..1d78d68df
--- /dev/null
+++ b/selfdrive/ui/navd/map_renderer.cc
@@ -0,0 +1,200 @@
+#include "selfdrive/ui/navd/map_renderer.h"
+
+#include
+#include
+#include
+
+#include "selfdrive/ui/qt/maps/map_helpers.h"
+#include "selfdrive/common/timing.h"
+
+const float ZOOM = 13.5; // Don't go below 13 or features will start to disappear
+const int WIDTH = 256;
+const int HEIGHT = WIDTH;
+
+const int NUM_VIPC_BUFFERS = 4;
+
+MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool enable_vipc) : m_settings(settings) {
+ QSurfaceFormat fmt;
+ fmt.setRenderableType(QSurfaceFormat::OpenGLES);
+
+ ctx = std::make_unique();
+ ctx->setFormat(fmt);
+ ctx->create();
+ assert(ctx->isValid());
+
+ surface = std::make_unique();
+ surface->setFormat(ctx->format());
+ surface->create();
+
+ ctx->makeCurrent(surface.get());
+ assert(QOpenGLContext::currentContext() == ctx.get());
+
+ gl_functions.reset(ctx->functions());
+ gl_functions->initializeOpenGLFunctions();
+
+ QOpenGLFramebufferObjectFormat fbo_format;
+ fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format));
+
+ m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1));
+ m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), ZOOM);
+ m_map->setStyleUrl("mapbox://styles/commaai/ckvmksrpd4n0a14pfdo5heqzr");
+ m_map->createRenderer();
+
+ m_map->resize(fbo->size());
+ m_map->setFramebufferObject(fbo->handle(), fbo->size());
+ gl_functions->glViewport(0, 0, WIDTH, HEIGHT);
+
+ if (enable_vipc) {
+ qWarning() << "Enabling navd map rendering";
+ vipc_server.reset(new VisionIpcServer("navd"));
+ vipc_server->create_buffers(VisionStreamType::VISION_STREAM_RGB_MAP, NUM_VIPC_BUFFERS, true, WIDTH, HEIGHT);
+ vipc_server->start_listener();
+
+ pm.reset(new PubMaster({"navThumbnail"}));
+ }
+}
+
+void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) {
+ if (m_map.isNull()) {
+ return;
+ }
+
+ m_map->setCoordinate(position);
+ m_map->setBearing(bearing);
+ update();
+}
+
+bool MapRenderer::loaded() {
+ return m_map->isFullyLoaded();
+}
+
+void MapRenderer::update() {
+ gl_functions->glClear(GL_COLOR_BUFFER_BIT);
+ m_map->render();
+ gl_functions->glFlush();
+
+ sendVipc();
+}
+
+void MapRenderer::sendVipc() {
+ if (!vipc_server || !loaded()) {
+ return;
+ }
+
+ QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+ uint64_t ts = nanos_since_boot();
+ VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_RGB_MAP);
+ VisionIpcBufExtra extra = {
+ .frame_id = frame_id,
+ .timestamp_sof = ts,
+ .timestamp_eof = ts,
+ };
+
+ assert(cap.sizeInBytes() == buf->len);
+ memcpy(buf->addr, cap.bits(), buf->len);
+ vipc_server->send(buf, &extra);
+
+ if (frame_id % 100 == 0) {
+ // Write jpeg into buffer
+ QByteArray buffer_bytes;
+ QBuffer buffer(&buffer_bytes);
+ buffer.open(QIODevice::WriteOnly);
+ cap.save(&buffer, "JPG", 50);
+
+ kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size());
+
+ // Send thumbnail
+ MessageBuilder msg;
+ auto thumbnaild = msg.initEvent().initNavThumbnail();
+ thumbnaild.setFrameId(frame_id);
+ thumbnaild.setTimestampEof(ts);
+ thumbnaild.setThumbnail(buffer_kj);
+ pm->send("navThumbnail", msg);
+ }
+
+ frame_id++;
+}
+
+uint8_t* MapRenderer::getImage() {
+ QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+ uint8_t* buf = new uint8_t[cap.sizeInBytes()];
+ memcpy(buf, cap.bits(), cap.sizeInBytes());
+
+ return buf;
+}
+
+void MapRenderer::updateRoute(QList coordinates) {
+ if (m_map.isNull()) return;
+ initLayers();
+
+ auto route_points = coordinate_list_to_collection(coordinates);
+ QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {});
+ QVariantMap navSource;
+ navSource["type"] = "geojson";
+ navSource["data"] = QVariant::fromValue(feature);
+ m_map->updateSource("navSource", navSource);
+ m_map->setLayoutProperty("navLayer", "visibility", "visible");
+}
+
+void MapRenderer::initLayers() {
+ if (!m_map->layerExists("navLayer")) {
+ QVariantMap nav;
+ nav["id"] = "navLayer";
+ nav["type"] = "line";
+ nav["source"] = "navSource";
+ m_map->addLayer(nav, "road-intersection");
+ m_map->setPaintProperty("navLayer", "line-color", QColor("blue"));
+ m_map->setPaintProperty("navLayer", "line-width", 3);
+ m_map->setLayoutProperty("navLayer", "line-cap", "round");
+ }
+}
+
+MapRenderer::~MapRenderer() {
+}
+
+extern "C" {
+ MapRenderer* map_renderer_init() {
+ char *argv[] = {
+ (char*)"navd",
+ nullptr
+ };
+ int argc = 0;
+ QApplication *app = new QApplication(argc, argv);
+ assert(app);
+
+ QMapboxGLSettings settings;
+ settings.setApiBaseUrl(MAPS_HOST);
+ settings.setAccessToken(get_mapbox_token());
+
+ return new MapRenderer(settings, false);
+ }
+
+ void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) {
+ inst->updatePosition({lat, lon}, bearing);
+ QApplication::processEvents();
+ }
+
+ void map_renderer_update_route(MapRenderer *inst, char* polyline) {
+ inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline)));
+ }
+
+ void map_renderer_update(MapRenderer *inst) {
+ inst->update();
+ }
+
+ void map_renderer_process(MapRenderer *inst) {
+ QApplication::processEvents();
+ }
+
+ bool map_renderer_loaded(MapRenderer *inst) {
+ return inst->loaded();
+ }
+
+ uint8_t * map_renderer_get_image(MapRenderer *inst) {
+ return inst->getImage();
+ }
+
+ void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) {
+ delete[] buf;
+ }
+}
diff --git a/selfdrive/ui/navd/map_renderer.h b/selfdrive/ui/navd/map_renderer.h
new file mode 100644
index 000000000..1746e7669
--- /dev/null
+++ b/selfdrive/ui/navd/map_renderer.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "cereal/visionipc/visionipc_server.h"
+#include "cereal/messaging/messaging.h"
+
+
+class MapRenderer : public QObject {
+ Q_OBJECT
+
+public:
+ MapRenderer(const QMapboxGLSettings &, bool enable_vipc=true);
+ uint8_t* getImage();
+ void update();
+ bool loaded();
+ ~MapRenderer();
+
+
+private:
+ std::unique_ptr ctx;
+ std::unique_ptr surface;
+ std::unique_ptr gl_functions;
+ std::unique_ptr fbo;
+
+ std::unique_ptr vipc_server;
+ std::unique_ptr pm;
+ void sendVipc();
+
+ QMapboxGLSettings m_settings;
+ QScopedPointer m_map;
+
+ void initLayers();
+
+ uint32_t frame_id = 0;
+
+public slots:
+ void updatePosition(QMapbox::Coordinate position, float bearing);
+ void updateRoute(QList coordinates);
+};
diff --git a/selfdrive/ui/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc
index 30d070dc7..f3cb7df3d 100644
--- a/selfdrive/ui/navd/route_engine.cc
+++ b/selfdrive/ui/navd/route_engine.cc
@@ -91,7 +91,6 @@ static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMa
}
}
}
-
}
RouteEngine::RouteEngine() {
@@ -99,14 +98,17 @@ RouteEngine::RouteEngine() {
pm = new PubMaster({"navInstruction", "navRoute"});
// Timers
- timer = new QTimer(this);
- QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate()));
- timer->start(1000);
+ route_timer = new QTimer(this);
+ QObject::connect(route_timer, SIGNAL(timeout()), this, SLOT(routeUpdate()));
+ route_timer->start(1000);
+
+ msg_timer = new QTimer(this);
+ QObject::connect(msg_timer, SIGNAL(timeout()), this, SLOT(msgUpdate()));
+ msg_timer->start(50);
// Build routing engine
QVariantMap parameters;
- QString token = MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
- parameters["mapbox.access_token"] = token;
+ parameters["mapbox.access_token"] = get_mapbox_token();
parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/";
geoservice_provider = new QGeoServiceProvider("mapbox", parameters);
@@ -124,12 +126,14 @@ RouteEngine::RouteEngine() {
}
}
-void RouteEngine::timerUpdate() {
- sm->update(0);
+void RouteEngine::msgUpdate() {
+ sm->update(1000);
if (!sm->updated("liveLocationKalman")) {
+ active = false;
return;
}
+
if (sm->updated("managerState")) {
for (auto const &p : (*sm)["managerState"].getManagerState().getProcesses()) {
if (p.getName() == "ui" && p.getRunning()) {
@@ -153,6 +157,15 @@ void RouteEngine::timerUpdate() {
if (localizer_valid) {
last_bearing = RAD2DEG(orientation.getValue()[2]);
last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]);
+ emit positionUpdated(*last_position, *last_bearing);
+ }
+
+ active = true;
+}
+
+void RouteEngine::routeUpdate() {
+ if (!active) {
+ return;
}
recomputeRoute();
@@ -261,6 +274,10 @@ bool RouteEngine::shouldRecompute() {
}
void RouteEngine::recomputeRoute() {
+ if (!last_position) {
+ return;
+ }
+
auto new_destination = coordinate_from_param("NavDestination");
if (!new_destination) {
clearRoute();
@@ -308,6 +325,9 @@ void RouteEngine::routeCalculated(QGeoRouteReply *reply) {
route = reply->routes().at(0);
segment = route.firstRouteSegment();
+
+ auto path = route.path();
+ emit routeUpdated(path);
} else {
qWarning() << "Got empty route response";
}
diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h
index 9a6c99041..33cbc7910 100644
--- a/selfdrive/ui/navd/route_engine.h
+++ b/selfdrive/ui/navd/route_engine.h
@@ -23,7 +23,8 @@ public:
SubMaster *sm;
PubMaster *pm;
- QTimer* timer;
+ QTimer* msg_timer;
+ QTimer* route_timer;
std::optional ui_pid;
@@ -41,6 +42,7 @@ public:
bool localizer_valid = false;
// Route recompute
+ bool active = false;
int recompute_backoff = 0;
int recompute_countdown = 0;
void calculateRoute(QMapbox::Coordinate destination);
@@ -48,8 +50,13 @@ public:
bool shouldRecompute();
private slots:
- void timerUpdate();
+ void routeUpdate();
+ void msgUpdate();
void routeCalculated(QGeoRouteReply *reply);
void recomputeRoute();
void sendRoute();
+
+signals:
+ void positionUpdated(QMapbox::Coordinate position, float bearing);
+ void routeUpdated(QList coordinates);
};
diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
deleted file mode 100644
index 612b924f3..000000000
--- a/selfdrive/ui/paint.cc
+++ /dev/null
@@ -1,311 +0,0 @@
-#include "selfdrive/ui/paint.h"
-
-#include
-
-#ifdef __APPLE__
-#include
-#define NANOVG_GL3_IMPLEMENTATION
-#define nvgCreate nvgCreateGL3
-#else
-#include
-#define NANOVG_GLES3_IMPLEMENTATION
-#define nvgCreate nvgCreateGLES3
-#endif
-
-#define NANOVG_GLES3_IMPLEMENTATION
-#include
-#include
-
-#include "selfdrive/common/util.h"
-#include "selfdrive/hardware/hw.h"
-#include "selfdrive/ui/ui.h"
-
-static void ui_draw_text(const UIState *s, float x, float y, const char *string, float size, NVGcolor color, const char *font_name) {
- nvgFontFace(s->vg, font_name);
- nvgFontSize(s->vg, size);
- nvgFillColor(s->vg, color);
- nvgText(s->vg, x, y, string, NULL);
-}
-
-static void draw_chevron(UIState *s, float x, float y, float sz, NVGcolor fillColor, NVGcolor glowColor) {
- // glow
- float g_xo = sz/5;
- float g_yo = sz/10;
- nvgBeginPath(s->vg);
- nvgMoveTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo);
- nvgLineTo(s->vg, x, y-g_xo);
- nvgLineTo(s->vg, x-(sz*1.35)-g_xo, y+sz+g_yo);
- nvgClosePath(s->vg);
- nvgFillColor(s->vg, glowColor);
- nvgFill(s->vg);
-
- // chevron
- nvgBeginPath(s->vg);
- nvgMoveTo(s->vg, x+(sz*1.25), y+sz);
- nvgLineTo(s->vg, x, y);
- nvgLineTo(s->vg, x-(sz*1.25), y+sz);
- nvgClosePath(s->vg);
- nvgFillColor(s->vg, fillColor);
- nvgFill(s->vg);
-}
-
-static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, int radius, const char *image, NVGcolor color, float img_alpha) {
- nvgBeginPath(s->vg);
- nvgCircle(s->vg, center_x, center_y, radius);
- nvgFillColor(s->vg, color);
- nvgFill(s->vg);
- const int img_size = radius * 1.5;
- ui_draw_image(s, {center_x - (img_size / 2), center_y - (img_size / 2), img_size, img_size}, image, img_alpha);
-}
-
-static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, int radius, const char *image, bool active) {
- float bg_alpha = active ? 0.3f : 0.1f;
- float img_alpha = active ? 1.0f : 0.15f;
- ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
-}
-
-static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &lead_data, const vertex_data &vd) {
- // Draw lead car indicator
- auto [x, y] = vd;
-
- float fillAlpha = 0;
- float speedBuff = 10.;
- float leadBuff = 40.;
- float d_rel = lead_data.getDRel();
- float v_rel = lead_data.getVRel();
- if (d_rel < leadBuff) {
- fillAlpha = 255*(1.0-(d_rel/leadBuff));
- if (v_rel < 0) {
- fillAlpha += 255*(-1*(v_rel/speedBuff));
- }
- fillAlpha = (int)(fmin(fillAlpha, 255));
- }
-
- float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35;
- x = std::clamp(x, 0.f, s->fb_w - sz / 2);
- y = std::fmin(s->fb_h - sz * .6, y);
- draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
-}
-
-static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *color, NVGpaint *paint) {
- if (vd.cnt == 0) return;
-
- const vertex_data *v = &vd.v[0];
- nvgBeginPath(s->vg);
- nvgMoveTo(s->vg, v[0].x, v[0].y);
- for (int i = 1; i < vd.cnt; i++) {
- nvgLineTo(s->vg, v[i].x, v[i].y);
- }
- nvgClosePath(s->vg);
- if (color) {
- nvgFillColor(s->vg, *color);
- } else if (paint) {
- nvgFillPaint(s->vg, *paint);
- }
- nvgFill(s->vg);
-}
-
-static void ui_draw_vision_lane_lines(UIState *s) {
- const UIScene &scene = s->scene;
- NVGpaint track_bg;
- if (!scene.end_to_end) {
- // paint lanelines
- for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
- NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]);
- ui_draw_line(s, scene.lane_line_vertices[i], &color, nullptr);
- }
-
- // paint road edges
- for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
- NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0 - scene.road_edge_stds[i], 0.0, 1.0));
- ui_draw_line(s, scene.road_edge_vertices[i], &color, nullptr);
- }
- track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
- COLOR_WHITE, COLOR_WHITE_ALPHA(0));
- } else {
- track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
- COLOR_RED, COLOR_RED_ALPHA(0));
- }
- // paint path
- ui_draw_line(s, scene.track_vertices, nullptr, &track_bg);
-}
-
-// Draw all world space objects.
-static void ui_draw_world(UIState *s) {
- nvgScissor(s->vg, 0, 0, s->fb_w, s->fb_h);
-
- // Draw lane edges and vision/mpc tracks
- ui_draw_vision_lane_lines(s);
-
- // Draw lead indicators if openpilot is handling longitudinal
- if (s->scene.longitudinal_control) {
- auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
- auto lead_two = (*s->sm)["radarState"].getRadarState().getLeadTwo();
- if (lead_one.getStatus()) {
- draw_lead(s, lead_one, s->scene.lead_vertices[0]);
- }
- if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
- draw_lead(s, lead_two, s->scene.lead_vertices[1]);
- }
- }
- nvgResetScissor(s->vg);
-}
-
-static void ui_draw_vision_maxspeed(UIState *s) {
- const int SET_SPEED_NA = 255;
- float maxspeed = (*s->sm)["controlsState"].getControlsState().getVCruise();
- const bool is_cruise_set = maxspeed != 0 && maxspeed != SET_SPEED_NA;
- if (is_cruise_set && !s->scene.is_metric) { maxspeed *= KM_TO_MILE; }
-
- const Rect rect = {bdr_s * 2, int(bdr_s * 1.5), 184, 202};
- ui_fill_rect(s->vg, rect, COLOR_BLACK_ALPHA(100), 30.);
- ui_draw_rect(s->vg, rect, COLOR_WHITE_ALPHA(100), 10, 20.);
-
- nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
- ui_draw_text(s, rect.centerX(), 118, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), "sans-regular");
- if (is_cruise_set) {
- const std::string maxspeed_str = std::to_string((int)std::nearbyint(maxspeed));
- ui_draw_text(s, rect.centerX(), 212, maxspeed_str.c_str(), 48 * 2.5, COLOR_WHITE, "sans-bold");
- } else {
- ui_draw_text(s, rect.centerX(), 212, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), "sans-semibold");
- }
-}
-
-static void ui_draw_vision_speed(UIState *s) {
- const float speed = std::max(0.0, (*s->sm)["carState"].getCarState().getVEgo() * (s->scene.is_metric ? MS_TO_KPH : MS_TO_MPH));
- const std::string speed_str = std::to_string((int)std::nearbyint(speed));
- nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
- ui_draw_text(s, s->fb_w/2, 210, speed_str.c_str(), 96 * 2.5, COLOR_WHITE, "sans-bold");
- ui_draw_text(s, s->fb_w/2, 290, s->scene.is_metric ? "km/h" : "mph", 36 * 2.5, COLOR_WHITE_ALPHA(200), "sans-regular");
-}
-
-static void ui_draw_vision_event(UIState *s) {
- if (s->scene.engageable) {
- // draw steering wheel
- const int radius = 96;
- const int center_x = s->fb_w - radius - bdr_s * 2;
- const int center_y = radius + (bdr_s * 1.5);
- const QColor &color = bg_colors[s->status];
- NVGcolor nvg_color = nvgRGBA(color.red(), color.green(), color.blue(), color.alpha());
- ui_draw_circle_image(s, center_x, center_y, radius, "wheel", nvg_color, 1.0f);
- }
-}
-
-static void ui_draw_vision_face(UIState *s) {
- const int radius = 96;
- const int center_x = radius + (bdr_s * 2);
- const int center_y = s->fb_h - footer_h / 2;
- ui_draw_circle_image(s, center_x, center_y, radius, "driver_face", s->scene.dm_active);
-}
-
-static void ui_draw_vision_header(UIState *s) {
- NVGpaint gradient = nvgLinearGradient(s->vg, 0, header_h - (header_h / 2.5), 0, header_h,
- nvgRGBAf(0, 0, 0, 0.45), nvgRGBAf(0, 0, 0, 0));
- ui_fill_rect(s->vg, {0, 0, s->fb_w , header_h}, gradient);
- ui_draw_vision_maxspeed(s);
- ui_draw_vision_speed(s);
- ui_draw_vision_event(s);
-}
-
-static void ui_draw_vision(UIState *s) {
- const UIScene *scene = &s->scene;
- // Draw augmented elements
- if (scene->world_objects_visible) {
- ui_draw_world(s);
- }
- // Set Speed, Current Speed, Status/Events
- ui_draw_vision_header(s);
- if ((*s->sm)["controlsState"].getControlsState().getAlertSize() == cereal::ControlsState::AlertSize::NONE) {
- ui_draw_vision_face(s);
- }
-}
-
-void ui_draw(UIState *s, int w, int h) {
- // Update intrinsics matrix after possible wide camera toggle change
- if (s->fb_w != w || s->fb_h != h) {
- ui_resize(s, w, h);
- }
- glEnable(GL_BLEND);
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
- nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
- ui_draw_vision(s);
- nvgEndFrame(s->vg);
- glDisable(GL_BLEND);
-}
-
-void ui_draw_image(const UIState *s, const Rect &r, const char *name, float alpha) {
- nvgBeginPath(s->vg);
- NVGpaint imgPaint = nvgImagePattern(s->vg, r.x, r.y, r.w, r.h, 0, s->images.at(name), alpha);
- nvgRect(s->vg, r.x, r.y, r.w, r.h);
- nvgFillPaint(s->vg, imgPaint);
- nvgFill(s->vg);
-}
-
-void ui_draw_rect(NVGcontext *vg, const Rect &r, NVGcolor color, int width, float radius) {
- nvgBeginPath(vg);
- radius > 0 ? nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius) : nvgRect(vg, r.x, r.y, r.w, r.h);
- nvgStrokeColor(vg, color);
- nvgStrokeWidth(vg, width);
- nvgStroke(vg);
-}
-
-static inline void fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor *color, const NVGpaint *paint, float radius) {
- nvgBeginPath(vg);
- radius > 0 ? nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius) : nvgRect(vg, r.x, r.y, r.w, r.h);
- if (color) nvgFillColor(vg, *color);
- if (paint) nvgFillPaint(vg, *paint);
- nvgFill(vg);
-}
-void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor &color, float radius) {
- fill_rect(vg, r, &color, nullptr, radius);
-}
-void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGpaint &paint, float radius) {
- fill_rect(vg, r, nullptr, &paint, radius);
-}
-
-void ui_nvg_init(UIState *s) {
- // on EON, we enable MSAA
- s->vg = Hardware::EON() ? nvgCreate(0) : nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
- assert(s->vg);
-
- // init fonts
- std::pair fonts[] = {
- {"sans-regular", "../assets/fonts/opensans_regular.ttf"},
- {"sans-semibold", "../assets/fonts/opensans_semibold.ttf"},
- {"sans-bold", "../assets/fonts/opensans_bold.ttf"},
- };
- for (auto [name, file] : fonts) {
- int font_id = nvgCreateFont(s->vg, name, file);
- assert(font_id >= 0);
- }
-
- // init images
- std::vector> images = {
- {"wheel", "../assets/img_chffr_wheel.png"},
- {"driver_face", "../assets/img_driver_face.png"},
- };
- for (auto [name, file] : images) {
- s->images[name] = nvgCreateImage(s->vg, file, 1);
- assert(s->images[name] != 0);
- }
-}
-
-void ui_resize(UIState *s, int width, int height) {
- s->fb_w = width;
- s->fb_h = height;
-
- auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
- float zoom = ZOOM / intrinsic_matrix.v[0];
- if (s->wide_camera) {
- zoom *= 0.5;
- }
-
- // Apply transformation such that video pixel coordinates match video
- // 1) Put (0, 0) in the middle of the video
- // 2) Apply same scaling as video
- // 3) Put (0, 0) in top left corner of video
- s->car_space_transform.reset();
- s->car_space_transform.translate(width / 2, height / 2 + y_offset)
- .scale(zoom, zoom)
- .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
-}
diff --git a/selfdrive/ui/paint.h b/selfdrive/ui/paint.h
deleted file mode 100644
index 4ce7cbd13..000000000
--- a/selfdrive/ui/paint.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#pragma once
-
-#include "selfdrive/ui/ui.h"
-
-void ui_draw(UIState *s, int w, int h);
-void ui_draw_image(const UIState *s, const Rect &r, const char *name, float alpha);
-void ui_draw_rect(NVGcontext *vg, const Rect &r, NVGcolor color, int width, float radius = 0);
-void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGpaint &paint, float radius = 0);
-void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor &color, float radius = 0);
-void ui_nvg_init(UIState *s);
-void ui_resize(UIState *s, int width, int height);
-void ui_update_params(UIState *s);
diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc
index 6339884ae..8ac9b4131 100644
--- a/selfdrive/ui/qt/api.cc
+++ b/selfdrive/ui/qt/api.cc
@@ -4,6 +4,7 @@
#include
#include
+#include
#include
#include
#include
@@ -62,18 +63,20 @@ QString create_jwt(const QJsonObject &payloads, int expiry) {
} // namespace CommaApi
HttpRequest::HttpRequest(QObject *parent, bool create_jwt, int timeout) : create_jwt(create_jwt), QObject(parent) {
- networkAccessManager = new QNetworkAccessManager(this);
-
networkTimer = new QTimer(this);
networkTimer->setSingleShot(true);
networkTimer->setInterval(timeout);
connect(networkTimer, &QTimer::timeout, this, &HttpRequest::requestTimeout);
}
-bool HttpRequest::active() {
+bool HttpRequest::active() const {
return reply != nullptr;
}
+bool HttpRequest::timeout() const {
+ return reply && reply->error() == QNetworkReply::OperationCanceledError;
+}
+
void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Method method) {
if (active()) {
qDebug() << "HttpRequest is active";
@@ -97,9 +100,9 @@ void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Meth
}
if (method == HttpRequest::Method::GET) {
- reply = networkAccessManager->get(request);
+ reply = nam()->get(request);
} else if (method == HttpRequest::Method::DELETE) {
- reply = networkAccessManager->deleteResource(request);
+ reply = nam()->deleteResource(request);
}
networkTimer->start();
@@ -110,29 +113,31 @@ void HttpRequest::requestTimeout() {
reply->abort();
}
-// This function should always emit something
void HttpRequest::requestFinished() {
- bool success = false;
- if (reply->error() != QNetworkReply::OperationCanceledError) {
- networkTimer->stop();
- QString response = reply->readAll();
+ networkTimer->stop();
- if (reply->error() == QNetworkReply::NoError) {
- success = true;
- emit receivedResponse(response);
+ if (reply->error() == QNetworkReply::NoError) {
+ emit requestDone(reply->readAll(), true);
+ } else {
+ QString error;
+ if (reply->error() == QNetworkReply::OperationCanceledError) {
+ nam()->clearAccessCache();
+ nam()->clearConnectionCache();
+ error = "Request timed out";
} else {
- emit failedResponse(reply->errorString());
-
if (reply->error() == QNetworkReply::ContentAccessDenied || reply->error() == QNetworkReply::AuthenticationRequiredError) {
qWarning() << ">> Unauthorized. Authenticate with tools/lib/auth.py <<";
}
+ error = reply->errorString();
}
- } else {
- networkAccessManager->clearAccessCache();
- networkAccessManager->clearConnectionCache();
- emit timeoutResponse("timeout");
+ emit requestDone(error, false);
}
- emit requestDone(success);
+
reply->deleteLater();
reply = nullptr;
}
+
+QNetworkAccessManager *HttpRequest::nam() {
+ static QNetworkAccessManager *networkAccessManager = new QNetworkAccessManager(qApp);
+ return networkAccessManager;
+}
diff --git a/selfdrive/ui/qt/api.h b/selfdrive/ui/qt/api.h
index 1c7c333ea..8f7467810 100644
--- a/selfdrive/ui/qt/api.h
+++ b/selfdrive/ui/qt/api.h
@@ -27,23 +27,21 @@ public:
explicit HttpRequest(QObject* parent, bool create_jwt = true, int timeout = 20000);
void sendRequest(const QString &requestURL, const Method method = Method::GET);
- bool active();
+ bool active() const;
+ bool timeout() const;
+
+signals:
+ void requestDone(const QString &response, bool success);
protected:
QNetworkReply *reply = nullptr;
private:
- QNetworkAccessManager *networkAccessManager = nullptr;
+ static QNetworkAccessManager *nam();
QTimer *networkTimer = nullptr;
bool create_jwt;
private slots:
void requestTimeout();
void requestFinished();
-
-signals:
- void requestDone(bool success);
- void receivedResponse(const QString &response);
- void failedResponse(const QString &errorString);
- void timeoutResponse(const QString &errorString);
};
diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc
index 5484c5d74..1e8926495 100644
--- a/selfdrive/ui/qt/home.cc
+++ b/selfdrive/ui/qt/home.cc
@@ -178,6 +178,6 @@ void OffroadHome::refresh() {
update_notif->setVisible(updateAvailable);
alert_notif->setVisible(alerts);
if (alerts) {
- alert_notif->setText(QString::number(alerts) + " ALERT" + (alerts > 1 ? "S" : ""));
+ alert_notif->setText(QString::number(alerts) + (alerts > 1 ? " ALERTS" : " ALERT"));
}
}
diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc
index 7bee7f7ca..e1bfe96c8 100644
--- a/selfdrive/ui/qt/maps/map.cc
+++ b/selfdrive/ui/qt/maps/map.cc
@@ -112,10 +112,6 @@ void MapWindow::timerUpdate() {
update();
- if (m_map.isNull()) {
- return;
- }
-
sm->update(0);
if (sm->updated("liveLocationKalman")) {
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
@@ -134,6 +130,21 @@ void MapWindow::timerUpdate() {
velocity_filter.update(velocity);
}
}
+
+ if (sm->updated("navRoute")) {
+ qWarning() << "Got new navRoute from navd. Opening map:" << allow_open;
+
+ // Only open the map on setting destination the first time
+ if (allow_open) {
+ setVisible(true); // Show map on destination set/change
+ allow_open = false;
+ }
+ }
+
+ if (m_map.isNull()) {
+ return;
+ }
+
loaded_once = loaded_once || m_map->isFullyLoaded();
if (!loaded_once) {
map_instructions->showError("Map Loading");
@@ -186,7 +197,7 @@ void MapWindow::timerUpdate() {
}
if (sm->rcv_frame("navRoute") != route_rcv_frame) {
- qWarning() << "Got new navRoute from navd";
+ qWarning() << "Updating navLayer with new route";
auto route = (*sm)["navRoute"].getNavRoute();
auto route_points = capnp_coordinate_list_to_collection(route.getCoordinates());
QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {});
@@ -196,11 +207,6 @@ void MapWindow::timerUpdate() {
m_map->updateSource("navSource", navSource);
m_map->setLayoutProperty("navLayer", "visibility", "visible");
- // Only open the map on setting destination the first time
- if (allow_open) {
- setVisible(true); // Show map on destination set/change
- allow_open = false;
- }
route_rcv_frame = sm->rcv_frame("navRoute");
}
}
diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h
index b74f5bf29..c62089cb2 100644
--- a/selfdrive/ui/qt/maps/map.h
+++ b/selfdrive/ui/qt/maps/map.h
@@ -22,9 +22,6 @@
#include "selfdrive/common/util.h"
#include "cereal/messaging/messaging.h"
-const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str();
-const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str();
-
class MapInstructions : public QWidget {
Q_OBJECT
diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc
index 422ae99b6..f87e80403 100644
--- a/selfdrive/ui/qt/maps/map_helpers.cc
+++ b/selfdrive/ui/qt/maps/map_helpers.cc
@@ -4,7 +4,25 @@
#include
#include "selfdrive/common/params.h"
+#include "selfdrive/hardware/hw.h"
+#include "selfdrive/ui/qt/api.h"
+QString get_mapbox_token() {
+ // Valid for 4 weeks since we can't swap tokens on the fly
+ return MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
+}
+
+QMapboxGLSettings get_mapbox_settings() {
+ QMapboxGLSettings settings;
+
+ if (!Hardware::PC()) {
+ settings.setCacheDatabasePath(MAPS_CACHE_PATH);
+ }
+ settings.setApiBaseUrl(MAPS_HOST);
+ settings.setAccessToken(get_mapbox_token());
+
+ return settings;
+}
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) {
return QGeoCoordinate(in.first, in.second);
@@ -83,6 +101,48 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(QList polyline_to_coordinate_list(const QString &polylineString) {
+ QList path;
+ if (polylineString.isEmpty())
+ return path;
+
+ QByteArray data = polylineString.toLatin1();
+
+ bool parsingLatitude = true;
+
+ int shift = 0;
+ int value = 0;
+
+ QGeoCoordinate coord(0, 0);
+
+ for (int i = 0; i < data.length(); ++i) {
+ unsigned char c = data.at(i) - 63;
+
+ value |= (c & 0x1f) << shift;
+ shift += 5;
+
+ // another chunk
+ if (c & 0x20)
+ continue;
+
+ int diff = (value & 1) ? ~(value >> 1) : (value >> 1);
+
+ if (parsingLatitude) {
+ coord.setLatitude(coord.latitude() + (double)diff/1e6);
+ } else {
+ coord.setLongitude(coord.longitude() + (double)diff/1e6);
+ path.append(coord);
+ }
+
+ parsingLatitude = !parsingLatitude;
+
+ value = 0;
+ shift = 0;
+ }
+
+ return path;
+}
+
static QGeoCoordinate sub(QGeoCoordinate v, QGeoCoordinate w) {
return QGeoCoordinate(v.latitude() - w.latitude(), v.longitude() - w.longitude());
}
diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h
index 53a44f42d..1c8cacbeb 100644
--- a/selfdrive/ui/qt/maps/map_helpers.h
+++ b/selfdrive/ui/qt/maps/map_helpers.h
@@ -5,12 +5,17 @@
#include
#include
+#include "selfdrive/common/util.h"
#include "common/transformations/coordinates.hpp"
#include "common/transformations/orientation.hpp"
#include "cereal/messaging/messaging.h"
-#define RAD2DEG(x) ((x) * 180.0 / M_PI)
+const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str();
+const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str();
+const QString MAPS_CACHE_PATH = "/data/mbgl-cache-navd.db";
+QString get_mapbox_token();
+QMapboxGLSettings get_mapbox_settings();
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in);
QMapbox::CoordinatesCollections model_to_collection(
const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF,
@@ -19,6 +24,7 @@ QMapbox::CoordinatesCollections model_to_collection(
QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c);
QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader &coordinate_list);
QMapbox::CoordinatesCollections coordinate_list_to_collection(QList