diff --git a/.gitignore b/.gitignore
index b4385e16d..5fc18bb07 100644
--- a/.gitignore
+++ b/.gitignore
@@ -42,7 +42,6 @@ selfdrive/logcatd/logcatd
selfdrive/mapd/default_speeds_by_region.json
selfdrive/proclogd/proclogd
selfdrive/ui/_ui
-selfdrive/ui/_soundd
selfdrive/test/longitudinal_maneuvers/out
selfdrive/visiond/visiond
selfdrive/loggerd/loggerd
@@ -78,3 +77,7 @@ selfdrive/modeld/thneed/compile
models/*.thneed
*.bz2
+
+build/
+
+!**/.gitkeep
diff --git a/Jenkinsfile b/Jenkinsfile
index d41a1f904..bcdb7c99f 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -37,7 +37,7 @@ EOF"""
def phone_steps(String device_type, steps) {
lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) {
- timeout(time: 150, unit: 'MINUTES') {
+ timeout(time: 60, unit: 'MINUTES') {
phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),)
steps.each { item ->
phone(device_ip, item[0], item[1])
@@ -53,7 +53,7 @@ pipeline {
SOURCE_DIR = "/data/openpilot_source/"
}
options {
- timeout(time: 3, unit: 'HOURS')
+ timeout(time: 4, unit: 'HOURS')
}
stages {
@@ -145,10 +145,10 @@ pipeline {
stages {
stage('parallel tests') {
parallel {
- stage('Devel Tests') {
+ stage('C2: build') {
steps {
phone_steps("eon-build", [
- ["build devel", "cd $SOURCE_DIR/release && EXTRA_FILES='tools/' ./build_devel.sh"],
+ ["build master-ci", "cd $SOURCE_DIR/release && EXTRA_FILES='tools/' ./build_devel.sh"],
["build openpilot", "cd selfdrive/manager && ./build.py"],
["test manager", "python selfdrive/manager/test/test_manager.py"],
["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
@@ -157,7 +157,7 @@ pipeline {
}
}
- stage('Replay Tests') {
+ stage('C2: replay') {
steps {
phone_steps("eon2", [
["build", "cd selfdrive/manager && ./build.py"],
@@ -166,7 +166,7 @@ pipeline {
}
}
- stage('HW + Unit Tests') {
+ stage('C2: HW + Unit Tests') {
steps {
phone_steps("eon", [
["build", "cd selfdrive/manager && ./build.py"],
@@ -201,29 +201,33 @@ pipeline {
}
*/
- stage('tici Build') {
+ stage('C3: build') {
environment {
R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}"
}
steps {
phone_steps("tici", [
- ["build", "cd selfdrive/manager && ./build.py"],
+ ["build master-ci", "cd $SOURCE_DIR/release && EXTRA_FILES='tools/' ./build_devel.sh"],
+ ["build openpilot", "cd selfdrive/manager && ./build.py"],
+ ["test manager", "python selfdrive/manager/test/test_manager.py"],
["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
+ ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"],
])
}
}
- stage('Unit Tests (tici)') {
+ stage('C3: HW + Unit Tests') {
steps {
phone_steps("tici2", [
["build", "cd selfdrive/manager && ./build.py"],
+ ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"],
["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"],
])
}
}
- stage('EON camerad') {
+ stage('C2: camerad') {
steps {
phone_steps("eon-party", [
["build", "cd selfdrive/manager && ./build.py"],
@@ -233,7 +237,7 @@ pipeline {
}
}
- stage('tici camerad') {
+ stage('C3: camerad') {
steps {
phone_steps("tici-party", [
["build", "cd selfdrive/manager && ./build.py"],
diff --git a/README.md b/README.md
index abe87a72f..b9a58f6a1 100755
--- a/README.md
+++ b/README.md
@@ -17,7 +17,7 @@ Table of Contents
What is openpilot?
------
-[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW) and Lane Departure Warning (LDW) for a growing variety of [supported car makes, models and model years](docs/CARS.md). In addition, while openpilot is engaged, a camera based Driver Monitoring (DM) feature alerts distracted and asleep drivers. See more about [the vehicle integration and limitations here](docs/INTEGRATION.md).
+[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW) and Lane Departure Warning (LDW) for a growing variety of [supported car makes, models and model years](docs/CARS.md). In addition, while openpilot is engaged, a camera based Driver Monitoring (DM) feature alerts distracted and asleep drivers. See more about [the vehicle integration](docs/INTEGRATION.md) and [limitations](docs/LIMITATIONS.md).
@@ -61,6 +61,8 @@ Community and Contributing
openpilot is developed by [comma](https://comma.ai/) and by users like you. We welcome both pull requests and issues on [GitHub](http://github.com/commaai/openpilot). Bug fixes and new car ports are encouraged. Check out [the contributing docs](docs/CONTRIBUTING.md).
+Documentation related to openpilot development can be found on [docs.comma.ai](https://docs.comma.ai). Information about running openpilot (e.g. FAQ, fingerprinting, troubleshooting, custom forks, community hardware) should go on the [wiki](https://github.com/commaai/openpilot/wiki).
+
You can add support for your car by following guides we have written for [Brand](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/) and [Model](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) ports. Generally, a car with adaptive cruise control and lane keep assist is a good candidate. [Join our Discord](https://discord.comma.ai) to discuss car ports: most car makes have a dedicated channel.
Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs/).
diff --git a/RELEASES.md b/RELEASES.md
index 64f0814e2..12a90fba6 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,17 @@
+Version 0.8.11 (2021-11-29)
+========================
+ * Support for CAN FD on the red panda
+ * Support for an external panda on the comma three
+ * Navigation: Show more detailed instructions when approaching maneuver
+ * Fixed occasional steering faults on GM cars thanks to jyoung8607!
+ * Nissan ECU firmware fingerprinting thanks to robin-reckmann, martinl, and razem-io!
+ * Cadillac Escalade ESV 2016 support thanks to Gibby!
+ * Genesis G70 2020 support thanks to tecandrew!
+ * Hyundai Santa Fe Hybrid 2022 support thanks to sunnyhaibin!
+ * Mazda CX-9 2021 support thanks to Jacar!
+ * Volkswagen Polo 2020 support thanks to jyoung8607!
+ * Volkswagen T-Roc 2021 support thanks to jyoung8607!
+
Version 0.8.10 (2021-11-01)
========================
* New driving model
diff --git a/SConstruct b/SConstruct
index 18c8d2f12..67acd3eac 100644
--- a/SConstruct
+++ b/SConstruct
@@ -13,9 +13,9 @@ AddOption('--test',
action='store_true',
help='build test files')
-AddOption('--setup',
+AddOption('--extras',
action='store_true',
- help='build setup and installer files')
+ help='build misc extras, like setup and installer files')
AddOption('--kaitai',
action='store_true',
@@ -68,6 +68,7 @@ lenv = {
"PYTHONPATH": Dir("#").abspath + ":" + Dir("#pyextra/").abspath,
"ACADOS_SOURCE_DIR": Dir("#third_party/acados/acados").abspath,
+ "ACADOS_PYTHON_INTERFACE_PATH": Dir("#pyextra/acados_template").abspath,
"TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer",
}
@@ -181,12 +182,14 @@ env = Environment(
"-O2",
"-Wunused",
"-Werror",
+ "-Wshadow",
"-Wno-unknown-warning-option",
"-Wno-deprecated-register",
"-Wno-register",
"-Wno-inconsistent-missing-override",
"-Wno-c99-designator",
"-Wno-reorder-init-list",
+ "-Wno-error=unused-but-set-variable",
] + cflags + ccflags,
CPPPATH=cpppath + [
@@ -267,7 +270,7 @@ def abspath(x):
py_include = sysconfig.get_paths()['include']
envCython = env.Clone()
envCython["CPPPATH"] += [py_include, np.get_include()]
-envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-deprecated-declarations"]
+envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"]
envCython["LIBS"] = []
if arch == "Darwin":
@@ -429,6 +432,9 @@ SConscript(['selfdrive/ui/SConscript'])
if arch != "Darwin":
SConscript(['selfdrive/logcatd/SConscript'])
+if GetOption('test'):
+ SConscript('panda/tests/safety/SConscript')
+
external_sconscript = GetOption('external_sconscript')
if external_sconscript:
SConscript([external_sconscript])
diff --git a/cereal/car.capnp b/cereal/car.capnp
index d78acb28e..ce81c7eaf 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -104,6 +104,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
wideRoadCameraError @102;
localizerMalfunction @103;
highCpuUsage @105;
+ cruiseMismatch @106;
driverMonitorLowAccDEPRECATED @68;
radarCanErrorDEPRECATED @15;
@@ -150,6 +151,7 @@ struct CarState {
# brake pedal, 0.0-1.0
brake @5 :Float32; # this is user pedal only
brakePressed @6 :Bool; # this is user pedal only
+ brakeHoldActive @38 :Bool;
# steering wheel
steeringAngleDeg @7 :Float32;
@@ -289,6 +291,8 @@ struct CarControl {
active @7 :Bool;
actuators @6 :Actuators;
+ roll @8 :Float32;
+ pitch @9 :Float32;
cruiseControl @4 :CruiseControl;
hudControl @5 :HUDControl;
@@ -350,11 +354,11 @@ struct CarControl {
chimeEngage @1;
chimeDisengage @2;
chimeError @3;
- chimeWarning1 @4;
- chimeWarning2 @5;
- chimeWarningRepeat @6;
+ chimeWarning1 @4; # unused
+ chimeWarningRepeat @5;
+ chimeWarningRepeatInfinite @6;
chimePrompt @7;
- chimeWarning2Repeat @8;
+ chimeWarning2RepeatInfinite @8;
}
}
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 8c6245ab5..92ec6429d 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -307,6 +307,7 @@ struct DeviceState @0xa4d8b5af2aa492eb {
chargingDisabled @18 :Bool;
offroadPowerUsageUwh @23 :UInt32;
carBatteryCapacityUwh @25 :UInt32;
+ powerDrawW @40 :Float32;
# device thermals
cpuTempC @26 :List(Float32);
@@ -315,11 +316,18 @@ struct DeviceState @0xa4d8b5af2aa492eb {
ambientTempC @30 :Float32;
nvmeTempC @35 :List(Float32);
modemTempC @36 :List(Float32);
+ pmicTempC @39 :List(Float32);
+ thermalZones @38 :List(ThermalZone);
thermalStatus @14 :ThermalStatus;
fanSpeedPercentDesired @10 :UInt16;
screenBrightnessPercent @37 :Int8;
+ struct ThermalZone {
+ name @0 :Text;
+ temp @1 :Float32;
+ }
+
enum ThermalStatus {
green @0;
yellow @1;
@@ -685,6 +693,7 @@ struct ModelDataV2 {
orientation @5 :XYZTData;
velocity @6 :XYZTData;
orientationRate @7 :XYZTData;
+ acceleration @19 :XYZTData;
# prediction lanelines and road edges
laneLines @8 :List(XYZTData);
@@ -1285,6 +1294,7 @@ struct DriverMonitoringState @0xb83cda094a1da284 {
struct Boot {
wallTimeNanos @0 :UInt64;
pstore @4 :Map(Text, Data);
+ commands @5 :Map(Text, Data);
launchLog @3 :Text;
lastKmsgDEPRECATED @1 :Data;
@@ -1372,6 +1382,44 @@ struct UploaderState {
lastFilename @6 :Text;
}
+struct NavInstruction {
+ maneuverPrimaryText @0 :Text;
+ maneuverSecondaryText @1 :Text;
+ maneuverDistance @2 :Float32; # m
+ maneuverType @3 :Text; # TODO: Make Enum
+ maneuverModifier @4 :Text; # TODO: Make Enum
+
+ distanceRemaining @5 :Float32; # m
+ timeRemaining @6 :Float32; # s
+ timeRemainingTypical @7 :Float32; # s
+
+ lanes @8 :List(Lane);
+ showFull @9 :Bool;
+
+ struct Lane {
+ directions @0 :List(Direction);
+ active @1 :Bool;
+ activeDirection @2 :Direction;
+ }
+
+ enum Direction {
+ none @0;
+ left @1;
+ right @2;
+ straight @3;
+ }
+
+}
+
+struct NavRoute {
+ coordinates @0 :List(Coordinate);
+
+ struct Coordinate {
+ latitude @0 :Float32;
+ longitude @1 :Float32;
+ }
+}
+
struct Event {
logMonoTime @0 :UInt64; # nanoseconds
valid @67 :Bool = true;
@@ -1429,6 +1477,9 @@ struct Event {
deviceState @6 :DeviceState;
logMessage @18 :Text;
+ # navigation
+ navInstruction @82 :NavInstruction;
+ navRoute @83 :NavRoute;
# *********** debug ***********
testJoystick @52 :Joystick;
diff --git a/cereal/services.py b/cereal/services.py
index 4cf10a344..7b9287780 100755
--- a/cereal/services.py
+++ b/cereal/services.py
@@ -62,6 +62,8 @@ services = {
"modelV2": (True, 20., 40),
"managerState": (True, 2., 1),
"uploaderState": (True, 0., 1),
+ "navInstruction": (True, 0.),
+ "navRoute": (True, 0.),
# debug
"testJoystick": (False, 0.),
diff --git a/cereal/visionipc/visionbuf.cc b/cereal/visionipc/visionbuf.cc
index 39c521913..e94a8bbc0 100644
--- a/cereal/visionipc/visionbuf.cc
+++ b/cereal/visionipc/visionbuf.cc
@@ -22,19 +22,28 @@ void visionbuf_compute_aligned_width_and_height(int width, int height, int *alig
#endif
}
-void VisionBuf::init_rgb(size_t width, size_t height, size_t stride) {
+void VisionBuf::init_rgb(size_t init_width, size_t init_height, size_t init_stride) {
this->rgb = true;
- this->width = width;
- this->height = height;
- this->stride = stride;
+ this->width = init_width;
+ this->height = init_height;
+ this->stride = init_stride;
}
-void VisionBuf::init_yuv(size_t width, size_t height){
+void VisionBuf::init_yuv(size_t init_width, size_t init_height){
this->rgb = false;
- this->width = width;
- this->height = height;
+ this->width = init_width;
+ this->height = init_height;
this->y = (uint8_t *)this->addr;
- this->u = this->y + (width * height);
- this->v = this->u + (width / 2 * height / 2);
+ this->u = this->y + (this->width * this->height);
+ this->v = this->u + (this->width / 2 * this->height / 2);
+}
+
+
+uint64_t VisionBuf::get_frame_id() {
+ return *frame_id;
+}
+
+void VisionBuf::set_frame_id(uint64_t id) {
+ *frame_id = id;
}
diff --git a/cereal/visionipc/visionbuf.h b/cereal/visionipc/visionbuf.h
index 165a3dbdd..418cbacb9 100644
--- a/cereal/visionipc/visionbuf.h
+++ b/cereal/visionipc/visionbuf.h
@@ -18,6 +18,7 @@ enum VisionStreamType {
VISION_STREAM_YUV_BACK,
VISION_STREAM_YUV_FRONT,
VISION_STREAM_YUV_WIDE,
+ VISION_STREAM_RGB_MAP,
VISION_STREAM_MAX,
};
@@ -26,6 +27,7 @@ class VisionBuf {
size_t len = 0;
size_t mmap_len = 0;
void * addr = nullptr;
+ uint64_t *frame_id;
int fd = 0;
bool rgb = false;
@@ -57,6 +59,9 @@ class VisionBuf {
void init_yuv(size_t width, size_t height);
int sync(int dir);
int free();
+
+ void set_frame_id(uint64_t id);
+ uint64_t get_frame_id();
};
void visionbuf_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
diff --git a/cereal/visionipc/visionbuf_cl.cc b/cereal/visionipc/visionbuf_cl.cc
index c203c944c..0286d28fd 100644
--- a/cereal/visionipc/visionbuf_cl.cc
+++ b/cereal/visionipc/visionbuf_cl.cc
@@ -32,14 +32,11 @@ static void *malloc_with_fd(size_t len, int *fd) {
return addr;
}
-void VisionBuf::allocate(size_t len) {
- int fd;
- void *addr = malloc_with_fd(len, &fd);
-
- this->len = len;
- this->mmap_len = len;
- this->addr = addr;
- this->fd = fd;
+void VisionBuf::allocate(size_t length) {
+ this->len = length;
+ this->mmap_len = this->len;
+ this->addr = malloc_with_fd(this->len, &this->fd);
+ this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len);
}
void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx){
@@ -57,6 +54,8 @@ void VisionBuf::import(){
assert(this->fd >= 0);
this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0);
assert(this->addr != MAP_FAILED);
+
+ this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len);
}
diff --git a/cereal/visionipc/visionbuf_ion.cc b/cereal/visionipc/visionbuf_ion.cc
index c367304e9..c66b668c6 100644
--- a/cereal/visionipc/visionbuf_ion.cc
+++ b/cereal/visionipc/visionbuf_ion.cc
@@ -47,13 +47,13 @@ static void ion_init() {
}
}
-void VisionBuf::allocate(size_t len) {
+void VisionBuf::allocate(size_t length) {
int err;
ion_init();
struct ion_allocation_data ion_alloc = {0};
- ion_alloc.len = len + PADDING_CL;
+ ion_alloc.len = length + PADDING_CL + sizeof(uint64_t);
ion_alloc.align = 4096;
ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID;
ion_alloc.flags = ION_FLAG_CACHED;
@@ -66,18 +66,19 @@ void VisionBuf::allocate(size_t len) {
err = HANDLE_EINTR(ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data));
assert(err == 0);
- void *addr = mmap(NULL, ion_alloc.len,
- PROT_READ | PROT_WRITE,
- MAP_SHARED, ion_fd_data.fd, 0);
- assert(addr != MAP_FAILED);
+ void *mmap_addr = mmap(NULL, ion_alloc.len,
+ PROT_READ | PROT_WRITE,
+ MAP_SHARED, ion_fd_data.fd, 0);
+ assert(mmap_addr != MAP_FAILED);
- memset(addr, 0, ion_alloc.len);
+ memset(mmap_addr, 0, ion_alloc.len);
- this->len = len;
+ this->len = length;
this->mmap_len = ion_alloc.len;
- this->addr = addr;
+ this->addr = mmap_addr;
this->handle = ion_alloc.handle;
this->fd = ion_fd_data.fd;
+ this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len + PADDING_CL);
}
void VisionBuf::import(){
@@ -95,6 +96,8 @@ void VisionBuf::import(){
this->handle = fd_data.handle;
this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0);
assert(this->addr != MAP_FAILED);
+
+ this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len + PADDING_CL);
}
void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx) {
diff --git a/cereal/visionipc/visionipc_server.cc b/cereal/visionipc/visionipc_server.cc
index fdacd7843..e15c1aee4 100644
--- a/cereal/visionipc/visionipc_server.cc
+++ b/cereal/visionipc/visionipc_server.cc
@@ -45,7 +45,6 @@ void VisionIpcServer::create_buffers(VisionStreamType type, size_t num_buffers,
size = width * height * 3 / 2;
}
-
// Create map + alloc requested buffers
for (size_t i = 0; i < num_buffers; i++){
VisionBuf* buf = new VisionBuf();
diff --git a/cereal/visionipc/visionipc_tests.cc b/cereal/visionipc/visionipc_tests.cc
index c2cd43da2..a68bda0b8 100644
--- a/cereal/visionipc/visionipc_tests.cc
+++ b/cereal/visionipc/visionipc_tests.cc
@@ -68,6 +68,7 @@ TEST_CASE("Send single buffer"){
VisionIpcBufExtra extra = {0};
extra.frame_id = 1337;
+ buf->set_frame_id(extra.frame_id);
server.send(buf, &extra);
@@ -76,6 +77,7 @@ TEST_CASE("Send single buffer"){
REQUIRE(recv_buf != nullptr);
REQUIRE(*(uint64_t*)recv_buf->addr == 1234);
REQUIRE(extra_recv.frame_id == extra.frame_id);
+ REQUIRE(recv_buf->get_frame_id() == extra.frame_id);
}
diff --git a/common/markdown.py b/common/markdown.py
new file mode 100755
index 000000000..30c5bc2c0
--- /dev/null
+++ b/common/markdown.py
@@ -0,0 +1,48 @@
+from typing import List
+
+HTML_REPLACEMENTS = [
+ (r'&', r'&'),
+ (r'"', r'"'),
+]
+
+
+def parse_markdown(text: str, tab_length: int = 2) -> str:
+ lines = text.split("\n")
+ output: List[str] = []
+ list_level = 0
+
+ def end_outstanding_lists(level: int, end_level: int) -> int:
+ while level > end_level:
+ level -= 1
+ output.append("")
+ if level > 0:
+ output.append("")
+ return end_level
+
+ for i, line in enumerate(lines):
+ if i + 1 < len(lines) and lines[i + 1].startswith("==="): # heading
+ output.append(f"{line}
")
+ elif line.startswith("==="):
+ pass
+ elif line.lstrip().startswith("* "): # list
+ line_level = 1 + line.count(" " * tab_length, 0, line.index("*"))
+ if list_level >= line_level:
+ list_level = end_outstanding_lists(list_level, line_level)
+ else:
+ list_level += 1
+ if list_level > 1:
+ output[-1] = output[-1].replace("", "")
+ output.append("")
+ output.append(f"- {line.replace('*', '', 1).lstrip()}
")
+ else:
+ list_level = end_outstanding_lists(list_level, 0)
+ if len(line) > 0:
+ output.append(line)
+
+ end_outstanding_lists(list_level, 0)
+ output_str = "\n".join(output) + "\n"
+
+ for (fr, to) in HTML_REPLACEMENTS:
+ output_str = output_str.replace(fr, to)
+
+ return output_str
diff --git a/common/params_pxd.pxd b/common/params_pxd.pxd
deleted file mode 100644
index ecf520041..000000000
--- a/common/params_pxd.pxd
+++ /dev/null
@@ -1,28 +0,0 @@
-from libcpp.string cimport string
-from libcpp cimport bool
-
-cdef extern from "selfdrive/common/params.cc":
- pass
-
-cdef extern from "selfdrive/common/util.cc":
- pass
-
-cdef extern from "selfdrive/common/params.h":
- cpdef enum ParamKeyType:
- PERSISTENT
- CLEAR_ON_MANAGER_START
- CLEAR_ON_PANDA_DISCONNECT
- CLEAR_ON_IGNITION_ON
- CLEAR_ON_IGNITION_OFF
- ALL
-
- cdef cppclass Params:
- Params() nogil
- Params(string) nogil
- string get(string, bool) nogil
- bool getBool(string) nogil
- int remove(string) nogil
- int put(string, string) nogil
- int putBool(string, bool) nogil
- bool checkKey(string) nogil
- void clearAll(ParamKeyType)
diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx
index 198c69068..7b35e815d 100755
--- a/common/params_pyx.pyx
+++ b/common/params_pyx.pyx
@@ -2,27 +2,30 @@
# cython: language_level = 3
from libcpp cimport bool
from libcpp.string cimport string
-from common.params_pxd cimport Params as c_Params, ParamKeyType as c_ParamKeyType
-
-import os
import threading
-from common.basedir import BASEDIR
+cdef extern from "selfdrive/common/params.h":
+ cpdef enum ParamKeyType:
+ PERSISTENT
+ CLEAR_ON_MANAGER_START
+ CLEAR_ON_PANDA_DISCONNECT
+ CLEAR_ON_IGNITION_ON
+ CLEAR_ON_IGNITION_OFF
+ ALL
+
+ cdef cppclass c_Params "Params":
+ c_Params(string) nogil
+ string get(string, bool) nogil
+ bool getBool(string) nogil
+ int remove(string) nogil
+ int put(string, string) nogil
+ int putBool(string, bool) nogil
+ bool checkKey(string) nogil
+ void clearAll(ParamKeyType)
-cdef class ParamKeyType:
- PERSISTENT = c_ParamKeyType.PERSISTENT
- CLEAR_ON_MANAGER_START = c_ParamKeyType.CLEAR_ON_MANAGER_START
- CLEAR_ON_PANDA_DISCONNECT = c_ParamKeyType.CLEAR_ON_PANDA_DISCONNECT
- CLEAR_ON_IGNITION_ON = c_ParamKeyType.CLEAR_ON_IGNITION_ON
- CLEAR_ON_IGNITION_OFF = c_ParamKeyType.CLEAR_ON_IGNITION_OFF
- ALL = c_ParamKeyType.ALL
def ensure_bytes(v):
- if isinstance(v, str):
- return v.encode()
- else:
- return v
-
+ return v.encode() if isinstance(v, str) else v;
class UnknownKeyName(Exception):
pass
@@ -30,36 +33,25 @@ class UnknownKeyName(Exception):
cdef class Params:
cdef c_Params* p
- def __cinit__(self, d=None):
- cdef string path
- if d is None:
- with nogil:
- self.p = new c_Params()
- else:
- path = d.encode()
- with nogil:
- self.p = new c_Params(path)
+ def __cinit__(self, d=""):
+ cdef string path = d.encode()
+ with nogil:
+ self.p = new c_Params(path)
def __dealloc__(self):
del self.p
- def clear_all(self, tx_type=None):
- if tx_type is None:
- tx_type = ParamKeyType.ALL
-
+ def clear_all(self, tx_type=ParamKeyType.ALL):
self.p.clearAll(tx_type)
def check_key(self, key):
key = ensure_bytes(key)
-
if not self.p.checkKey(key):
raise UnknownKeyName(key)
-
return key
def get(self, key, bool block=False, encoding=None):
cdef string k = self.check_key(key)
-
cdef string val
with nogil:
val = self.p.get(k, block)
@@ -72,10 +64,7 @@ cdef class Params:
else:
return None
- if encoding is not None:
- return val.decode(encoding)
- else:
- return val
+ return val if encoding is None else val.decode(encoding)
def get_bool(self, key):
cdef string k = self.check_key(key)
@@ -106,8 +95,7 @@ cdef class Params:
with nogil:
self.p.remove(k)
-
-def put_nonblocking(key, val, d=None):
+def put_nonblocking(key, val, d=""):
def f(key, val):
params = Params(d)
cdef string k = ensure_bytes(key)
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 481240f99..1215909fc 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -81,6 +81,9 @@ model_frame_from_road_frame = np.dot(model_intrinsics,
bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
get_view_frame_from_road_frame(0, 0, 0, model_height))
+bigmodel_frame_from_calib_frame = np.dot(bigmodel_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 9766a7264..30f3131c7 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -1,5 +1,6 @@
-Supported Cars
-------
+# Supported Cars
+
+## comma.ai supported cars
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
@@ -23,11 +24,11 @@ Supported Cars
| Honda | Insight 2019-21 | All | Stock | 0mph | 3mph |
| Honda | Inspire 2018 | All | Stock | 0mph | 3mph |
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph1 | 0mph |
-| Honda | Passport 2019 | All | openpilot | 25mph1 | 12mph |
+| Honda | Passport 2019-21 | All | openpilot | 25mph1 | 12mph |
| Honda | Pilot 2016-21 | Honda Sensing | openpilot | 25mph1 | 12mph |
| Honda | Ridgeline 2017-21 | Honda Sensing | openpilot | 25mph1 | 12mph |
| Hyundai | Palisade 2020-21 | All | Stock | 0mph | 0mph |
-| Hyundai | Sonata 2020-21 | All | Stock | 0mph | 0mph |
+| Hyundai | Sonata 2020-22 | All | Stock | 0mph | 0mph |
| Lexus | CT Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph |
| Lexus | ES 2019-21 | All | openpilot | 0mph | 0mph |
| Lexus | ES Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph |
@@ -45,7 +46,7 @@ Supported Cars
| Toyota | Avalon 2016-21 | TSS-P | Stock3| 20mph1 | 0mph |
| Toyota | Avalon Hybrid 2019-21 | TSS-P | Stock3| 20mph1 | 0mph |
| Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph |
-| Toyota | Camry 2021 | All | openpilot | 0mph4 | 0mph |
+| 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 |
@@ -55,14 +56,14 @@ Supported Cars
| Toyota | Corolla Hatchback 2019-22 | All | openpilot | 0mph | 0mph |
| Toyota | Corolla Hybrid 2020-22 | All | openpilot | 0mph | 0mph |
| Toyota | Highlander 2017-19 | All | Stock3| 0mph | 0mph |
-| Toyota | Highlander 2020-21 | All | openpilot | 0mph | 0mph |
+| Toyota | Highlander 2020-22 | All | openpilot | 0mph | 0mph |
| Toyota | Highlander Hybrid 2017-19 | All | Stock3| 0mph | 0mph |
-| Toyota | Highlander Hybrid 2020-21 | All | openpilot | 0mph | 0mph |
+| Toyota | Highlander Hybrid 2020-22 | All | openpilot | 0mph | 0mph |
| Toyota | Mirai 2021 | All | openpilot | 0mph | 0mph |
| Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph |
| Toyota | Prius 2021 | All | openpilot | 0mph | 0mph |
| Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph |
-| Toyota | Prius Prime 2021 | All | openpilot | 0mph | 0mph |
+| Toyota | Prius Prime 2021-22 | All | openpilot | 0mph | 0mph |
| Toyota | Rav4 2016-18 | TSS-P | Stock3| 20mph1 | 0mph |
| Toyota | Rav4 2019-21 | All | openpilot | 0mph | 0mph |
| Toyota | Rav4 Hybrid 2016-18 | TSS-P | Stock3| 0mph | 0mph |
@@ -74,8 +75,7 @@ Supported Cars
3When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
428mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-Community Maintained Cars and Features
-------
+## Community Maintained Cars and Features
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
@@ -85,6 +85,7 @@ Community Maintained Cars and Features
| 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 |
+| Cadillac | Escalade ESV 20161 | ACC + LKAS | openpilot | 0mph | 7mph |
| Chevrolet | Malibu 20171 | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chevrolet | Volt 2017-181 | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
@@ -92,6 +93,7 @@ Community Maintained Cars and Features
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph |
| Chrysler | Pacifica Hybrid 2019-21 | Adaptive Cruise | Stock | 0mph | 39mph |
| Genesis | G70 2018 | All | Stock | 0mph | 0mph |
+| Genesis | G70 2020 | All | Stock | 0mph | 0mph |
| Genesis | G80 2018 | All | Stock | 0mph | 0mph |
| Genesis | G90 2018 | All | Stock | 0mph | 0mph |
| GMC | Acadia 20181 | Adaptive Cruise | openpilot | 0mph | 7mph |
@@ -110,6 +112,7 @@ Community Maintained Cars and Features
| Hyundai | Kona Hybrid 2020 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph |
| Hyundai | Santa Fe 2021-22 | All | Stock | 0mph | 0mph |
+| Hyundai | Santa Fe Hybrid 2022 | All | Stock | 0mph | 0mph |
| Hyundai | Sonata 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph |
| Hyundai | Sonata Hybrid 2021-22 | All | Stock | 0mph | 0mph |
| Hyundai | Veloster 2019-20 | SCC + LKAS | Stock | 5mph | 0mph |
@@ -127,6 +130,7 @@ Community Maintained Cars and Features
| Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Telluride 2020 | SCC + LKAS | Stock | 0mph | 0mph |
+| Mazda | CX-9 2021 | All | Stock | 0mph | 28mph |
| Nissan | Altima 2019-20 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Leaf 2018-22 | ProPILOT | Stock | 0mph | 0mph |
| Nissan | Rogue 2018-20 | ProPILOT | Stock | 0mph | 0mph |
@@ -158,7 +162,9 @@ Community Maintained Cars and Features
| Volkswagen| Jetta 2018-20 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Jetta GLI 2021 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Passat 2016-183 | Driver Assistance | Stock | 0mph | 0mph |
+| Volkswagen| Polo 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| T-Cross 20214 | Driver Assistance | Stock | 0mph | 0mph |
+| Volkswagen| T-Roc 20214 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Taos 20224 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Tiguan 2020 | Driver Assistance | Stock | 0mph | 0mph |
| Volkswagen| Touran 2017 | Driver Assistance | Stock | 0mph | 0mph |
@@ -169,7 +175,7 @@ Community Maintained Cars and Features
4Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering,
remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). For the newer design,
in the interim, choose "VW J533 Development" from the vehicle drop-down for a harness that integrates at the CAN gateway inside the dashboard.
-Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`.
+Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them.
To promote a car from community maintained, it must meet a few requirements. We must own one from the brand, we must sell the harness for it, has full ISO26262 in both panda and openpilot, there must be a path forward for longitudinal control, it must have AEB still enabled, and it must support fingerprinting 2.0
diff --git a/docs/INTEGRATION.md b/docs/INTEGRATION.md
index a6b8194aa..97b72e39d 100644
--- a/docs/INTEGRATION.md
+++ b/docs/INTEGRATION.md
@@ -1,5 +1,4 @@
-Integration with Stock Features
-------
+# Integration with Stock Features
In all supported cars:
* Stock Lane Keep Assist (LKA) and stock ALC are replaced by openpilot ALC, which only functions when openpilot is engaged by the user.
@@ -10,64 +9,3 @@ Additionally, on specific supported cars (see ACC column in [supported cars](CAR
* openpilot FCW operates in addition to stock FCW.
openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning.
-
-Limitations of openpilot ALC and LDW
-------
-
-openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times.
-
-While changing lanes, openpilot is not capable of looking next to you or checking your blind spot. Only nudge the wheel to initiate a lane change after you have confirmed it's safe to do so.
-
-Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to:
-
-* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
-* The road facing camera is obstructed, covered or damaged by mud, ice, snow, etc.
-* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
-* The device is mounted incorrectly.
-* When in sharp curves, like on-off ramps, intersections etc...; openpilot is designed to be limited in the amount of steering torque it can produce.
-* In the presence of restricted lanes or construction zones.
-* When driving on highly banked roads or in presence of strong cross-wind.
-* Extremely hot or cold temperatures.
-* Bright light (due to oncoming headlights, direct sunlight, etc.).
-* Driving on hills, narrow, or winding roads.
-
-The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
-
-Limitations of openpilot ACC and FCW
-------
-
-openpilot ACC and openpilot FCW are not systems that allow careless or inattentive driving. It is still necessary for the driver to pay close attention to the vehicle’s surroundings and to be ready to re-take control of the gas and the brake at all times.
-
-Many factors can impact the performance of openpilot ACC and openpilot FCW, causing them to be unable to function as intended. These include, but are not limited to:
-
-* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
-* The road facing camera or radar are obstructed, covered, or damaged by mud, ice, snow, etc.
-* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
-* The device is mounted incorrectly.
-* Approaching a toll booth, a bridge or a large metal plate.
-* When driving on roads with pedestrians, cyclists, etc...
-* In presence of traffic signs or stop lights, which are not detected by openpilot at this time.
-* When the posted speed limit is below the user selected set speed. openpilot does not detect speed limits at this time.
-* In presence of vehicles in the same lane that are not moving.
-* When abrupt braking maneuvers are required. openpilot is designed to be limited in the amount of deceleration and acceleration that it can produce.
-* When surrounding vehicles perform close cut-ins from neighbor lanes.
-* Driving on hills, narrow, or winding roads.
-* Extremely hot or cold temperatures.
-* Bright light (due to oncoming headlights, direct sunlight, etc.).
-* Interference from other equipment that generates radar waves.
-
-The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
-
-Limitations of openpilot DM
-------
-
-openpilot DM should not be considered an exact measurement of the alertness of the driver.
-
-Many factors can impact the performance of openpilot DM, causing it to be unable to function as intended. These include, but are not limited to:
-
-* Low light conditions, such as driving at night or in dark tunnels.
-* Bright light (due to oncoming headlights, direct sunlight, etc.).
-* The driver's face is partially or completely outside field of view of the driver facing camera.
-* The driver facing camera is obstructed, covered, or damaged.
-
-The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention.
diff --git a/docs/LIMITATIONS.md b/docs/LIMITATIONS.md
new file mode 100644
index 000000000..8c112659c
--- /dev/null
+++ b/docs/LIMITATIONS.md
@@ -0,0 +1,58 @@
+# Limitations
+## Limitations of openpilot ALC and LDW
+
+openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times.
+
+While changing lanes, openpilot is not capable of looking next to you or checking your blind spot. Only nudge the wheel to initiate a lane change after you have confirmed it's safe to do so.
+
+Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to:
+
+* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
+* The road facing camera is obstructed, covered or damaged by mud, ice, snow, etc.
+* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
+* The device is mounted incorrectly.
+* When in sharp curves, like on-off ramps, intersections etc...; openpilot is designed to be limited in the amount of steering torque it can produce.
+* In the presence of restricted lanes or construction zones.
+* When driving on highly banked roads or in presence of strong cross-wind.
+* Extremely hot or cold temperatures.
+* Bright light (due to oncoming headlights, direct sunlight, etc.).
+* Driving on hills, narrow, or winding roads.
+
+The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
+
+## Limitations of openpilot ACC and FCW
+
+openpilot ACC and openpilot FCW are not systems that allow careless or inattentive driving. It is still necessary for the driver to pay close attention to the vehicle’s surroundings and to be ready to re-take control of the gas and the brake at all times.
+
+Many factors can impact the performance of openpilot ACC and openpilot FCW, causing them to be unable to function as intended. These include, but are not limited to:
+
+* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
+* The road facing camera or radar are obstructed, covered, or damaged by mud, ice, snow, etc.
+* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
+* The device is mounted incorrectly.
+* Approaching a toll booth, a bridge or a large metal plate.
+* When driving on roads with pedestrians, cyclists, etc...
+* In presence of traffic signs or stop lights, which are not detected by openpilot at this time.
+* When the posted speed limit is below the user selected set speed. openpilot does not detect speed limits at this time.
+* In presence of vehicles in the same lane that are not moving.
+* When abrupt braking maneuvers are required. openpilot is designed to be limited in the amount of deceleration and acceleration that it can produce.
+* When surrounding vehicles perform close cut-ins from neighbor lanes.
+* Driving on hills, narrow, or winding roads.
+* Extremely hot or cold temperatures.
+* Bright light (due to oncoming headlights, direct sunlight, etc.).
+* Interference from other equipment that generates radar waves.
+
+The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
+
+## Limitations of openpilot DM
+
+openpilot DM should not be considered an exact measurement of the alertness of the driver.
+
+Many factors can impact the performance of openpilot DM, causing it to be unable to function as intended. These include, but are not limited to:
+
+* Low light conditions, such as driving at night or in dark tunnels.
+* Bright light (due to oncoming headlights, direct sunlight, etc.).
+* The driver's face is partially or completely outside field of view of the driver facing camera.
+* The driver facing camera is obstructed, covered, or damaged.
+
+The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention.
diff --git a/docs/SAFETY.md b/docs/SAFETY.md
deleted file mode 100644
index d36d53437..000000000
--- a/docs/SAFETY.md
+++ /dev/null
@@ -1,34 +0,0 @@
-openpilot Safety
-======
-
-openpilot is an Adaptive Cruise Control (ACC) and Automated Lane Centering (ALC) system.
-Like other ACC and ALC systems, openpilot is a failsafe passive system and it requires the
-driver to be alert and to pay attention at all times.
-
-In order to enforce driver alertness, openpilot includes a driver monitoring feature
-that alerts the driver when distracted.
-
-However, even with an attentive driver, we must make further efforts for the system to be
-safe. We repeat, **driver alertness is necessary, but not sufficient, for openpilot to be
-used safely** and openpilot is provided with no warranty of fitness for any purpose.
-
-openpilot is developed in good faith to be compliant with FMVSS requirements and to follow
-industry standards of safety for Level 2 Driver Assistance Systems. In particular, we observe
-ISO26262 guidelines, including those from [pertinent documents](https://www.nhtsa.gov/sites/nhtsa.dot.gov/files/documents/13498a_812_573_alcsystemreport.pdf)
-released by NHTSA. In addition, we impose strict coding guidelines (like [MISRA C : 2012](https://www.misra.org.uk/what-is-misra/))
-on parts of openpilot that are safety relevant. We also perform software-in-the-loop,
-hardware-in-the-loop and in-vehicle tests before each software release.
-
-Following Hazard and Risk Analysis and FMEA, at a very high level, we have designed openpilot
-ensuring two main safety requirements.
-
-1. The driver must always be capable to immediately retake manual control of the vehicle,
- by stepping on either pedal or by pressing the cancel button.
-2. The vehicle must not alter its trajectory too quickly for the driver to safely
- react. This means that while the system is engaged, the actuators are constrained
- to operate within reasonable limits.
-
-For additional safety implementation details, refer to [panda safety model](https://github.com/commaai/panda#safety-model). For vehicle specific implementation of the safety concept, refer to [panda/board/safety/](https://github.com/commaai/panda/tree/master/board/safety).
-
-**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
- not fully meeting the above requirements.
diff --git a/opendbc/can/common.cc b/opendbc/can/common.cc
index 0d9d23b5c..2a089980c 100644
--- a/opendbc/can/common.cc
+++ b/opendbc/can/common.cc
@@ -144,6 +144,9 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
case 0x12B: // GRA_ACC_01 Steering wheel controls for ACC
crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter];
break;
+ case 0x12E: // ACC_07 Automatic Cruise Control
+ crc ^= (uint8_t[]){0xF8,0xE5,0x97,0xC9,0xD6,0x07,0x47,0x21,0x66,0xDD,0xCF,0x6F,0xA1,0x94,0x74,0x63}[counter];
+ break;
case 0x187: // EV_Gearshift "Gear" selection data for EVs with no gearbox
crc ^= (uint8_t[]){0x7F,0xED,0x17,0xC2,0x7C,0xEB,0x44,0x21,0x01,0xFA,0xDB,0x15,0x4A,0x6B,0x23,0x05}[counter];
break;
@@ -172,7 +175,6 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR
}
-
unsigned int pedal_checksum(uint64_t d, int l) {
uint8_t crc = 0xFF;
uint8_t poly = 0xD5; // standard crc8
diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc
index 3b225c73e..883f2ee0a 100644
--- a/opendbc/lexus_ct200h_2018_pt_generated.dbc
+++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc
index 51c584df6..30e8cbbaf 100644
--- a/opendbc/lexus_is_2018_pt_generated.dbc
+++ b/opendbc/lexus_is_2018_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/lexus_nx300_2018_pt_generated.dbc b/opendbc/lexus_nx300_2018_pt_generated.dbc
index f00cd2c65..7b4fcaa13 100644
--- a/opendbc/lexus_nx300_2018_pt_generated.dbc
+++ b/opendbc/lexus_nx300_2018_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc
index ddb4ad6de..c7cb4616d 100644
--- a/opendbc/lexus_nx300h_2018_pt_generated.dbc
+++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc
index 4c316fee7..7a1b1a704 100644
--- a/opendbc/lexus_rx_350_2016_pt_generated.dbc
+++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
index c1d67f5c9..b481318a1 100644
--- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
+++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/nissan_leaf_2018.dbc b/opendbc/nissan_leaf_2018.dbc
index 5172e01c8..6483d9cbf 100644
--- a/opendbc/nissan_leaf_2018.dbc
+++ b/opendbc/nissan_leaf_2018.dbc
@@ -83,7 +83,7 @@ BO_ 569 CRUISE_THROTTLE: 8 XXX
SG_ FOLLOW_DISTANCE_BUTTON : 26|1@0+ (1,0) [0|1] "" XXX
SG_ CANCEL_BUTTON : 25|1@0+ (1,0) [0|1] "" XXX
SG_ PROPILOT_BUTTON : 24|1@0+ (1,0) [0|1] "" XXX
- SG_ unsure4 : 39|6@0+ (1,0) [0|63] "" XXX
+ SG_ USER_BRAKE_PRESSED : 37|1@0+ (1,0) [0|1] "" XXX
SG_ COUNTER : 32|2@1+ (1,0) [0|3] "" XXX
SG_ unsure5 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ unsure6 : 55|8@0+ (1,0) [0|255] "" XXX
diff --git a/opendbc/tesla_powertrain.dbc b/opendbc/tesla_powertrain.dbc
new file mode 100644
index 000000000..8572ee20f
--- /dev/null
+++ b/opendbc/tesla_powertrain.dbc
@@ -0,0 +1,179 @@
+VERSION ""
+
+
+NS_ :
+ NS_DESC_
+ CM_
+ BA_DEF_
+ BA_
+ VAL_
+ CAT_DEF_
+ CAT_
+ FILTER
+ BA_DEF_DEF_
+ EV_DATA_
+ ENVVAR_DATA_
+ SGTYPE_
+ SGTYPE_VAL_
+ BA_DEF_SGTYPE_
+ BA_SGTYPE_
+ SIG_TYPE_REF_
+ VAL_TABLE_
+ SIG_GROUP_
+ SIG_VALTYPE_
+ SIGTYPE_VALTYPE_
+ BO_TX_BU_
+ BA_DEF_REL_
+ BA_REL_
+ BA_DEF_DEF_REL_
+ BU_SG_REL_
+ BU_EV_REL_
+ BU_BO_REL_
+ SG_MUL_VAL_
+
+BS_:
+
+BU_:
+ NEO
+ MCU
+ GTW
+ EPAS
+ DI
+ ESP
+ SBW
+ STW
+ APP
+ DAS
+ XXX
+
+BO_ 262 DI_torque1: 8 DI
+ SG_ DI_torqueDriver : 0|13@1- (0.25,0) [-750|750] "Nm" NEO
+ SG_ DI_torque1Counter : 13|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_torqueMotor : 16|13@1- (0.25,0) [-750|750] "Nm" NEO
+ SG_ DI_soptState : 29|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_motorRPM : 32|16@1- (1,0) [-17000|17000] "RPM" NEO
+ SG_ DI_pedalPos : 48|8@1+ (0.4,0) [0|100] "%" NEO
+ SG_ DI_torque1Checksum : 56|8@1+ (1,0) [0|0] "" NEO
+
+BO_ 278 DI_torque2: 6 DI
+ SG_ DI_torqueEstimate : 0|12@1- (0.5,0) [-750|750] "Nm" NEO
+ SG_ DI_gear : 12|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_brakePedal : 15|1@1+ (1,0) [0|0] "" NEO
+ SG_ DI_vehicleSpeed : 16|12@1+ (0.05,-25) [-25|179.75] "MPH" NEO
+ SG_ DI_gearRequest : 28|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_torqueInterfaceFailure : 31|1@1+ (1,0) [0|0] "" NEO
+ SG_ DI_torque2Counter : 32|4@1+ (1,0) [0|0] "" NEO
+ SG_ DI_brakePedalState : 36|2@1+ (1,0) [0|0] "" NEO
+ SG_ DI_epbParkRequest : 38|1@1+ (1,0) [0|0] "" NEO
+ SG_ DI_epbInterfaceReady : 39|1@1+ (1,0) [0|0] "" NEO
+ SG_ DI_torque2Checksum : 40|8@1+ (1,0) [0|0] "" NEO
+
+BO_ 504 BrakeMessage: 8 XXX
+ SG_ driverBrakeStatus : 2|2@1+ (1,0) [0|3] "" XXX
+
+BO_ 568 STW_ACTN_RQ: 8 STW
+ SG_ SpdCtrlLvr_Stat : 0|6@1+ (1,0) [0|0] "" NEO
+ SG_ VSL_Enbl_Rq : 6|1@1+ (1,0) [0|0] "" NEO
+ SG_ SpdCtrlLvrStat_Inv : 7|1@1+ (1,0) [0|0] "" NEO
+ SG_ DTR_Dist_Rq : 8|8@1+ (1,0) [0|200] "" NEO
+ SG_ TurnIndLvr_Stat : 16|2@1+ (1,0) [0|0] "" NEO
+ SG_ HiBmLvr_Stat : 18|2@1+ (1,0) [0|0] "" NEO
+ SG_ WprWashSw_Psd : 20|2@1+ (1,0) [0|0] "" NEO
+ SG_ WprWash_R_Sw_Posn_V2 : 22|2@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Lvr_Stat : 24|3@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Cond_Flt : 27|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Cond_Psd : 28|2@1+ (1,0) [0|0] "" NEO
+ SG_ HrnSw_Psd : 30|2@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw00_Psd : 32|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw01_Psd : 33|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw02_Psd : 34|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw03_Psd : 35|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw04_Psd : 36|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw05_Psd : 37|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw06_Psd : 38|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw07_Psd : 39|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw08_Psd : 40|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw09_Psd : 41|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw10_Psd : 42|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw11_Psd : 43|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw12_Psd : 44|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw13_Psd : 45|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw14_Psd : 46|1@1+ (1,0) [0|0] "" NEO
+ SG_ StW_Sw15_Psd : 47|1@1+ (1,0) [0|0] "" NEO
+ SG_ WprSw6Posn : 48|3@1+ (1,0) [0|0] "" NEO
+ SG_ MC_STW_ACTN_RQ : 52|4@1+ (1,0) [0|15] "" NEO
+ SG_ CRC_STW_ACTN_RQ : 56|8@1+ (1,0) [0|0] "" NEO
+
+BO_ 598 DI_state: 8 DI
+ SG_ DI_systemState : 0|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_vehicleHoldState : 3|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_proximity : 6|1@1+ (1,0) [0|0] "" NEO
+ SG_ DI_driveReady : 7|1@1+ (1,0) [0|0] "" NEO
+ SG_ DI_regenLight : 8|1@1+ (1,0) [0|0] "" NEO
+ SG_ DI_state : 9|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_cruiseState : 12|4@1+ (1,0) [0|0] "" NEO
+ SG_ DI_analogSpeed : 16|12@1+ (0.1,0) [0|150] "speed" NEO
+ SG_ DI_immobilizerState : 28|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_speedUnits : 31|1@1+ (1,0) [0|1] "" NEO
+ SG_ DI_cruiseSet : 32|9@1+ (0.5,0) [0|255.5] "speed" NEO
+ SG_ DI_aebState : 41|3@1+ (1,0) [0|0] "" NEO
+ SG_ DI_stateCounter : 44|4@1+ (1,0) [0|0] "" NEO
+ SG_ DI_digitalSpeed : 48|8@1+ (1,0) [0|250] "" NEO
+ SG_ DI_stateChecksum : 56|8@1+ (1,0) [0|0] "" NEO
+
+BO_ 703 DAS_control: 8 GTW
+ SG_ DAS_setSpeed : 0|12@1+ (0.1,0) [0|409.4] "kph" DI,PM,APS
+ SG_ DAS_accState : 12|4@1+ (1,0) [0|0] "" DI,PM,APS
+ SG_ DAS_aebEvent : 16|2@1+ (1,0) [0|3] "" DI,PM,APS
+ SG_ DAS_jerkMin : 18|9@1+ (0.03,-15.232) [-15.232|0.098] "m/s^3" DI,PM,APS
+ SG_ DAS_jerkMax : 27|8@1+ (0.059,0) [0|15.045] "m/s^3" DI,PM,APS
+ SG_ DAS_accelMin : 35|9@1+ (0.04,-15) [-15|5.44] "m/s^2" DI,PM,APS
+ SG_ DAS_accelMax : 44|9@1+ (0.04,-15) [-15|5.44] "m/s^2" DI,PM,APS
+ SG_ DAS_controlCounter : 53|3@1+ (1,0) [0|0] "" DI,PM,APS
+ SG_ DAS_controlChecksum : 56|8@1+ (1,0) [0|0] "" DI,PM,APS
+
+VAL_ 262 DI_torqueDriver -4096 "SNA" ;
+VAL_ 262 DI_torqueMotor -4096 "SNA" ;
+VAL_ 262 DI_soptState 7 "SOPT_TEST_SNA" 4 "SOPT_TEST_NOT_RUN" 3 "SOPT_TEST_PASSED" 2 "SOPT_TEST_FAILED" 1 "SOPT_TEST_IN_PROGRESS" 0 "SOPT_PRE_TEST" ;
+VAL_ 262 DI_motorRPM -32768 "SNA" ;
+VAL_ 262 DI_pedalPos 255 "SNA" ;
+VAL_ 278 DI_torqueEstimate -2048 "SNA" ;
+VAL_ 278 DI_gear 7 "DI_GEAR_SNA" 4 "DI_GEAR_D" 3 "DI_GEAR_N" 2 "DI_GEAR_R" 1 "DI_GEAR_P" 0 "DI_GEAR_INVALID" ;
+VAL_ 278 DI_brakePedal 1 "Applied" 0 "Not_applied" ;
+VAL_ 278 DI_vehicleSpeed 4095 "SNA" ;
+VAL_ 278 DI_gearRequest 7 "DI_GEAR_SNA" 4 "DI_GEAR_D" 3 "DI_GEAR_N" 2 "DI_GEAR_R" 1 "DI_GEAR_P" 0 "DI_GEAR_INVALID" ;
+VAL_ 278 DI_torqueInterfaceFailure 1 "TORQUE_INTERFACE_FAILED" 0 "TORQUE_INTERFACE_NORMAL" ;
+VAL_ 278 DI_brakePedalState 3 "SNA" 2 "INVALID" 1 "ON" 0 "OFF" ;
+VAL_ 278 DI_epbParkRequest 1 "Park_requested" 0 "No_request" ;
+VAL_ 278 DI_epbInterfaceReady 1 "EPB_INTERFACE_READY" 0 "EPB_INTERFACE_NOT_READY" ;
+VAL_ 504 driverBrakeStatus 2 "APPLIED" 1 "NOT_APPLIED" ;
+VAL_ 568 SpdCtrlLvr_Stat 32 "DN_1ST" 16 "UP_1ST" 8 "DN_2ND" 4 "UP_2ND" 2 "RWD" 1 "FWD" 0 "IDLE" ;
+VAL_ 568 DTR_Dist_Rq 255 "SNA" 200 "ACC_DIST_7" 166 "ACC_DIST_6" 133 "ACC_DIST_5" 100 "ACC_DIST_4" 66 "ACC_DIST_3" 33 "ACC_DIST_2" 0 "ACC_DIST_1" ;
+VAL_ 568 TurnIndLvr_Stat 3 "SNA" 2 "RIGHT" 1 "LEFT" 0 "IDLE" ;
+VAL_ 568 HiBmLvr_Stat 3 "SNA" 2 "HIBM_FLSH_ON_PSD" 1 "HIBM_ON_PSD" 0 "IDLE" ;
+VAL_ 568 WprWashSw_Psd 3 "SNA" 2 "WASH" 1 "TIPWIPE" 0 "NPSD" ;
+VAL_ 568 WprWash_R_Sw_Posn_V2 3 "SNA" 2 "WASH" 1 "INTERVAL" 0 "OFF" ;
+VAL_ 568 StW_Lvr_Stat 4 "STW_BACK" 3 "STW_FWD" 2 "STW_DOWN" 1 "STW_UP" 0 "NPSD" ;
+VAL_ 568 StW_Cond_Psd 3 "SNA" 2 "DOWN" 1 "UP" 0 "NPSD" ;
+VAL_ 568 HrnSw_Psd 3 "SNA" 2 "NDEF2" 1 "PSD" 0 "NPSD" ;
+VAL_ 568 StW_Sw00_Psd 1 "PRESSED" 0 "NOT_PRESSED_SNA" ;
+VAL_ 568 StW_Sw01_Psd 1 "PRESSED" 0 "NOT_PRESSED_SNA" ;
+VAL_ 568 StW_Sw03_Psd 1 "PRESSED" 0 "NOT_PRESSED_SNA" ;
+VAL_ 568 StW_Sw04_Psd 1 "PRESSED" 0 "NOT_PRESSED_SNA" ;
+VAL_ 568 WprSw6Posn 7 "SNA" 6 "STAGE2" 5 "STAGE1" 4 "INTERVAL4" 3 "INTERVAL3" 2 "INTERVAL2" 1 "INTERVAL1" 0 "OFF" ;
+VAL_ 598 DI_aebState 2 "ENABLED" 4 "FAULT" 7 "SNA" 1 "STANDBY" 3 "STANDSTILL" 0 "UNAVAILABLE" ;
+VAL_ 598 DI_analogSpeed 4095 "SNA" ;
+VAL_ 598 DI_cruiseState 2 "ENABLED" 5 "FAULT" 0 "OFF" 4 "OVERRIDE" 7 "PRE_CANCEL" 6 "PRE_FAULT" 1 "STANDBY" 3 "STANDSTILL" ;
+VAL_ 598 DI_digitalSpeed 255 "SNA" ;
+VAL_ 598 DI_immobilizerState 2 "AUTHENTICATING" 3 "DISARMED" 6 "FAULT" 4 "IDLE" 0 "INIT_SNA" 1 "REQUEST" 5 "RESET" ;
+VAL_ 598 DI_speedUnits 1 "KPH" 0 "MPH" ;
+VAL_ 598 DI_state 3 "ABORT" 4 "ENABLE" 2 "FAULT" 1 "STANDBY" 0 "UNAVAILABLE" ;
+VAL_ 598 DI_systemState 3 "ABORT" 4 "ENABLE" 2 "FAULT" 1 "STANDBY" 0 "UNAVAILABLE" ;
+VAL_ 598 DI_vehicleHoldState 2 "BLEND_IN" 4 "BLEND_OUT" 6 "FAULT" 7 "INIT" 5 "PARK" 1 "STANDBY" 3 "STANDSTILL" 0 "UNAVAILABLE" ;
+VAL_ 703 DAS_setSpeed 4095 "SNA" ;
+VAL_ 703 DAS_accState 15 "FAULT_SNA" 13 "ACC_CANCEL_GENERIC_SILENT" 11 "APC_SELFPARK_START" 10 "APC_UNPARK_COMPLETE" 9 "APC_PAUSE" 8 "APC_ABORT" 7 "APC_COMPLETE" 6 "APC_FORWARD" 5 "APC_BACKWARD" 4 "ACC_ON" 3 "ACC_HOLD" 0 "ACC_CANCEL_GENERIC" ;
+VAL_ 703 DAS_aebEvent 3 "AEB_SNA" 2 "AEB_FAULT" 1 "AEB_ACTIVE" 0 "AEB_NOT_ACTIVE" ;
+VAL_ 703 DAS_jerkMin 511 "SNA" ;
+VAL_ 703 DAS_jerkMax 255 "SNA" ;
+VAL_ 703 DAS_accelMin 511 "SNA" ;
+VAL_ 703 DAS_accelMax 511 "SNA" ;
\ No newline at end of file
diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc
index 25753613d..8043d75ae 100644
--- a/opendbc/toyota_avalon_2017_pt_generated.dbc
+++ b/opendbc/toyota_avalon_2017_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
index ec35feef2..72ef797a3 100644
--- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc
index db13a143a..9ff7b5852 100644
--- a/opendbc/toyota_corolla_2017_pt_generated.dbc
+++ b/opendbc/toyota_corolla_2017_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc
index ca7c16b1e..7c279f0da 100644
--- a/opendbc/toyota_highlander_2017_pt_generated.dbc
+++ b/opendbc/toyota_highlander_2017_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
index 843d1e2ce..a2b5da03a 100644
--- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
index 0f94c584c..a6d4dc6d7 100644
--- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc
index 10cf508b7..c84fa415f 100644
--- a/opendbc/toyota_nodsu_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc
index 1d29dbf81..af9603c7d 100644
--- a/opendbc/toyota_prius_2017_pt_generated.dbc
+++ b/opendbc/toyota_prius_2017_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc
index 7a2f0bd4c..41021cce3 100644
--- a/opendbc/toyota_rav4_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_2017_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
index 61ff80a6d..9dba21a70 100644
--- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
index ac7660643..05e282b07 100644
--- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
+++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
@@ -128,15 +128,18 @@ BO_ 742 LEAD_INFO: 8 DSU
BO_ 835 ACC_CONTROL: 8 DSU
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX
+ SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX
+ SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX
SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+ SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
@@ -160,6 +163,8 @@ BO_ 951 ESP_CONTROL: 8 ESP
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
@@ -288,7 +293,7 @@ BO_ 1163 RSA3: 8 FCM
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
-
+
BO_ 1571 CENTRAL_GATEWAY_UNIT: 8 CGW
SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX
SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX
@@ -311,6 +316,11 @@ CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
+CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD";
+CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1";
+CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1";
+CM_ SG_ 835 DISTANCE "Display Distance Bars on HUD Permanently";
+CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars";
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
@@ -341,6 +351,9 @@ VAL_ 614 STATE 3 "enabled" 1 "disabled";
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
+VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok";
+VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear";
+VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press";
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
diff --git a/opendbc/vw_mqb_2010.dbc b/opendbc/vw_mqb_2010.dbc
index 63477a1cb..3a032c056 100644
--- a/opendbc/vw_mqb_2010.dbc
+++ b/opendbc/vw_mqb_2010.dbc
@@ -1253,8 +1253,19 @@ 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_ ACC_07_BZ : 8|4@1+ (1,0) [0|15] "" XXX
- SG_ ACC_07_CRC : 0|8@1+ (1,0) [0|255] "" 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
BO_ 264 Fahrwerk_01: 8 XXX
SG_ Fahrwerk_01_BZ : 8|4@1+ (1,0) [0|15] "" XXX
@@ -1372,6 +1383,13 @@ CM_ SG_ 294 254 "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_ 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";
diff --git a/panda/__init__.py b/panda/__init__.py
index 81ee2f013..a9bf38f48 100644
--- a/panda/__init__.py
+++ b/panda/__init__.py
@@ -1,8 +1,8 @@
# flake8: noqa
# pylint: skip-file
from .python import Panda, PandaWifiStreaming, PandaDFU, flash_release, \
- BASEDIR, ensure_st_up_to_date, PandaSerial, \
- DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, MCU_TYPE_F4
+ BASEDIR, ensure_st_up_to_date, PandaSerial, pack_can_buffer, unpack_can_buffer, \
+ DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, MCU_TYPE_F4, DLC_TO_LEN, LEN_TO_DLC
from .python.config import BOOTSTUB_ADDRESS, BLOCK_SIZE_FX, APP_ADDRESS_FX, \
BLOCK_SIZE_H7, APP_ADDRESS_H7, DEVICE_SERIAL_NUMBER_ADDR_H7, \
diff --git a/panda/board/can_definitions.h b/panda/board/can_definitions.h
new file mode 100644
index 000000000..552cf3ff9
--- /dev/null
+++ b/panda/board/can_definitions.h
@@ -0,0 +1,29 @@
+#include "dlc_to_len.h"
+
+#define CAN_PACKET_VERSION 2
+typedef struct {
+ unsigned char reserved : 1;
+ unsigned char bus : 3;
+ unsigned char data_len_code : 4;
+ unsigned char rejected : 1;
+ unsigned char returned : 1;
+ unsigned char extended : 1;
+ unsigned int addr : 29;
+ unsigned char data[CANPACKET_DATA_SIZE_MAX];
+} __attribute__((packed, aligned(4))) CANPacket_t;
+
+#define GET_BUS(msg) ((msg)->bus)
+#define GET_LEN(msg) (dlc_to_len[(msg)->data_len_code])
+#define GET_ADDR(msg) ((msg)->addr)
+
+// Flasher and pedal use raw mailbox access
+#define GET_MAILBOX_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
+#define GET_MAILBOX_BYTES_04(msg) ((msg)->RDLR)
+#define GET_MAILBOX_BYTES_48(msg) ((msg)->RDHR)
+
+#define CAN_INIT_TIMEOUT_MS 500U
+
+#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))
diff --git a/panda/board/config.h b/panda/board/config.h
index d62bcb95a..311a0a2e1 100644
--- a/panda/board/config.h
+++ b/panda/board/config.h
@@ -34,16 +34,6 @@
#define MAX_RESP_LEN 0x40U
-#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
-#define GET_LEN(msg) ((msg)->RDTR & 0xF)
-#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
-#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
-#define GET_BYTES_04(msg) ((msg)->RDLR)
-#define GET_BYTES_48(msg) ((msg)->RDHR)
-#define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask))
-
-#define CAN_INIT_TIMEOUT_MS 500U
-
#include
#ifdef STM32H7
#include "stm32h7/stm32h7_config.h"
diff --git a/panda/board/dlc_to_len.h b/panda/board/dlc_to_len.h
new file mode 100644
index 000000000..82d743ffb
--- /dev/null
+++ b/panda/board/dlc_to_len.h
@@ -0,0 +1 @@
+unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U};
diff --git a/panda/board/drivers/bxcan.h b/panda/board/drivers/bxcan.h
index 7828925bd..1d1745cd3 100644
--- a/panda/board/drivers/bxcan.h
+++ b/panda/board/drivers/bxcan.h
@@ -9,7 +9,7 @@ bool can_set_speed(uint8_t can_number) {
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
- ret &= llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number));
+ ret &= llcan_set_speed(CAN, bus_config[bus_number].can_speed, can_loopback, (unsigned int)(can_silent) & (1U << can_number));
return ret;
}
@@ -17,7 +17,7 @@ bool can_set_speed(uint8_t can_number) {
void can_set_gmlan(uint8_t bus) {
if(current_board->has_hw_gmlan){
// first, disable GMLAN on prev bus
- uint8_t prev_bus = can_num_lookup[3];
+ uint8_t prev_bus = bus_config[3].can_num_lookup;
if (bus != prev_bus) {
switch (prev_bus) {
case 1:
@@ -26,9 +26,9 @@ void can_set_gmlan(uint8_t bus) {
puth(prev_bus + 1U);
puts("\n");
current_board->set_can_mode(CAN_MODE_NORMAL);
- bus_lookup[prev_bus] = prev_bus;
- can_num_lookup[prev_bus] = prev_bus;
- can_num_lookup[3] = -1;
+ bus_config[prev_bus].bus_lookup = prev_bus;
+ bus_config[prev_bus].can_num_lookup = prev_bus;
+ bus_config[3].can_num_lookup = -1;
bool ret = can_init(prev_bus);
UNUSED(ret);
break;
@@ -46,9 +46,9 @@ void can_set_gmlan(uint8_t bus) {
puth(bus + 1U);
puts("\n");
current_board->set_can_mode((bus == 1U) ? CAN_MODE_GMLAN_CAN2 : CAN_MODE_GMLAN_CAN3);
- bus_lookup[bus] = 3;
- can_num_lookup[bus] = -1;
- can_num_lookup[3] = bus;
+ bus_config[bus].bus_lookup = 3;
+ bus_config[bus].can_num_lookup = -1;
+ bus_config[3].can_num_lookup = bus;
bool ret = can_init(bus);
UNUSED(ret);
break;
@@ -101,18 +101,23 @@ void process_can(uint8_t can_number) {
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
// check for empty mailbox
- CAN_FIFOMailBox_TypeDef to_send;
+ CANPacket_t to_send;
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
// add successfully transmitted message to my fifo
if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
can_txd_cnt += 1;
if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
- CAN_FIFOMailBox_TypeDef to_push;
- to_push.RIR = CAN->sTxMailBox[0].TIR;
- to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
- to_push.RDLR = CAN->sTxMailBox[0].TDLR;
- to_push.RDHR = CAN->sTxMailBox[0].TDHR;
+ CANPacket_t to_push;
+ 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.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);
+ WORD_TO_BYTE_ARRAY(&to_push.data[4], CAN->sTxMailBox[0].TDHR);
+
can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
}
@@ -136,10 +141,12 @@ 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].TDLR = to_send.RDLR;
- CAN->sTxMailBox[0].TDHR = to_send.RDHR;
- CAN->sTxMailBox[0].TDTR = to_send.RDTR;
- CAN->sTxMailBox[0].TIR = to_send.RIR;
+ CAN->sTxMailBox[0].TIR = ((to_send.extended != 0) ? (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]);
+ // Send request TXRQ
+ CAN->sTxMailBox[0].TIR |= 0x1U;
usb_cb_ep3_out_complete();
}
@@ -161,23 +168,29 @@ void can_rx(uint8_t can_number) {
pending_can_live = 1;
// add to my fifo
- CAN_FIFOMailBox_TypeDef to_push;
- to_push.RIR = CAN->sFIFOMailBox[0].RIR;
- to_push.RDTR = CAN->sFIFOMailBox[0].RDTR;
- to_push.RDLR = CAN->sFIFOMailBox[0].RDLR;
- to_push.RDHR = CAN->sFIFOMailBox[0].RDHR;
+ CANPacket_t to_push;
- // modify RDTR for our API
- to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
+ 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.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);
+ WORD_TO_BYTE_ARRAY(&to_push.data[4], CAN->sFIFOMailBox[0].RDHR);
// forwarding (panda only)
- int bus_fwd_num = (can_forwarding[bus_number] != -1) ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
+ int bus_fwd_num = safety_fwd_hook(bus_number, &to_push);
if (bus_fwd_num != -1) {
- CAN_FIFOMailBox_TypeDef to_send;
- to_send.RIR = to_push.RIR | 1; // TXRQ
- to_send.RDTR = to_push.RDTR;
- to_send.RDLR = to_push.RDLR;
- to_send.RDHR = to_push.RDHR;
+ CANPacket_t to_send;
+
+ to_send.returned = 0U;
+ to_send.rejected = 0U;
+ to_send.extended = to_push.extended; // TXRQ
+ to_send.addr = to_push.addr;
+ to_send.bus = to_push.bus;
+ to_send.data_len_code = to_push.data_len_code;
+ (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]);
can_send(&to_send, bus_fwd_num, true);
}
diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h
index cbce796d8..9ee7415a1 100644
--- a/panda/board/drivers/can_common.h
+++ b/panda/board/drivers/can_common.h
@@ -2,11 +2,19 @@ typedef struct {
volatile uint32_t w_ptr;
volatile uint32_t r_ptr;
uint32_t fifo_size;
- CAN_FIFOMailBox_TypeDef *elems;
+ CANPacket_t *elems;
} can_ring;
-#define CAN_BUS_RET_FLAG 0x80U
-#define CAN_BUS_NUM_MASK 0x7FU
+typedef struct {
+ uint8_t bus_lookup;
+ uint8_t can_num_lookup;
+ uint32_t can_speed;
+ uint32_t can_data_speed;
+ bool canfd_enabled;
+ bool brs_enabled;
+} bus_config_t;
+
+#define CAN_BUS_NUM_MASK 0x3FU
#define BUS_MAX 4U
@@ -21,8 +29,6 @@ extern int pending_can_live;
// must reinit after changing these
extern int can_loopback;
extern int can_silent;
-extern uint32_t can_speed[4];
-extern uint32_t can_data_speed[3];
// Ignition detected from CAN meessages
bool ignition_can = false;
@@ -43,14 +49,19 @@ void process_can(uint8_t can_number);
// ********************* instantiate queues *********************
#define can_buffer(x, size) \
- CAN_FIFOMailBox_TypeDef elems_##x[size]; \
- can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CAN_FIFOMailBox_TypeDef *)&(elems_##x) };
+ CANPacket_t elems_##x[size]; \
+ can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CANPacket_t *)&(elems_##x) };
+#ifdef STM32H7
+__attribute__((section(".ram_d1"))) can_buffer(rx_q, 0x1000)
+__attribute__((section(".ram_d1"))) can_buffer(txgmlan_q, 0x1A0)
+#else
can_buffer(rx_q, 0x1000)
-can_buffer(tx1_q, 0x100)
-can_buffer(tx2_q, 0x100)
-can_buffer(tx3_q, 0x100)
-can_buffer(txgmlan_q, 0x100)
+can_buffer(txgmlan_q, 0x1A0)
+#endif
+can_buffer(tx1_q, 0x1A0)
+can_buffer(tx2_q, 0x1A0)
+can_buffer(tx3_q, 0x1A0)
// FIXME:
// cppcheck-suppress misra-c2012-9.3
can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q};
@@ -63,7 +74,7 @@ int can_err_cnt = 0;
int can_overflow_cnt = 0;
// ********************* interrupt safe queue *********************
-bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
+bool can_pop(can_ring *q, CANPacket_t *elem) {
bool ret = 0;
ENTER_CRITICAL();
@@ -81,7 +92,7 @@ bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
return ret;
}
-bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
+bool can_push(can_ring *q, CANPacket_t *elem) {
bool ret = false;
uint32_t next_w_ptr;
@@ -150,20 +161,21 @@ void can_clear(can_ring *q) {
// can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc);
// bus_lookup: Translates from 'can number' to 'bus number'.
// can_num_lookup: Translates from 'bus number' to 'can number'.
-// can_forwarding: Given a bus num, lookup bus num to forward to. -1 means no forward.
// Helpers
// Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3
-uint8_t bus_lookup[] = {0,1,2};
-uint8_t can_num_lookup[] = {0,1,2,-1};
-int8_t can_forwarding[] = {-1,-1,-1,-1};
-uint32_t can_speed[] = {5000, 5000, 5000, 333};
-uint32_t can_data_speed[] = {5000, 5000, 5000}; //For CAN FD with BRS only
+bus_config_t bus_config[] = {
+ { .bus_lookup = 0U, .can_num_lookup = 0U, .can_speed = 5000U, .can_data_speed = 5000U, .canfd_enabled = false, .brs_enabled = false },
+ { .bus_lookup = 1U, .can_num_lookup = 1U, .can_speed = 5000U, .can_data_speed = 5000U, .canfd_enabled = false, .brs_enabled = false },
+ { .bus_lookup = 2U, .can_num_lookup = 2U, .can_speed = 5000U, .can_data_speed = 5000U, .canfd_enabled = false, .brs_enabled = false },
+ { .bus_lookup = 0xFFU, .can_num_lookup = 0xFFU, .can_speed = 333U, .can_data_speed = 333U, .canfd_enabled = false, .brs_enabled = false },
+};
+
#define CAN_MAX 3U
#define CANIF_FROM_CAN_NUM(num) (cans[num])
-#define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num])
-#define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num])
+#define BUS_NUM_FROM_CAN_NUM(num) (bus_config[num].bus_lookup)
+#define CAN_NUM_FROM_BUS_NUM(num) (bus_config[num].can_num_lookup)
void can_init_all(void) {
bool ret = true;
@@ -175,13 +187,13 @@ void can_init_all(void) {
}
void can_flip_buses(uint8_t bus1, uint8_t bus2){
- bus_lookup[bus1] = bus2;
- bus_lookup[bus2] = bus1;
- can_num_lookup[bus1] = bus2;
- can_num_lookup[bus2] = bus1;
+ bus_config[bus1].bus_lookup = bus2;
+ bus_config[bus2].bus_lookup = bus1;
+ bus_config[bus1].can_num_lookup = bus2;
+ bus_config[bus2].can_num_lookup = bus1;
}
-void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+void ignition_can_hook(CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
@@ -189,23 +201,30 @@ void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) {
ignition_can_cnt = 0U; // reset counter
if (bus == 0) {
+ // GM exception
// TODO: verify on all supported GM models that we can reliably detect ignition using only this signal,
// 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;
}
- // GM exception
if ((addr == 0x1F1) && (len == 8)) {
// Bit 5 is ignition "on"
bool ignition_gm = ((GET_BYTE(to_push, 0) & 0x20) != 0);
ignition_can = ignition_gm || ignition_cadillac;
}
+
// Tesla exception
if ((addr == 0x348) && (len == 8)) {
// GTW_status
ignition_can = (GET_BYTE(to_push, 0) & 0x1) != 0;
}
+
+ // Mazda exception
+ if ((addr == 0x9E) && (len == 8)) {
+ ignition_can = (GET_BYTE(to_push, 0) >> 4) == 0xDU;
+ }
+
}
}
@@ -217,22 +236,19 @@ bool can_tx_check_min_slots_free(uint32_t min) {
(can_slots_empty(&can_txgmlan_q) >= min);
}
-void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) {
+void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook) {
if (skip_tx_hook || safety_tx_hook(to_push) != 0) {
if (bus_number < BUS_MAX) {
// add CAN packet to send queue
- // bus number isn't passed through
- to_push->RDTR &= 0xF;
- if ((bus_number == 3U) && (can_num_lookup[3] == 0xFFU)) {
+ if ((bus_number == 3U) && (bus_config[3].can_num_lookup == 0xFFU)) {
gmlan_send_errs += bitbang_gmlan(to_push) ? 0U : 1U;
} else {
can_fwd_errs += can_push(can_queues[bus_number], to_push) ? 0U : 1U;
process_can(CAN_NUM_FROM_BUS_NUM(bus_number));
}
}
+ } else {
+ to_push->rejected = 1U;
+ can_send_errs += can_push(&can_rx_q, to_push) ? 0U : 1U;
}
}
-
-void can_set_forwarding(int from, int to) {
- can_forwarding[from] = to;
-}
diff --git a/panda/board/drivers/fdcan.h b/panda/board/drivers/fdcan.h
index b0931e226..601fc2d69 100644
--- a/panda/board/drivers/fdcan.h
+++ b/panda/board/drivers/fdcan.h
@@ -5,6 +5,11 @@
#define BUS_OFF_FAIL_LIMIT 2U
uint8_t bus_off_err[] = {0U, 0U, 0U};
+typedef struct {
+ volatile uint32_t header[2];
+ volatile uint32_t data_word[CANPACKET_DATA_SIZE_MAX/4U];
+} canfd_fifo;
+
FDCAN_GlobalTypeDef *cans[] = {FDCAN1, FDCAN2, FDCAN3};
bool can_set_speed(uint8_t can_number) {
@@ -12,7 +17,7 @@ bool can_set_speed(uint8_t can_number) {
FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
- ret &= llcan_set_speed(CANx, can_speed[bus_number], can_data_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number));
+ ret &= llcan_set_speed(CANx, bus_config[bus_number].can_speed, bus_config[bus_number].can_data_speed, can_loopback, (unsigned int)(can_silent) & (1U << can_number));
return ret;
}
@@ -42,36 +47,41 @@ void process_can(uint8_t can_number) {
FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
-
+
CANx->IR |= FDCAN_IR_TFE; // Clear Tx FIFO Empty flag
if ((CANx->TXFQS & FDCAN_TXFQS_TFQF) == 0) {
- CAN_FIFOMailBox_TypeDef to_send;
+ CANPacket_t to_send;
if (can_pop(can_queues[bus_number], &to_send)) {
can_tx_cnt += 1;
uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE);
uint8_t tx_index = (CANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F;
// only send if we have received a packet
- CAN_FIFOMailBox_TypeDef *fifo;
- fifo = (CAN_FIFOMailBox_TypeDef *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE));
+ canfd_fifo *fifo;
+ fifo = (canfd_fifo *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE));
- // Convert from "mailbox type"
- fifo->RIR = ((to_send.RIR & 0x6) << 28) | (to_send.RIR >> 3); // identifier format and frame type | identifier
- //REDEBUG: enable CAN FD and BRS for test purposes
- //fifo->RDTR = ((to_send.RDTR & 0xF) << 16) | ((to_send.RDTR) >> 16) | (1U << 21) | (1U << 20); // DLC (length) | timestamp | enable CAN FD | enable BRS
- fifo->RDTR = ((to_send.RDTR & 0xF) << 16) | ((to_send.RDTR) >> 16); // DLC (length) | timestamp
- fifo->RDLR = to_send.RDLR;
- fifo->RDHR = to_send.RDHR;
-
- CANx->TXBAR = (1UL << tx_index);
+ fifo->header[0] = (to_send.extended << 30) | ((to_send.extended != 0) ? (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;
+ for (unsigned int i = 0; i < data_len_w; i++) {
+ BYTE_ARRAY_TO_WORD(fifo->data_word[i], &to_send.data[i*4]);
+ }
+
+ CANx->TXBAR = (1UL << tx_index);
// Send back to USB
can_txd_cnt += 1;
- CAN_FIFOMailBox_TypeDef to_push;
- to_push.RIR = to_send.RIR;
- to_push.RDTR = (to_send.RDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
- to_push.RDLR = to_send.RDLR;
- to_push.RDHR = to_send.RDHR;
+ CANPacket_t to_push;
+
+ to_push.returned = 1U;
+ to_push.rejected = 0U;
+ to_push.extended = to_send.extended;
+ to_push.addr = to_send.addr;
+ to_push.bus = to_send.bus;
+ to_push.data_len_code = to_send.data_len_code;
+ (void)memcpy(to_push.data, to_send.data, dlc_to_len[to_push.data_len_code]);
can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
usb_cb_ep3_out_complete();
@@ -123,29 +133,37 @@ void can_rx(uint8_t can_number) {
rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F);
uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET);
- CAN_FIFOMailBox_TypeDef to_push;
- CAN_FIFOMailBox_TypeDef *fifo;
+ CANPacket_t to_push;
+ canfd_fifo *fifo;
// getting address
- fifo = (CAN_FIFOMailBox_TypeDef *)(RxFIFO0SA + (rx_fifo_idx * FDCAN_RX_FIFO_0_EL_SIZE));
+ fifo = (canfd_fifo *)(RxFIFO0SA + (rx_fifo_idx * FDCAN_RX_FIFO_0_EL_SIZE));
- // Need to convert real CAN frame format to mailbox "type"
- to_push.RIR = ((fifo->RIR >> 28) & 0x6) | (fifo->RIR << 3); // identifier format and frame type | identifier
- to_push.RDTR = ((fifo->RDTR >> 16) & 0xF) | (fifo->RDTR << 16); // DLC (length) | timestamp
- to_push.RDLR = fifo->RDLR;
- to_push.RDHR = fifo->RDHR;
+ 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.bus = bus_number;
+ to_push.data_len_code = ((fifo->header[1] >> 16) & 0xFU);
- // modify RDTR for our API
- to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
+ 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;
+ for (unsigned int i = 0; i < data_len_w; i++) {
+ WORD_TO_BYTE_ARRAY(&to_push.data[i*4], fifo->data_word[i]);
+ }
// forwarding (panda only)
- int bus_fwd_num = (can_forwarding[bus_number] != -1) ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
+ int bus_fwd_num = safety_fwd_hook(bus_number, &to_push);
if (bus_fwd_num != -1) {
- CAN_FIFOMailBox_TypeDef to_send;
- to_send.RIR = to_push.RIR;
- to_send.RDTR = to_push.RDTR;
- to_send.RDLR = to_push.RDLR;
- to_send.RDHR = to_push.RDHR;
+ CANPacket_t to_send;
+
+ to_send.returned = 0U;
+ to_send.rejected = 0U;
+ to_send.extended = to_push.extended;
+ to_send.addr = to_push.addr;
+ to_send.bus = to_push.bus;
+ to_send.data_len_code = to_push.data_len_code;
+ (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]);
can_send(&to_send, bus_fwd_num, true);
}
@@ -155,7 +173,7 @@ void can_rx(uint8_t can_number) {
current_board->set_led(LED_BLUE, true);
can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
- // update read index
+ // update read index
CANx->RXF0A = rx_fifo_idx;
}
@@ -165,10 +183,9 @@ void can_rx(uint8_t can_number) {
#endif
CANx->IR |= (FDCAN_IR_PEA | FDCAN_IR_PED | FDCAN_IR_RF0L | FDCAN_IR_RF0F | FDCAN_IR_EW | FDCAN_IR_MRAF | FDCAN_IR_TOO); // Clean all error flags
can_err_cnt += 1;
- } else {
-
+ } else {
+
}
-
}
void FDCAN1_IT0_IRQ_Handler(void) { can_rx(0); }
diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h
index 063ac8502..9b95e0dd8 100644
--- a/panda/board/drivers/gmlan_alt.h
+++ b/panda/board/drivers/gmlan_alt.h
@@ -75,7 +75,7 @@ int append_int(char *in, int in_len, int val, int val_len) {
return in_len_copy;
}
-int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
+int get_bit_message(char *out, CANPacket_t *to_bang) {
char pkt[MAX_BITS_CAN_PACKET];
char footer[] = {
1, // CRC delimiter
@@ -88,18 +88,18 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
int len = 0;
// test packet
- int dlc_len = to_bang->RDTR & 0xF;
+ int dlc_len = GET_LEN(to_bang);
len = append_int(pkt, len, 0, 1); // Start-of-frame
- if ((to_bang->RIR & 4) != 0) {
+ if (to_bang->extended != 0) {
// extended identifier
- len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
+ len = append_int(pkt, len, GET_ADDR(to_bang) >> 18, 11); // Identifier
len = append_int(pkt, len, 3, 2); // SRR+IDE
- len = append_int(pkt, len, (to_bang->RIR >> 3) & ((1U << 18) - 1U), 18); // Identifier
+ len = append_int(pkt, len, (GET_ADDR(to_bang)) & ((1U << 18) - 1U), 18); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+r1+r0
} else {
// standard identifier
- len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
+ len = append_int(pkt, len, GET_ADDR(to_bang), 11); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+IDE+reserved
}
@@ -107,8 +107,7 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
// append data
for (int i = 0; i < dlc_len; i++) {
- unsigned char dat = ((unsigned char *)(&(to_bang->RDLR)))[i];
- len = append_int(pkt, len, dat, 8);
+ len = append_int(pkt, len, to_bang->data[i], 8);
}
// append crc
@@ -269,7 +268,7 @@ void TIM12_IRQ_Handler(void) {
TIM12->SR = 0;
}
-bool bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
+bool bitbang_gmlan(CANPacket_t *to_bang) {
gmlan_send_ok = true;
gmlan_alt_mode = BITBANG;
diff --git a/panda/board/drivers/usb.h b/panda/board/drivers/usb.h
index 2316f7220..6b743bd19 100644
--- a/panda/board/drivers/usb.h
+++ b/panda/board/drivers/usb.h
@@ -23,7 +23,8 @@ typedef union _USB_Setup {
}
USB_Setup_TypeDef;
-#define MAX_CAN_MSGS_PER_BULK_TRANSFER 4U
+#define MAX_CAN_MSGS_PER_BULK_TRANSFER 51U
+#define MAX_EP1_CHUNK_PER_BULK_TRANSFER 16256 // max data stream chunk in bytes, shouldn't be higher than 16320 or counter will overflow
bool usb_eopf_detected = false;
@@ -493,7 +494,7 @@ void usb_setup(void) {
USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP;
USBx_OUTEP(2)->DOEPINT = 0xFF;
- USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
+ USBx_OUTEP(3)->DOEPTSIZ = (32U << 19) | 0x800U;
USBx_OUTEP(3)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2U << 18) |
USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP;
USBx_OUTEP(3)->DOEPINT = 0xFF;
@@ -934,7 +935,7 @@ void usb_irqhandler(void) {
void usb_outep3_resume_if_paused(void) {
ENTER_CRITICAL();
if (!outep3_processing && (USBx_OUTEP(3)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0) {
- USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
+ USBx_OUTEP(3)->DOEPTSIZ = (32U << 19) | 0x800U;
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
}
EXIT_CRITICAL();
diff --git a/panda/board/flasher.h b/panda/board/flasher.h
index 764eddb93..534b65e78 100644
--- a/panda/board/flasher.h
+++ b/panda/board/flasher.h
@@ -183,7 +183,7 @@ void CAN1_RX0_IRQ_Handler(void) {
if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
uint8_t dat[8];
for (int i = 0; i < 8; i++) {
- dat[i] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
+ dat[i] = GET_MAILBOX_BYTE(&CAN->sFIFOMailBox[0], i);
}
uint8_t odat[8];
uint8_t type = dat[0] & 0xF0;
diff --git a/panda/board/get_sdk.sh b/panda/board/get_sdk.sh
index 84964244e..c0d05651a 100755
--- a/panda/board/get_sdk.sh
+++ b/panda/board/get_sdk.sh
@@ -1,3 +1,3 @@
#!/bin/bash
-sudo apt-get install gcc-arm-none-eabi python-pip
+sudo apt-get install dfu-util gcc-arm-none-eabi python3-pip
sudo pip install libusb1 pycryptodome requests
diff --git a/panda/board/get_sdk_mac.sh b/panda/board/get_sdk_mac.sh
index 2daba2a14..9e4b71590 100755
--- a/panda/board/get_sdk_mac.sh
+++ b/panda/board/get_sdk_mac.sh
@@ -1,7 +1,7 @@
#!/bin/bash
# Need formula for gcc
sudo easy_install pip
-/usr/bin/ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)"
+/bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
brew tap ArmMbed/homebrew-formulae
brew install python dfu-util arm-none-eabi-gcc
pip install --user libusb1 pycryptodome requests
diff --git a/panda/board/libc.h b/panda/board/libc.h
index 14dde7351..1e32d4206 100644
--- a/panda/board/libc.h
+++ b/panda/board/libc.h
@@ -14,13 +14,36 @@ void *memset(void *str, int c, unsigned int n) {
return str;
}
-void *memcpy(void *dest, const void *src, unsigned int n) {
- uint8_t *d = dest;
- const uint8_t *s = src;
- for (unsigned int i = 0; i < n; i++) {
- *d = *s;
- d++;
- s++;
+#define UNALIGNED(X, Y) \
+ (((uint32_t)(X) & (sizeof(uint32_t) - 1U)) | ((uint32_t)(Y) & (sizeof(uint32_t) - 1U)))
+
+void *memcpy(void *dest, const void *src, unsigned int len) {
+ unsigned int n = len;
+ uint8_t *d8 = dest;
+ const uint8_t *s8 = src;
+
+ if ((n >= 4U) && !UNALIGNED(s8, d8)) {
+ uint32_t *d32 = (uint32_t *)d8; // cppcheck-suppress misra-c2012-11.3 ; already checked that it's properly aligned
+ const uint32_t *s32 = (const uint32_t *)s8; // cppcheck-suppress misra-c2012-11.3 ; already checked that it's properly aligned
+
+ while(n >= 16U) {
+ *d32 = *s32; d32++; s32++;
+ *d32 = *s32; d32++; s32++;
+ *d32 = *s32; d32++; s32++;
+ *d32 = *s32; d32++; s32++;
+ n -= 16U;
+ }
+
+ while(n >= 4U) {
+ *d32 = *s32; d32++; s32++;
+ n -= 4U;
+ }
+
+ d8 = (uint8_t *)d32;
+ s8 = (const uint8_t *)s32;
+ }
+ while (n-- > 0U) {
+ *d8 = *s8; d8++; s8++;
}
return dest;
}
diff --git a/panda/board/main.c b/panda/board/main.c
index 9ac7bd90f..ac7a75a5e 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -20,11 +20,14 @@
#include "drivers/bxcan.h"
#endif
+#include "usb_protocol.h"
+
#include "obj/gitversion.h"
extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used
// When changing this struct, boardd and python/__init__.py needs to be kept up to date!
+#define HEALTH_PACKET_VERSION 1
struct __attribute__((packed)) health_t {
uint32_t uptime_pkt;
uint32_t voltage_pkt;
@@ -192,15 +195,7 @@ int get_rtc_pkt(void *dat) {
return sizeof(t);
}
-int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
- UNUSED(hardwired);
- CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata;
- int ilen = 0;
- while (ilen < MIN(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) {
- ilen++;
- }
- return ilen*0x10;
-}
+
// send on serial, first byte to select the ring
void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
@@ -218,23 +213,6 @@ void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
}
}
-// send on CAN
-void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
- UNUSED(hardwired);
- int dpkt = 0;
- uint32_t *d32 = (uint32_t *)usbdata;
- for (dpkt = 0; dpkt < (len / 4); dpkt += 4) {
- CAN_FIFOMailBox_TypeDef to_push;
- to_push.RDHR = d32[dpkt + 3];
- to_push.RDLR = d32[dpkt + 2];
- to_push.RDTR = d32[dpkt + 1];
- to_push.RIR = d32[dpkt];
-
- uint8_t bus_number = (to_push.RDTR >> 4) & CAN_BUS_NUM_MASK;
- can_send(&to_push, bus_number, false);
- }
-}
-
void usb_cb_ep3_out_complete(void) {
if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
usb_outep3_resume_if_paused();
@@ -454,24 +432,17 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
set_safety_mode(setup->b.wValue.w, (uint16_t) setup->b.wIndex.w);
}
break;
- // **** 0xdd: enable can forwarding
+ // **** 0xdd: get healthpacket and CANPacket versions
case 0xdd:
- // wValue = Can Bus Num to forward from
- // wIndex = Can Bus Num to forward to
- if ((setup->b.wValue.w < BUS_MAX) && (setup->b.wIndex.w < BUS_MAX) &&
- (setup->b.wValue.w != setup->b.wIndex.w)) { // set forwarding
- can_set_forwarding(setup->b.wValue.w, setup->b.wIndex.w & CAN_BUS_NUM_MASK);
- } else if((setup->b.wValue.w < BUS_MAX) && (setup->b.wIndex.w == 0xFFU)){ //Clear Forwarding
- can_set_forwarding(setup->b.wValue.w, -1);
- } else {
- puts("Invalid CAN bus forwarding\n");
- }
+ resp[0] = HEALTH_PACKET_VERSION;
+ resp[1] = CAN_PACKET_VERSION;
+ resp_len = 2;
break;
// **** 0xde: set can bitrate
case 0xde:
if (setup->b.wValue.w < BUS_MAX) {
// TODO: add sanity check, ideally check if value is correct(from array of correct values)
- can_speed[setup->b.wValue.w] = setup->b.wIndex.w;
+ bus_config[setup->b.wValue.w].can_speed = setup->b.wIndex.w;
bool ret = can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w));
UNUSED(ret);
}
@@ -628,11 +599,21 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
case 0xf9:
if (setup->b.wValue.w < CAN_MAX) {
// TODO: add sanity check, ideally check if value is correct(from array of correct values)
- can_data_speed[setup->b.wValue.w] = setup->b.wIndex.w;
+ bus_config[setup->b.wValue.w].can_data_speed = setup->b.wIndex.w;
+ bus_config[setup->b.wValue.w].canfd_enabled = (setup->b.wIndex.w >= bus_config[setup->b.wValue.w].can_speed) ? true : false;
+ bus_config[setup->b.wValue.w].brs_enabled = (setup->b.wIndex.w > bus_config[setup->b.wValue.w].can_speed) ? true : false;
bool ret = can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w));
UNUSED(ret);
}
break;
+ // **** 0xfa: check if CAN FD and BRS are enabled
+ case 0xfa:
+ if (setup->b.wValue.w < CAN_MAX) {
+ resp[0] = bus_config[setup->b.wValue.w].canfd_enabled;
+ resp[1] = bus_config[setup->b.wValue.w].brs_enabled;
+ resp_len = 2;
+ }
+ break;
default:
puts("NO HANDLER ");
puth(setup->b.bRequest);
diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c
index 32d85aab3..7553b3e50 100644
--- a/panda/board/pedal/main.c
+++ b/panda/board/pedal/main.c
@@ -132,11 +132,11 @@ void CAN1_RX0_IRQ_Handler(void) {
int address = CAN->sFIFOMailBox[0].RIR >> 21;
if (address == CAN_GAS_INPUT) {
// softloader entry
- if (GET_BYTES_04(&CAN->sFIFOMailBox[0]) == 0xdeadface) {
- if (GET_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x0ab00b1e) {
+ if (GET_MAILBOX_BYTES_04(&CAN->sFIFOMailBox[0]) == 0xdeadface) {
+ if (GET_MAILBOX_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x0ab00b1e) {
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
NVIC_SystemReset();
- } else if (GET_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x02b00b1e) {
+ } else if (GET_MAILBOX_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x02b00b1e) {
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
NVIC_SystemReset();
} else {
@@ -147,7 +147,7 @@ void CAN1_RX0_IRQ_Handler(void) {
// normal packet
uint8_t dat[8];
for (int i=0; i<8; i++) {
- dat[i] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
+ dat[i] = GET_MAILBOX_BYTE(&CAN->sFIFOMailBox[0], i);
}
uint16_t value_0 = (dat[0] << 8) | dat[1];
uint16_t value_1 = (dat[2] << 8) | dat[3];
diff --git a/panda/board/safety.h b/panda/board/safety.h
index 5f5b03c3a..a833dc1b1 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -5,7 +5,6 @@
#include "safety/safety_honda.h"
#include "safety/safety_toyota.h"
#include "safety/safety_tesla.h"
-#include "safety/safety_gm_ascm.h"
#include "safety/safety_gm.h"
#include "safety/safety_ford.h"
#include "safety/safety_hyundai.h"
@@ -39,26 +38,27 @@
#define SAFETY_SUBARU_LEGACY 22U
#define SAFETY_HYUNDAI_LEGACY 23U
#define SAFETY_HYUNDAI_COMMUNITY 24U
+#define SAFETY_STELLANTIS 25U
uint16_t current_safety_mode = SAFETY_SILENT;
int16_t current_safety_param = 0;
const safety_hooks *current_hooks = &nooutput_hooks;
const addr_checks *current_rx_checks = &default_rx_checks;
-int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+int safety_rx_hook(CANPacket_t *to_push) {
return current_hooks->rx(to_push);
}
-int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
- return current_hooks->tx(to_send);
+int safety_tx_hook(CANPacket_t *to_send) {
+ return (relay_malfunction ? -1 : current_hooks->tx(to_send));
}
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len) {
return current_hooks->tx_lin(lin_num, data, len);
}
-int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
- return current_hooks->fwd(bus_num, to_fwd);
+int safety_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
+ return (relay_malfunction ? -1 : current_hooks->fwd(bus_num, to_fwd));
}
// Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8
@@ -76,7 +76,7 @@ void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) {
}
}
-bool msg_allowed(CAN_FIFOMailBox_TypeDef *to_send, const CanMsg msg_list[], int len) {
+bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len) {
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
int length = GET_LEN(to_send);
@@ -97,7 +97,7 @@ uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
return ts - ts_last;
}
-int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len) {
+int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], const int len) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
int length = GET_LEN(to_push);
@@ -171,11 +171,11 @@ void update_addr_timestamp(AddrCheckStruct addr_list[], int index) {
}
}
-bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push,
+bool addr_safety_check(CANPacket_t *to_push,
const addr_checks *rx_checks,
- uint8_t (*get_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
- uint8_t (*compute_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
- uint8_t (*get_counter)(CAN_FIFOMailBox_TypeDef *to_push)) {
+ uint8_t (*get_checksum)(CANPacket_t *to_push),
+ uint8_t (*compute_checksum)(CANPacket_t *to_push),
+ uint8_t (*get_counter)(CANPacket_t *to_push)) {
int index = get_addr_check_index(to_push, rx_checks->check, rx_checks->len);
update_addr_timestamp(rx_checks->check, index);
@@ -241,7 +241,6 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_TOYOTA, &toyota_hooks},
{SAFETY_ELM327, &elm327_hooks},
{SAFETY_GM, &gm_hooks},
- {SAFETY_HONDA_BOSCH_GIRAFFE, &honda_bosch_giraffe_hooks},
{SAFETY_HONDA_BOSCH_HARNESS, &honda_bosch_harness_hooks},
{SAFETY_HYUNDAI, &hyundai_hooks},
{SAFETY_CHRYSLER, &chrysler_hooks},
@@ -250,13 +249,12 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_NISSAN, &nissan_hooks},
{SAFETY_NOOUTPUT, &nooutput_hooks},
{SAFETY_HYUNDAI_LEGACY, &hyundai_legacy_hooks},
+ {SAFETY_MAZDA, &mazda_hooks},
#ifdef ALLOW_DEBUG
{SAFETY_TESLA, &tesla_hooks},
- {SAFETY_MAZDA, &mazda_hooks},
{SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks},
{SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks},
{SAFETY_ALLOUTPUT, &alloutput_hooks},
- {SAFETY_GM_ASCM, &gm_ascm_hooks},
{SAFETY_FORD, &ford_hooks},
#endif
};
@@ -274,6 +272,7 @@ int set_safety_hooks(uint16_t mode, int16_t param) {
cruise_engaged_prev = false;
vehicle_speed = 0;
vehicle_moving = false;
+ acc_main_on = false;
desired_torque_last = 0;
rt_torque_last = 0;
ts_angle_last = 0;
diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h
index f1f6040ea..35474f346 100644
--- a/panda/board/safety/safety_chrysler.h
+++ b/panda/board/safety/safety_chrysler.h
@@ -18,12 +18,12 @@ AddrCheckStruct chrysler_addr_checks[] = {
#define CHRYSLER_ADDR_CHECK_LEN (sizeof(chrysler_addr_checks) / sizeof(chrysler_addr_checks[0]))
addr_checks chrysler_rx_checks = {chrysler_addr_checks, CHRYSLER_ADDR_CHECK_LEN};
-static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t chrysler_get_checksum(CANPacket_t *to_push) {
int checksum_byte = GET_LEN(to_push) - 1;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
}
-static uint8_t chrysler_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t chrysler_compute_checksum(CANPacket_t *to_push) {
/* This function does not want the checksum byte in the input data.
jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf */
uint8_t checksum = 0xFFU;
@@ -56,12 +56,12 @@ static uint8_t chrysler_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return ~checksum;
}
-static uint8_t chrysler_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t chrysler_get_counter(CANPacket_t *to_push) {
// Well defined counter only for 8 bytes messages
return (uint8_t)(GET_BYTE(to_push, 6) >> 4);
}
-static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int chrysler_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &chrysler_rx_checks,
chrysler_get_checksum, chrysler_compute_checksum,
@@ -117,7 +117,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
return valid;
}
-static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int chrysler_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
@@ -126,10 +126,6 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// LKA STEER
if (addr == 0x292) {
int desired_torque = ((GET_BYTE(to_send, 0) & 0x7U) << 8) + GET_BYTE(to_send, 1) - 1024U;
@@ -186,21 +182,21 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
return tx;
}
-static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int chrysler_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
int addr = GET_ADDR(to_fwd);
- if (!relay_malfunction) {
- // forward CAN 0 -> 2 so stock LKAS camera sees messages
- if (bus_num == 0) {
- bus_fwd = 2;
- }
- // forward all messages from camera except LKAS_COMMAND and LKAS_HUD
- if ((bus_num == 2) && (addr != 658) && (addr != 678)) {
- bus_fwd = 0;
- }
+ // forward CAN 0 -> 2 so stock LKAS camera sees messages
+ if (bus_num == 0) {
+ bus_fwd = 2;
}
+
+ // forward all messages from camera except LKAS_COMMAND and LKAS_HUD
+ if ((bus_num == 2) && (addr != 658) && (addr != 678)) {
+ bus_fwd = 0;
+ }
+
return bus_fwd;
}
diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h
index cdbb656e8..5cda26bc6 100644
--- a/panda/board/safety/safety_defaults.h
+++ b/panda/board/safety/safety_defaults.h
@@ -3,7 +3,7 @@ const addr_checks default_rx_checks = {
.len = 0,
};
-int default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+int default_rx_hook(CANPacket_t *to_push) {
UNUSED(to_push);
return true;
}
@@ -17,7 +17,7 @@ static const addr_checks* nooutput_init(int16_t param) {
return &default_rx_checks;
}
-static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int nooutput_tx_hook(CANPacket_t *to_send) {
UNUSED(to_send);
return false;
}
@@ -29,7 +29,7 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
return false;
}
-static int default_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int default_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
UNUSED(bus_num);
UNUSED(to_fwd);
return -1;
@@ -45,14 +45,19 @@ const safety_hooks nooutput_hooks = {
// *** all output safety mode ***
+// Enables passthrough mode where relay is open and bus 0 gets forwarded to bus 2 and vice versa
+const uint16_t ALLOUTPUT_PARAM_PASSTHROUGH = 1;
+bool alloutput_passthrough = false;
+
static const addr_checks* alloutput_init(int16_t param) {
UNUSED(param);
+ alloutput_passthrough = GET_FLAG(param, ALLOUTPUT_PARAM_PASSTHROUGH);
controls_allowed = true;
relay_malfunction_reset();
return &default_rx_checks;
}
-static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int alloutput_tx_hook(CANPacket_t *to_send) {
UNUSED(to_send);
return true;
}
@@ -64,10 +69,26 @@ static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
return true;
}
+static int alloutput_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
+ UNUSED(to_fwd);
+ int bus_fwd = -1;
+
+ if (alloutput_passthrough) {
+ if (bus_num == 0) {
+ bus_fwd = 2;
+ }
+ if (bus_num == 2) {
+ bus_fwd = 0;
+ }
+ }
+
+ return bus_fwd;
+}
+
const safety_hooks alloutput_hooks = {
.init = alloutput_init,
.rx = default_rx_hook,
.tx = alloutput_tx_hook,
.tx_lin = alloutput_tx_lin_hook,
- .fwd = default_fwd_hook,
+ .fwd = alloutput_fwd_hook,
};
diff --git a/panda/board/safety/safety_elm327.h b/panda/board/safety/safety_elm327.h
index e6655c3d4..d4f562a83 100644
--- a/panda/board/safety/safety_elm327.h
+++ b/panda/board/safety/safety_elm327.h
@@ -1,4 +1,4 @@
-static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int elm327_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h
index 2c0c677a8..6a8f05838 100644
--- a/panda/board/safety/safety_ford.h
+++ b/panda/board/safety/safety_ford.h
@@ -8,7 +8,7 @@
// brake > 0mph
-static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int ford_rx_hook(CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int bus = GET_BUS(to_push);
@@ -65,7 +65,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// else
// block all commands that produce actuation
-static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int ford_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
@@ -79,10 +79,6 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
- if (relay_malfunction) {
- tx = 0;
- }
-
// STEER: safety check
if (addr == 0x3CA) {
if (!current_controls_allowed) {
diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h
index 6d7c66882..6ef30f279 100644
--- a/panda/board/safety/safety_gm.h
+++ b/panda/board/safety/safety_gm.h
@@ -34,7 +34,7 @@ AddrCheckStruct gm_addr_checks[] = {
#define GM_RX_CHECK_LEN (sizeof(gm_addr_checks) / sizeof(gm_addr_checks[0]))
addr_checks gm_rx_checks = {gm_addr_checks, GM_RX_CHECK_LEN};
-static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int gm_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &gm_rx_checks, NULL, NULL, NULL);
@@ -104,7 +104,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// else
// block all commands that produce actuation
-static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int gm_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
@@ -113,10 +113,6 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = brake_pressed_prev && vehicle_moving;
diff --git a/panda/board/safety/safety_gm_ascm.h b/panda/board/safety/safety_gm_ascm.h
deleted file mode 100644
index e7eddf454..000000000
--- a/panda/board/safety/safety_gm_ascm.h
+++ /dev/null
@@ -1,43 +0,0 @@
-// BUS 0 is on the LKAS module (ASCM) side
-// BUS 2 is on the actuator (EPS) side
-
-static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
-
- int bus_fwd = -1;
-
- if (bus_num == 0) {
- int addr = GET_ADDR(to_fwd);
- bus_fwd = 2;
- // do not propagate lkas messages from ascm to actuators, unless supercruise is on
- // block 0x152 and 0x154, which are the lkas command from ASCM1 and ASCM2
- // block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1
- //if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) {
- if ((addr == 0x152) || (addr == 0x154)) {
- bool supercruise_on = (GET_BYTE(to_fwd, 4) & 0x10) != 0; // bit 36
- if (!supercruise_on) {
- bus_fwd = -1;
- }
- }
- if ((addr == 0x151) || (addr == 0x153) || (addr == 0x314)) {
- // on the chassis bus, the OBDII port is on the module side, so we need to read
- // the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to
- // the actuator as 0x152 and 0x154
- uint32_t fwd_addr = addr + 1;
- to_fwd->RIR = (fwd_addr << 21) | (to_fwd->RIR & 0x1fffff);
- }
- }
-
- if (bus_num == 2) {
- bus_fwd = 0;
- }
-
- return bus_fwd;
-}
-
-const safety_hooks gm_ascm_hooks = {
- .init = nooutput_init,
- .rx = default_rx_hook,
- .tx = alloutput_tx_hook,
- .tx_lin = nooutput_tx_lin_hook,
- .fwd = gm_ascm_fwd_hook,
-};
diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
index 82a48d635..246d18b81 100644
--- a/panda/board/safety/safety_honda.h
+++ b/panda/board/safety/safety_honda.h
@@ -7,9 +7,7 @@
// 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_BG_TX_MSGS[] = {{0xE4, 2, 5}, {0xE5, 2, 8}, {0x296, 0, 4}, {0x33D, 2, 5}}; // Bosch Giraffe
const CanMsg HONDA_BH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}}; // Bosch Harness
-const CanMsg HONDA_BG_LONG_TX_MSGS[] = {{0xE4, 0, 5}, {0x1DF, 0, 8}, {0x1EF, 0, 8}, {0x1FA, 0, 8}, {0x30C, 0, 8}, {0x33D, 0, 5}, {0x39F, 0, 8}, {0x18DAB0F1, 0, 8}}; // Bosch Giraffe w/ gas and brakes
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
// Roughly calculated using the offsets in openpilot +5%:
@@ -24,14 +22,24 @@ const int HONDA_BOSCH_NO_GAS_VALUE = -30000; // value sent when not requesting g
const int HONDA_BOSCH_GAS_MAX = 2000;
const int HONDA_BOSCH_ACCEL_MIN = -350; // max braking == -3.5m/s2
-// Nidec and Bosch giraffe have pt on bus 0
-AddrCheckStruct honda_addr_checks[] = {
+// Nidec has the powertrain bus on bus 0
+AddrCheckStruct honda_nidec_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 }}},
+ {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 }}},
};
-#define HONDA_ADDR_CHECKS_LEN (sizeof(honda_addr_checks) / sizeof(honda_addr_checks[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
+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[] = {
@@ -39,26 +47,28 @@ AddrCheckStruct honda_bh_addr_checks[] = {
{.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]))
const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
const uint16_t HONDA_PARAM_BOSCH_LONG = 2;
+const uint16_t HONDA_PARAM_NIDEC_ALT = 4;
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_BG_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW;
-addr_checks honda_rx_checks = {honda_addr_checks, HONDA_ADDR_CHECKS_LEN};
+enum {HONDA_N_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW;
+addr_checks honda_rx_checks = {honda_nidec_addr_checks, HONDA_NIDEC_ADDR_CHECKS_LEN};
-static uint8_t honda_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t honda_get_checksum(CANPacket_t *to_push) {
int checksum_byte = GET_LEN(to_push) - 1;
return (uint8_t)(GET_BYTE(to_push, checksum_byte)) & 0xFU;
}
-static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t honda_compute_checksum(CANPacket_t *to_push) {
int len = GET_LEN(to_push);
uint8_t checksum = 0U;
unsigned int addr = GET_ADDR(to_push);
@@ -75,12 +85,12 @@ static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return (8U - checksum) & 0xFU;
}
-static uint8_t honda_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t honda_get_counter(CANPacket_t *to_push) {
int counter_byte = GET_LEN(to_push) - 1;
return ((uint8_t)(GET_BYTE(to_push, counter_byte)) >> 4U) & 0x3U;
}
-static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int honda_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &honda_rx_checks,
honda_get_checksum, honda_compute_checksum, honda_get_counter);
@@ -96,17 +106,30 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
}
+ // 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);
+ if (!acc_main_on) {
+ controls_allowed = 0;
+ }
+ }
+
// state machine to enter and exit controls
// 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;
switch (button) {
+ case 1: // main
case 2: // cancel
controls_allowed = 0;
break;
case 3: // set
case 4: // resume
- controls_allowed = 1;
+ if (acc_main_on) {
+ controls_allowed = 1;
+ }
break;
default:
break; // any other button is irrelevant
@@ -186,17 +209,13 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// else
// block all commands that produce actuation
-static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int honda_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
- if ((honda_hw == HONDA_BG_HW) && !honda_bosch_long) {
- tx = msg_allowed(to_send, HONDA_BG_TX_MSGS, sizeof(HONDA_BG_TX_MSGS)/sizeof(HONDA_BG_TX_MSGS[0]));
- } else if ((honda_hw == HONDA_BG_HW) && honda_bosch_long) {
- tx = msg_allowed(to_send, HONDA_BG_LONG_TX_MSGS, sizeof(HONDA_BG_LONG_TX_MSGS)/sizeof(HONDA_BG_LONG_TX_MSGS[0]));
- } else if ((honda_hw == HONDA_BH_HW) && !honda_bosch_long) {
+ 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]));
@@ -204,10 +223,6 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = msg_allowed(to_send, HONDA_N_TX_MSGS, sizeof(HONDA_N_TX_MSGS)/sizeof(HONDA_N_TX_MSGS[0]));
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = brake_pressed_prev && vehicle_moving;
@@ -296,9 +311,9 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// 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)) {
- tx = 0;
- }
+ if ((GET_BYTES_04(to_send) != 0x00803E02) || (GET_BYTES_48(to_send) != 0x0)) {
+ tx = 0;
+ }
}
// 1 allows the message through
@@ -313,23 +328,12 @@ static const addr_checks* honda_nidec_init(int16_t param) {
honda_hw = HONDA_N_HW;
honda_alt_brake_msg = false;
honda_bosch_long = false;
- honda_rx_checks = (addr_checks){honda_addr_checks, HONDA_ADDR_CHECKS_LEN};
- return &honda_rx_checks;
-}
-static const addr_checks* honda_bosch_giraffe_init(int16_t param) {
- controls_allowed = false;
- relay_malfunction_reset();
- honda_hw = HONDA_BG_HW;
- // Checking for alternate brake override from safety parameter
- honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE);
-
- // radar disabled so allow gas/brakes
-#ifdef ALLOW_DEBUG
- honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG);
-#endif
-
- honda_rx_checks = (addr_checks){honda_addr_checks, HONDA_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;
}
@@ -349,48 +353,47 @@ static const addr_checks* honda_bosch_harness_init(int16_t param) {
return &honda_rx_checks;
}
-static int honda_nidec_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int honda_nidec_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
// fwd from car to camera. also fwd certain msgs from camera to car
// 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX,
// 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud,
int bus_fwd = -1;
- if (!relay_malfunction) {
- if (bus_num == 0) {
- bus_fwd = 2;
- }
- if (bus_num == 2) {
- // block stock lkas messages and stock acc messages (if OP is doing ACC)
- int addr = GET_ADDR(to_fwd);
- bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
- bool is_acc_hud_msg = addr == 0x30C;
- bool is_brake_msg = addr == 0x1FA;
- bool block_fwd = is_lkas_msg || is_acc_hud_msg || (is_brake_msg && !honda_fwd_brake);
- if (!block_fwd) {
- bus_fwd = 0;
- }
+ if (bus_num == 0) {
+ bus_fwd = 2;
+ }
+
+ if (bus_num == 2) {
+ // block stock lkas messages and stock acc messages (if OP is doing ACC)
+ int addr = GET_ADDR(to_fwd);
+ bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
+ bool is_acc_hud_msg = addr == 0x30C;
+ bool is_brake_msg = addr == 0x1FA;
+ bool block_fwd = is_lkas_msg || is_acc_hud_msg || (is_brake_msg && !honda_fwd_brake);
+ if (!block_fwd) {
+ bus_fwd = 0;
}
}
+
return bus_fwd;
}
-static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *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 (!relay_malfunction) {
- if (bus_num == bus_rdr_car) {
- bus_fwd = bus_rdr_cam;
- }
- if (bus_num == bus_rdr_cam) {
- int addr = GET_ADDR(to_fwd);
- int is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D);
- if (!is_lkas_msg) {
- bus_fwd = bus_rdr_car;
- }
+ if (bus_num == bus_rdr_car) {
+ bus_fwd = bus_rdr_cam;
+ }
+ if (bus_num == bus_rdr_cam) {
+ int addr = GET_ADDR(to_fwd);
+ int is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D);
+ if (!is_lkas_msg) {
+ bus_fwd = bus_rdr_car;
}
}
+
return bus_fwd;
}
@@ -402,14 +405,6 @@ const safety_hooks honda_nidec_hooks = {
.fwd = honda_nidec_fwd_hook,
};
-const safety_hooks honda_bosch_giraffe_hooks = {
- .init = honda_bosch_giraffe_init,
- .rx = honda_rx_hook,
- .tx = honda_tx_hook,
- .tx_lin = nooutput_tx_lin_hook,
- .fwd = honda_bosch_fwd_hook,
-};
-
const safety_hooks honda_bosch_harness_hooks = {
.init = honda_bosch_harness_init,
.rx = honda_rx_hook,
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
index 6ed94c8ac..4e4f0e65a 100644
--- a/panda/board/safety/safety_hyundai.h
+++ b/panda/board/safety/safety_hyundai.h
@@ -69,7 +69,7 @@ bool hyundai_longitudinal = false;
addr_checks hyundai_rx_checks = {hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
-static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t hyundai_get_counter(CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t cnt;
@@ -89,7 +89,7 @@ static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
return cnt;
}
-static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t hyundai_get_checksum(CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t chksum;
@@ -107,7 +107,7 @@ static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return chksum;
}
-static uint8_t hyundai_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t hyundai_compute_checksum(CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
uint8_t chksum = 0;
@@ -143,7 +143,7 @@ static uint8_t hyundai_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return chksum;
}
-static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int hyundai_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &hyundai_rx_checks,
hyundai_get_checksum, hyundai_compute_checksum,
@@ -223,7 +223,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
return valid;
}
-static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int hyundai_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
@@ -234,10 +234,6 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = msg_allowed(to_send, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0]));
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// FCA11: Block any potential actuation
if (addr == 909) {
int CR_VSM_DecCmd = GET_BYTE(to_send, 1);
@@ -342,19 +338,19 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
return tx;
}
-static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int hyundai_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
int addr = GET_ADDR(to_fwd);
+
// forward cam to ccan and viceversa, except lkas cmd
- if (!relay_malfunction) {
- if (bus_num == 0) {
- bus_fwd = 2;
- }
- if ((bus_num == 2) && (addr != 832) && (addr != 1157)) {
- bus_fwd = 0;
- }
+ if (bus_num == 0) {
+ bus_fwd = 2;
}
+ if ((bus_num == 2) && (addr != 832) && (addr != 1157)) {
+ bus_fwd = 0;
+ }
+
return bus_fwd;
}
diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h
index cefde980d..01cc29f1b 100644
--- a/panda/board/safety/safety_mazda.h
+++ b/panda/board/safety/safety_mazda.h
@@ -1,5 +1,6 @@
// CAN msgs we care about
#define MAZDA_LKAS 0x243
+#define MAZDA_LKAS_HUD 0x440
#define MAZDA_CRZ_CTRL 0x21c
#define MAZDA_CRZ_BTNS 0x09d
#define MAZDA_STEER_TORQUE 0x240
@@ -23,12 +24,7 @@
#define MAZDA_DRIVER_TORQUE_FACTOR 1
#define MAZDA_MAX_TORQUE_ERROR 350
-// lkas enable speed 52kph, disable at 45kph
-#define MAZDA_LKAS_ENABLE_SPEED 5200
-#define MAZDA_LKAS_DISABLE_SPEED 4500
-
-const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}};
-bool mazda_lkas_allowed = false;
+const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}};
AddrCheckStruct mazda_addr_checks[] = {
{.msg = {{MAZDA_CRZ_CTRL, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}},
@@ -41,7 +37,7 @@ AddrCheckStruct mazda_addr_checks[] = {
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(CAN_FIFOMailBox_TypeDef *to_push) {
+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)) {
int addr = GET_ADDR(to_push);
@@ -49,17 +45,7 @@ static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if (addr == MAZDA_ENGINE_DATA) {
// sample speed: scale by 0.01 to get kph
int speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3);
-
vehicle_moving = speed > 10; // moving when speed > 0.1 kph
-
- // Enable LKAS at 52kph going up, disable at 45kph going down
- if (speed > MAZDA_LKAS_ENABLE_SPEED) {
- mazda_lkas_allowed = true;
- } else if (speed < MAZDA_LKAS_DISABLE_SPEED) {
- mazda_lkas_allowed = false;
- } else {
- // Misra-able appeasment block!
- }
}
if (addr == MAZDA_STEER_TORQUE) {
@@ -73,13 +59,7 @@ static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
bool cruise_engaged = GET_BYTE(to_push, 0) & 8;
if (cruise_engaged) {
if (!cruise_engaged_prev) {
- // do not engage until we hit the speed at which lkas is on
- if (mazda_lkas_allowed) {
- controls_allowed = 1;
- } else {
- controls_allowed = 0;
- cruise_engaged = false;
- }
+ controls_allowed = 1;
}
} else {
controls_allowed = 0;
@@ -100,7 +80,7 @@ static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
return valid;
}
-static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int mazda_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
@@ -109,10 +89,6 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// Check if msg is sent on the main BUS
if (bus == MAZDA_MAIN) {
@@ -162,24 +138,36 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
}
+
+ // cruise buttons check
+ if (addr == MAZDA_CRZ_BTNS) {
+ // allow resume spamming while controls allowed, but
+ // only allow cancel while contrls not allowed
+ bool cancel_cmd = (GET_BYTE(to_send, 0) == 0x1U);
+ if (!controls_allowed && !cancel_cmd) {
+ tx = 0;
+ }
+ }
}
+
return tx;
}
-static int mazda_fwd_hook(int bus, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int mazda_fwd_hook(int bus, CANPacket_t *to_fwd) {
int bus_fwd = -1;
- if (!relay_malfunction) {
- int addr = GET_ADDR(to_fwd);
- if (bus == MAZDA_MAIN) {
- bus_fwd = MAZDA_CAM;
- } else if (bus == MAZDA_CAM) {
- if (!(addr == MAZDA_LKAS)) {
- bus_fwd = MAZDA_MAIN;
- }
- } else {
- bus_fwd = -1;
+ int addr = GET_ADDR(to_fwd);
+
+ if (bus == MAZDA_MAIN) {
+ bus_fwd = MAZDA_CAM;
+ } else if (bus == MAZDA_CAM) {
+ bool block = (addr == MAZDA_LKAS) || (addr == MAZDA_LKAS_HUD);
+ if (!block) {
+ bus_fwd = MAZDA_MAIN;
}
+ } else {
+ // don't fwd
}
+
return bus_fwd;
}
@@ -187,7 +175,6 @@ static const addr_checks* mazda_init(int16_t param) {
UNUSED(param);
controls_allowed = false;
relay_malfunction_reset();
- mazda_lkas_allowed = false;
return &mazda_rx_checks;
}
diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h
index 8913f9a50..2c2c6f680 100644
--- a/panda/board/safety/safety_nissan.h
+++ b/panda/board/safety/safety_nissan.h
@@ -11,7 +11,14 @@ const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = {
const int NISSAN_DEG_TO_CAN = 100;
-const CanMsg NISSAN_TX_MSGS[] = {{0x169, 0, 8}, {0x2b1, 0, 8}, {0x4cc, 0, 8}, {0x20b, 2, 6}, {0x20b, 1, 6}, {0x280, 2, 8}};
+const CanMsg NISSAN_TX_MSGS[] = {
+ {0x169, 0, 8}, // LKAS
+ {0x2b1, 0, 8}, // PROPILOT_HUD
+ {0x4cc, 0, 8}, // PROPILOT_HUD_INFO_MSG
+ {0x20b, 2, 6}, // CRUISE_THROTTLE (X-Trail)
+ {0x20b, 1, 6}, // CRUISE_THROTTLE (Altima)
+ {0x280, 2, 8} // CANCEL_MSG (Leaf)
+};
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
AddrCheckStruct nissan_addr_checks[] = {
@@ -34,7 +41,7 @@ addr_checks nissan_rx_checks = {nissan_addr_checks, NISSAN_ADDR_CHECK_LEN};
// EPS Location. false = V-CAN, true = C-CAN
bool nissan_alt_eps = false;
-static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int nissan_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &nissan_rx_checks, NULL, NULL, NULL);
@@ -71,12 +78,12 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
}
- // X-trail 0x454, Leaf 0x1cc
- if ((addr == 0x454) || (addr == 0x1cc)) {
+ // X-trail 0x454, Leaf 0x239
+ if ((addr == 0x454) || (addr == 0x239)) {
if (addr == 0x454){
brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
} else {
- brake_pressed = GET_BYTE(to_push, 0) > 3;
+ brake_pressed = ((GET_BYTE(to_push, 4) >> 5) & 1) != 0;
}
}
@@ -99,7 +106,7 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
-static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int nissan_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
bool violation = 0;
@@ -108,10 +115,6 @@ static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// 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));
@@ -163,7 +166,7 @@ static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
-static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int nissan_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
int addr = GET_ADDR(to_fwd);
@@ -182,11 +185,6 @@ static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
}
}
- if (relay_malfunction) {
- bus_fwd = -1;
- }
-
- // fallback to do not forward
return bus_fwd;
}
diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h
index cde90980c..36eecb513 100644
--- a/panda/board/safety/safety_subaru.h
+++ b/panda/board/safety/safety_subaru.h
@@ -35,17 +35,17 @@ AddrCheckStruct subaru_l_addr_checks[] = {
{.msg = {{0x144, 0, 8, .expected_timestep = 50000U}, { 0 }, { 0 }}},
};
#define SUBARU_L_ADDR_CHECK_LEN (sizeof(subaru_l_addr_checks) / sizeof(subaru_l_addr_checks[0]))
-addr_checks subaru_l_rx_checks = {subaru_addr_checks, SUBARU_L_ADDR_CHECK_LEN};
+addr_checks subaru_l_rx_checks = {subaru_l_addr_checks, SUBARU_L_ADDR_CHECK_LEN};
-static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t subaru_get_checksum(CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}
-static uint8_t subaru_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t subaru_get_counter(CANPacket_t *to_push) {
return (uint8_t)(GET_BYTE(to_push, 1) & 0xF);
}
-static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t subaru_compute_checksum(CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U);
@@ -55,7 +55,7 @@ static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return checksum;
}
-static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+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);
@@ -102,7 +102,7 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
return valid;
}
-static int subaru_legacy_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int subaru_legacy_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &subaru_l_rx_checks, NULL, NULL, NULL);
@@ -148,7 +148,7 @@ static int subaru_legacy_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
return valid;
}
-static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int subaru_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
@@ -156,10 +156,6 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// steer cmd checks
if (addr == 0x122) {
int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x1FFF);
@@ -212,7 +208,7 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
return tx;
}
-static int subaru_legacy_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int subaru_legacy_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
@@ -220,10 +216,6 @@ static int subaru_legacy_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// steer cmd checks
if (addr == 0x164) {
int desired_torque = ((GET_BYTES_04(to_send) >> 8) & 0x1FFF);
@@ -276,48 +268,46 @@ static int subaru_legacy_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
return tx;
}
-static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int subaru_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
- if (!relay_malfunction) {
- if (bus_num == 0) {
- bus_fwd = 2; // Camera CAN
- }
- if (bus_num == 2) {
- // Global platform
- // 0x122 ES_LKAS
- // 0x221 ES_Distance
- // 0x322 ES_LKAS_State
- int addr = GET_ADDR(to_fwd);
- int block_msg = ((addr == 0x122) || (addr == 0x221) || (addr == 0x322));
- if (!block_msg) {
- bus_fwd = 0; // Main CAN
- }
+ if (bus_num == 0) {
+ bus_fwd = 2; // Camera CAN
+ }
+
+ if (bus_num == 2) {
+ // Global platform
+ // 0x122 ES_LKAS
+ // 0x221 ES_Distance
+ // 0x322 ES_LKAS_State
+ int addr = GET_ADDR(to_fwd);
+ int block_msg = ((addr == 0x122) || (addr == 0x221) || (addr == 0x322));
+ if (!block_msg) {
+ bus_fwd = 0; // Main CAN
}
}
- // fallback to do not forward
+
return bus_fwd;
}
-static int subaru_legacy_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int subaru_legacy_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
- if (!relay_malfunction) {
- if (bus_num == 0) {
- bus_fwd = 2; // Camera CAN
- }
- if (bus_num == 2) {
- // Preglobal platform
- // 0x161 is ES_CruiseThrottle
- // 0x164 is ES_LKAS
- int addr = GET_ADDR(to_fwd);
- int block_msg = ((addr == 0x161) || (addr == 0x164));
- if (!block_msg) {
- bus_fwd = 0; // Main CAN
- }
+ if (bus_num == 0) {
+ bus_fwd = 2; // Camera CAN
+ }
+
+ if (bus_num == 2) {
+ // Preglobal platform
+ // 0x161 is ES_CruiseThrottle
+ // 0x164 is ES_LKAS
+ int addr = GET_ADDR(to_fwd);
+ int block_msg = ((addr == 0x161) || (addr == 0x164));
+ if (!block_msg) {
+ bus_fwd = 0; // Main CAN
}
}
- // fallback to do not forward
+
return bus_fwd;
}
diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h
index b9b0feaf6..359c90078 100644
--- a/panda/board/safety/safety_tesla.h
+++ b/panda/board/safety/safety_tesla.h
@@ -7,18 +7,29 @@ const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = {
{5., 3.5, .8}};
const int TESLA_DEG_TO_CAN = 10;
+const float TESLA_MAX_ACCEL = 2.0; // m/s^2
+const float TESLA_MIN_ACCEL = -3.5; // m/s^2
+
+const int TESLA_FLAG_POWERTRAIN = 1;
+const int TESLA_FLAG_LONGITUDINAL_CONTROL = 2;
const CanMsg TESLA_TX_MSGS[] = {
{0x488, 0, 4}, // DAS_steeringControl
{0x45, 0, 8}, // STW_ACTN_RQ
{0x45, 2, 8}, // STW_ACTN_RQ
+ {0x2b9, 0, 8}, // DAS_control
};
+#define TESLA_TX_LEN (sizeof(TESLA_TX_MSGS) / sizeof(TESLA_TX_MSGS[0]))
+
+const CanMsg TESLA_PT_TX_MSGS[] = {
+ {0x2bf, 0, 8}, // DAS_control
+};
+#define TESLA_PT_TX_LEN (sizeof(TESLA_PT_TX_MSGS) / sizeof(TESLA_PT_TX_MSGS[0]))
AddrCheckStruct tesla_addr_checks[] = {
{.msg = {{0x370, 0, 8, .expected_timestep = 40000U}, { 0 }, { 0 }}}, // EPAS_sysStatus (25Hz)
{.msg = {{0x108, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque1 (100Hz)
{.msg = {{0x118, 0, 6, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque2 (100Hz)
- {.msg = {{0x155, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, // ESP_B (50Hz)
{.msg = {{0x20a, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, // BrakeMessage (50Hz)
{.msg = {{0x368, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // DI_state (10Hz)
{.msg = {{0x318, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // GTW_carState (10Hz)
@@ -26,10 +37,20 @@ AddrCheckStruct tesla_addr_checks[] = {
#define TESLA_ADDR_CHECK_LEN (sizeof(tesla_addr_checks) / sizeof(tesla_addr_checks[0]))
addr_checks tesla_rx_checks = {tesla_addr_checks, TESLA_ADDR_CHECK_LEN};
-bool autopilot_enabled = false;
+AddrCheckStruct tesla_pt_addr_checks[] = {
+ {.msg = {{0x106, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque1 (100Hz)
+ {.msg = {{0x116, 0, 6, .expected_timestep = 10000U}, { 0 }, { 0 }}}, // DI_torque2 (100Hz)
+ {.msg = {{0x1f8, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}}, // BrakeMessage (50Hz)
+ {.msg = {{0x256, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}}, // DI_state (10Hz)
+};
+#define TESLA_PT_ADDR_CHECK_LEN (sizeof(tesla_pt_addr_checks) / sizeof(tesla_pt_addr_checks[0]))
+addr_checks tesla_pt_rx_checks = {tesla_pt_addr_checks, TESLA_PT_ADDR_CHECK_LEN};
-static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
- bool valid = addr_safety_check(to_push, &tesla_rx_checks,
+bool tesla_longitudinal = false;
+bool tesla_powertrain = false; // Are we the second panda intercepting the powertrain bus?
+
+static int tesla_rx_hook(CANPacket_t *to_push) {
+ bool valid = addr_safety_check(to_push, tesla_powertrain ? (&tesla_pt_rx_checks) : (&tesla_rx_checks),
NULL, NULL, NULL);
if(valid) {
@@ -37,30 +58,32 @@ static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
if(bus == 0) {
- 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;
- update_sample(&angle_meas, angle_meas_new);
+ if (!tesla_powertrain) {
+ 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;
+ update_sample(&angle_meas, angle_meas_new);
+ }
}
- if(addr == 0x155) {
- // Vehicle speed: (0.01 * val) * KPH_TO_MPS
- vehicle_speed = ((GET_BYTE(to_push, 5) << 8) | (GET_BYTE(to_push, 6))) * 0.01 / 3.6;
- vehicle_moving = vehicle_speed > 0.;
+ 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_moving = ABS(vehicle_speed) > 0.1;
}
- if(addr == 0x108) {
+ if(addr == (tesla_powertrain ? 0x106 : 0x108)) {
// Gas pressed
gas_pressed = (GET_BYTE(to_push, 6) != 0);
}
- if(addr == 0x20a) {
+ if(addr == (tesla_powertrain ? 0x1f8 : 0x20a)) {
// Brake pressed
brake_pressed = (((GET_BYTE(to_push, 0) & 0x0C) >> 2) != 1);
}
- if(addr == 0x368) {
+ if(addr == (tesla_powertrain ? 0x256 : 0x368)) {
// Cruise state
int cruise_state = (GET_BYTE(to_push, 1) >> 4);
bool cruise_engaged = (cruise_state == 2) || // ENABLED
@@ -79,42 +102,31 @@ static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
}
- if (bus == 2) {
- if (addr == 0x399) {
- // Autopilot status
- int autopilot_status = (GET_BYTE(to_push, 0) & 0xF);
- autopilot_enabled = (autopilot_status == 3) || // ACTIVE_1
- (autopilot_status == 4) || // ACTIVE_2
- (autopilot_status == 5); // ACTIVE_NAVIGATE_ON_AUTOPILOT
-
- if (autopilot_enabled) {
- controls_allowed = 0;
- }
- }
+ if (tesla_powertrain) {
+ // 0x2bf: DAS_control should not be received on bus 0
+ generic_rx_checks((addr == 0x2bf) && (bus == 0));
+ } else {
+ // 0x488: DAS_steeringControl should not be received on bus 0
+ generic_rx_checks((addr == 0x488) && (bus == 0));
}
-
- // 0x488: DAS_steeringControl should not be received on bus 0
- generic_rx_checks((addr == 0x488) && (bus == 0));
}
return valid;
}
-static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int tesla_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
bool violation = false;
- if(!msg_allowed(to_send, TESLA_TX_MSGS, sizeof(TESLA_TX_MSGS) / sizeof(TESLA_TX_MSGS[0]))) {
+ if(!msg_allowed(to_send,
+ tesla_powertrain ? TESLA_PT_TX_MSGS : TESLA_TX_MSGS,
+ tesla_powertrain ? TESLA_PT_TX_LEN : TESLA_TX_LEN)) {
tx = 0;
}
- if(relay_malfunction) {
- tx = 0;
- }
-
- if(addr == 0x488) {
+ 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));
@@ -150,7 +162,7 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
}
- if(addr == 0x45) {
+ 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);
if((control_lever_status != 0) && (control_lever_status != 1)) {
@@ -158,6 +170,33 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
}
+ if(addr == (tesla_powertrain ? 0x2bf : 0x2b9)) {
+ // 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;
+ 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);
+ float accel_max = (0.04 * raw_accel_max) - 15;
+ float accel_min = (0.04 * raw_accel_min) - 15;
+
+ if ((accel_max > TESLA_MAX_ACCEL) || (accel_min > TESLA_MAX_ACCEL)){
+ violation = true;
+ }
+
+ if ((accel_max < TESLA_MIN_ACCEL) || (accel_min < TESLA_MIN_ACCEL)){
+ violation = true;
+ }
+ } else {
+ violation = true;
+ }
+ }
+
if(violation) {
controls_allowed = 0;
tx = 0;
@@ -166,35 +205,43 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
return tx;
}
-static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int tesla_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
int addr = GET_ADDR(to_fwd);
if(bus_num == 0) {
- // Chassis to autopilot
+ // Chassis/PT to autopilot
bus_fwd = 2;
}
if(bus_num == 2) {
- // Autopilot to chassis
- bool block_msg = ((addr == 0x488) && !autopilot_enabled);
+ // Autopilot to chassis/PT
+ int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9);
+
+ bool block_msg = false;
+ if (!tesla_powertrain && (addr == 0x488)) {
+ block_msg = true;
+ }
+
+ if (tesla_longitudinal && (addr == das_control_addr)) {
+ block_msg = true;
+ }
+
if(!block_msg) {
bus_fwd = 0;
}
}
- if(relay_malfunction) {
- bus_fwd = -1;
- }
-
return bus_fwd;
}
static const addr_checks* tesla_init(int16_t param) {
- UNUSED(param);
+ tesla_powertrain = GET_FLAG(param, TESLA_FLAG_POWERTRAIN);
+ tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
controls_allowed = 0;
relay_malfunction_reset();
- return &tesla_rx_checks;
+
+ return tesla_powertrain ? (&tesla_pt_rx_checks) : (&tesla_rx_checks);
}
const safety_hooks tesla_hooks = {
diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index bc2815acc..843acab0b 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -44,7 +44,7 @@ addr_checks toyota_rx_checks = {toyota_addr_checks, TOYOTA_ADDR_CHECKS_LEN};
// global actuation limit states
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
-static uint8_t toyota_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t toyota_compute_checksum(CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U) + (uint8_t)(len);
@@ -54,12 +54,12 @@ static uint8_t toyota_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return checksum;
}
-static uint8_t toyota_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t toyota_get_checksum(CANPacket_t *to_push) {
int checksum_byte = GET_LEN(to_push) - 1;
return (uint8_t)(GET_BYTE(to_push, checksum_byte));
}
-static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+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);
@@ -134,7 +134,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
return valid;
}
-static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int toyota_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
@@ -144,10 +144,6 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
- if (relay_malfunction) {
- tx = 0;
- }
-
// Check if msg is sent on BUS 0
if (bus == 0) {
@@ -251,26 +247,27 @@ static const addr_checks* toyota_init(int16_t param) {
return &toyota_rx_checks;
}
-static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int toyota_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int bus_fwd = -1;
- if (!relay_malfunction) {
- if (bus_num == 0) {
- bus_fwd = 2;
- }
- if (bus_num == 2) {
- int addr = GET_ADDR(to_fwd);
- // block stock lkas messages and stock acc messages (if OP is doing ACC)
- // in TSS2, 0x191 is LTA which we need to block to avoid controls collision
- int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191));
- // in TSS2 the camera does ACC as well, so filter 0x343
- int is_acc_msg = (addr == 0x343);
- int block_msg = is_lkas_msg || is_acc_msg;
- if (!block_msg) {
- bus_fwd = 0;
- }
+
+ if (bus_num == 0) {
+ bus_fwd = 2;
+ }
+
+ if (bus_num == 2) {
+ int addr = GET_ADDR(to_fwd);
+ // block stock lkas messages and stock acc messages (if OP is doing ACC)
+ // in TSS2, 0x191 is LTA which we need to block to avoid controls collision
+ int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191));
+ // in TSS2 the camera does ACC as well, so filter 0x343
+ int is_acc_msg = (addr == 0x343);
+ int block_msg = is_lkas_msg || is_acc_msg;
+ if (!block_msg) {
+ bus_fwd = 0;
}
}
+
return bus_fwd;
}
diff --git a/panda/board/safety/safety_volkswagen.h b/panda/board/safety/safety_volkswagen.h
index 3332fe20a..42c6cac2f 100644
--- a/panda/board/safety/safety_volkswagen.h
+++ b/panda/board/safety/safety_volkswagen.h
@@ -58,22 +58,22 @@ int volkswagen_lane_msg = 0;
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
-static uint8_t volkswagen_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_get_checksum(CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}
-static uint8_t volkswagen_mqb_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_mqb_get_counter(CANPacket_t *to_push) {
// MQB message counters are consistently found at LSB 8.
return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
}
-static uint8_t volkswagen_pq_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_pq_get_counter(CANPacket_t *to_push) {
// Few PQ messages have counters, and their offsets are inconsistent. This
// function works only for Lenkhilfe_3 at this time.
return (uint8_t)(GET_BYTE(to_push, 1) & 0xF0U) >> 4;
}
-static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_mqb_compute_crc(CANPacket_t *to_push) {
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
@@ -108,7 +108,7 @@ static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
return crc ^ 0xFFU;
}
-static uint8_t volkswagen_pq_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_pq_compute_checksum(CANPacket_t *to_push) {
int len = GET_LEN(to_push);
uint8_t checksum = 0U;
@@ -140,7 +140,7 @@ static const addr_checks* volkswagen_pq_init(int16_t param) {
return &volkswagen_pq_rx_checks;
}
-static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+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);
@@ -200,7 +200,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
return valid;
}
-static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+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);
@@ -297,11 +297,11 @@ static bool volkswagen_steering_check(int desired_torque) {
return violation;
}
-static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int volkswagen_mqb_tx_hook(CANPacket_t *to_send) {
int addr = GET_ADDR(to_send);
int tx = 1;
- if (!msg_allowed(to_send, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN) || relay_malfunction) {
+ if (!msg_allowed(to_send, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN)) {
tx = 0;
}
@@ -333,11 +333,11 @@ static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
return tx;
}
-static int volkswagen_pq_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int volkswagen_pq_tx_hook(CANPacket_t *to_send) {
int addr = GET_ADDR(to_send);
int tx = 1;
- if (!msg_allowed(to_send, VOLKSWAGEN_PQ_TX_MSGS, VOLKSWAGEN_PQ_TX_MSGS_LEN) || relay_malfunction) {
+ if (!msg_allowed(to_send, VOLKSWAGEN_PQ_TX_MSGS, VOLKSWAGEN_PQ_TX_MSGS_LEN)) {
tx = 0;
}
@@ -370,31 +370,30 @@ static int volkswagen_pq_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
return tx;
}
-static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int volkswagen_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
int addr = GET_ADDR(to_fwd);
int bus_fwd = -1;
- if (!relay_malfunction) {
- switch (bus_num) {
- case 0:
- // Forward all traffic from the Extended CAN onward
- bus_fwd = 2;
- break;
- case 2:
- if ((addr == volkswagen_torque_msg) || (addr == volkswagen_lane_msg)) {
- // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera
- bus_fwd = -1;
- } else {
- // Forward all remaining traffic from Extended CAN devices to J533 gateway
- bus_fwd = 0;
- }
- break;
- default:
- // No other buses should be in use; fallback to do-not-forward
+ switch (bus_num) {
+ case 0:
+ // Forward all traffic from the Extended CAN onward
+ bus_fwd = 2;
+ break;
+ case 2:
+ if ((addr == volkswagen_torque_msg) || (addr == volkswagen_lane_msg)) {
+ // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera
bus_fwd = -1;
- break;
- }
+ } else {
+ // Forward all remaining traffic from Extended CAN devices to J533 gateway
+ bus_fwd = 0;
+ }
+ break;
+ default:
+ // No other buses should be in use; fallback to do-not-forward
+ bus_fwd = -1;
+ break;
}
+
return bus_fwd;
}
diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h
index a502a320a..d5705b482 100644
--- a/panda/board/safety_declarations.h
+++ b/panda/board/safety_declarations.h
@@ -1,3 +1,9 @@
+#define GET_BIT(msg, b) (((msg)->data[(int)((b) / 8)] >> ((b) % 8)) & 0x1)
+#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_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask))
+
const int MAX_WRONG_COUNTERS = 5;
const uint8_t MAX_MISSED_MSGS = 10U;
@@ -48,8 +54,8 @@ typedef struct {
int len;
} addr_checks;
-int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
-int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
+int safety_rx_hook(CANPacket_t *to_push);
+int safety_tx_hook(CANPacket_t *to_send);
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
int to_signed(int d, int bits);
@@ -63,25 +69,25 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
float interpolate(struct lookup_t xy, float x);
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]);
-bool msg_allowed(CAN_FIFOMailBox_TypeDef *to_send, const CanMsg msg_list[], int len);
-int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len);
+bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len);
+int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], const int len);
void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter);
void update_addr_timestamp(AddrCheckStruct addr_list[], int index);
bool is_msg_valid(AddrCheckStruct addr_list[], int index);
-bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push,
+bool addr_safety_check(CANPacket_t *to_push,
const addr_checks *rx_checks,
- uint8_t (*get_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
- uint8_t (*compute_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
- uint8_t (*get_counter)(CAN_FIFOMailBox_TypeDef *to_push));
+ uint8_t (*get_checksum)(CANPacket_t *to_push),
+ uint8_t (*compute_checksum)(CANPacket_t *to_push),
+ uint8_t (*get_counter)(CANPacket_t *to_push));
void generic_rx_checks(bool stock_ecu_detected);
void relay_malfunction_set(void);
void relay_malfunction_reset(void);
typedef const addr_checks* (*safety_hook_init)(int16_t param);
-typedef int (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
-typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send);
+typedef int (*rx_hook)(CANPacket_t *to_push);
+typedef int (*tx_hook)(CANPacket_t *to_send);
typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len);
-typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
+typedef int (*fwd_hook)(int bus_num, CANPacket_t *to_fwd);
typedef struct {
safety_hook_init init;
@@ -105,6 +111,7 @@ bool brake_pressed_prev = false;
bool cruise_engaged_prev = false;
float vehicle_speed = 0;
bool vehicle_moving = false;
+bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018
// for safety modes with torque steering control
int desired_torque_last = 0; // last desired steer torque
diff --git a/panda/board/stm32fx/stm32fx_config.h b/panda/board/stm32fx/stm32fx_config.h
index 0777d2eb2..8c5ca1c71 100644
--- a/panda/board/stm32fx/stm32fx_config.h
+++ b/panda/board/stm32fx/stm32fx_config.h
@@ -38,6 +38,10 @@
#define PROVISION_CHUNK_ADDRESS 0x1FFF79E0U
#define DEVICE_SERIAL_NUMBER_ADDRESS 0x1FFF79C0U
+#define CANPACKET_DATA_SIZE_MAX 8U
+
+#include "can_definitions.h"
+
#ifndef BOOTSTUB
#ifdef PANDA
#include "main_declarations.h"
diff --git a/panda/board/stm32h7/clock.h b/panda/board/stm32h7/clock.h
index ce692b408..627fedee5 100644
--- a/panda/board/stm32h7/clock.h
+++ b/panda/board/stm32h7/clock.h
@@ -1,8 +1,8 @@
void clock_init(void) {
//Set power mode to direct SMPS power supply(depends on the board layout)
register_set(&(PWR->CR3), PWR_CR3_SMPSEN, 0xFU); // powered only by SMPS
- //Set VOS level to VOS0. (VOS3 to 170Mhz, VOS2 to 300Mhz, VOS1 to 400Mhz, VOS0 to 550Mhz)
- register_set(&(PWR->D3CR), PWR_D3CR_VOS_1, 0xC000U); //VOS2
+ //Set VOS level (VOS3 to 170Mhz, VOS2 to 300Mhz, VOS1 to 400Mhz, VOS0 to 550Mhz)
+ register_set(&(PWR->D3CR), PWR_D3CR_VOS_1 | PWR_D3CR_VOS_0, 0xC000U); //VOS1, needed for 80Mhz CAN FD
while ((PWR->CSR1 & PWR_CSR1_ACTVOSRDY) == 0);
while ((PWR->CSR1 & PWR_CSR1_ACTVOS) != (PWR->D3CR & PWR_D3CR_VOS)); // check that VOS level was actually set
// Configure Flash ACR register LATENCY and WRHIGHFREQ (VOS0 range!)
@@ -10,12 +10,12 @@ void clock_init(void) {
// enable external oscillator HSE
register_set_bits(&(RCC->CR), RCC_CR_HSEON);
while ((RCC->CR & RCC_CR_HSERDY) == 0);
- // Specify the frequency source for PLL1, divider for DIVM1, divider for DIVM2 : HSE, 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, 0x3F3F3U);
+ // 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);
// *** PLL1 start ***
- // Specify multiplier N and dividers P, Q, R for PLL1 : 48, 1, 5, 2
- register_set(&(RCC->PLL1DIVR), 0x104002FU, 0x7F7FFFFFU);
+ // Specify multiplier N and dividers P, Q, R for PLL1 : 48, 1, 3, 2 (clock 240Mhz, PLL1Q 80Mhz for CAN FD)
+ register_set(&(RCC->PLL1DIVR), 0x102002FU, 0x7F7FFFFFU);
// Specify the input and output frequency ranges, enable dividers for PLL1
register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLL1RGE_2 | RCC_PLLCFGR_DIVP1EN | RCC_PLLCFGR_DIVQ1EN | RCC_PLLCFGR_DIVR1EN, 0x7000CU);
// Enable PLL1
@@ -23,6 +23,16 @@ 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);
@@ -30,8 +40,6 @@ void clock_init(void) {
register_set(&(RCC->D2CFGR), RCC_D2CFGR_D2PPRE1_DIV2 | RCC_D2CFGR_D2PPRE2_DIV2, 0x770U);
// RCC APB4 Clock Source
register_set(&(RCC->D3CFGR), RCC_D3CFGR_D3PPRE_DIV2, 0x70U);
- // PLL2P for ADC
- register_clear_bits(&(RCC->D3CFGR), RCC_D3CCIPR_ADCSEL);
// Set SysClock source to PLL
register_set(&(RCC->CFGR), RCC_CFGR_SW_PLL1, 0x7U);
@@ -41,12 +49,12 @@ void clock_init(void) {
register_set_bits(&(RCC->AHB4ENR), RCC_APB4ENR_SYSCFGEN);
//////////////END OTHER CLOCKS////////////////////
- // Configure clock source for USB
- register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_0, RCC_D2CCIP2R_USBSEL); //PLL1Q
- // Configure clock source for FDCAN
- register_set(&(RCC->D2CCIP1R), RCC_D2CCIP1R_FDCANSEL_0, RCC_D2CCIP1R_FDCANSEL); //PLL1Q
- // Configure clock source for ADC1,2,3
- register_set(&(RCC->D3CCIPR), RCC_D3CCIPR_ADCSEL_1, RCC_D3CCIPR_ADCSEL); //per_ck(currently HSE)
+ // Configure clock source for USB (PLL3Q at 48Mhz)
+ register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_1, 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))
+ register_set(&(RCC->D3CCIPR), RCC_D3CCIPR_ADCSEL_1, RCC_D3CCIPR_ADCSEL);
//Enable the Clock Security System
register_set_bits(&(RCC->CR), RCC_CR_CSSHSEON);
//Enable Vdd33usb supply level detector
diff --git a/panda/board/stm32h7/llfdcan.h b/panda/board/stm32h7/llfdcan.h
index e9440a4c4..04c9078dc 100644
--- a/panda/board/stm32h7/llfdcan.h
+++ b/panda/board/stm32h7/llfdcan.h
@@ -4,28 +4,28 @@
#define FDCAN_OFFSET_W 853UL // words for each FDCAN module
#define FDCAN_END_ADDRESS 0x4000D3FCUL // Message RAM has a width of 4 Bytes
-// With this settings we can go up to 6Mbit/s
+// With this settings we can go up to 5Mbit/s
#define CAN_SYNC_JW 1U // 1 to 4
#define CAN_PHASE_SEG1 6U // =(PROP_SEG + PHASE_SEG1) , 1 to 16
#define CAN_PHASE_SEG2 1U // 1 to 8
-#define CAN_PCLK 48000U // Sourced from PLL1Q
+#define CAN_PCLK 80000U // Sourced from PLL1Q
#define CAN_QUANTA (1U + CAN_PHASE_SEG1 + CAN_PHASE_SEG2)
// Valid speeds in kbps and their prescalers:
-// 10=600, 20=300, 50=120, 83.333=72, 100=60, 125=48, 250=24, 500=12, 1000=6, 2000=3, 3000=2, 6000=1
+// 10=1000, 20=500, 50=200, 83.333=120, 100=100, 125=80, 250=40, 500=20, 1000=10, 2000=5, 5000=2
#define can_speed_to_prescaler(x) (CAN_PCLK / CAN_QUANTA * 10U / (x))
// RX FIFO 0
-#define FDCAN_RX_FIFO_0_EL_CNT 32UL
+#define FDCAN_RX_FIFO_0_EL_CNT 24UL
#define FDCAN_RX_FIFO_0_HEAD_SIZE 8UL // bytes
-#define FDCAN_RX_FIFO_0_DATA_SIZE 8UL // bytes
+#define FDCAN_RX_FIFO_0_DATA_SIZE 64UL // bytes
#define FDCAN_RX_FIFO_0_EL_SIZE (FDCAN_RX_FIFO_0_HEAD_SIZE + FDCAN_RX_FIFO_0_DATA_SIZE)
#define FDCAN_RX_FIFO_0_EL_W_SIZE (FDCAN_RX_FIFO_0_EL_SIZE / 4UL)
#define FDCAN_RX_FIFO_0_OFFSET 0UL
// TX FIFO
-#define FDCAN_TX_FIFO_EL_CNT 32UL
+#define FDCAN_TX_FIFO_EL_CNT 16UL
#define FDCAN_TX_FIFO_HEAD_SIZE 8UL // bytes
-#define FDCAN_TX_FIFO_DATA_SIZE 8UL // bytes
+#define FDCAN_TX_FIFO_DATA_SIZE 64UL // bytes
#define FDCAN_TX_FIFO_EL_SIZE (FDCAN_TX_FIFO_HEAD_SIZE + FDCAN_TX_FIFO_DATA_SIZE)
#define FDCAN_TX_FIFO_EL_W_SIZE (FDCAN_TX_FIFO_EL_SIZE / 4UL)
#define FDCAN_TX_FIFO_OFFSET (FDCAN_RX_FIFO_0_OFFSET + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_W_SIZE))
@@ -33,13 +33,6 @@
#define CAN_NAME_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? "FDCAN1" : (((CAN_DEV) == FDCAN2) ? "FDCAN2" : "FDCAN3"))
#define CAN_NUM_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? 0UL : (((CAN_DEV) == FDCAN2) ? 1UL : 2UL))
-// For backwards compatibility with safety code
-typedef struct {
- __IO uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */
- __IO uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */
- __IO uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */
- __IO uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */
-} CAN_FIFOMailBox_TypeDef;
void puts(const char *a);
@@ -138,10 +131,10 @@ bool llcan_init(FDCAN_GlobalTypeDef *CANx) {
// Set TX mode to FIFO
CANx->TXBC &= ~(FDCAN_TXBC_TFQM);
- // Configure TX element size (for now 8 bytes, no need to change)
- //CANx->TXESC |= 0x000U;
- //Configure RX FIFO0, FIFO1, RX buffer element sizes (no need for now, using classic 8 bytes)
- register_set(&(CANx->RXESC), 0x0U, (FDCAN_RXESC_F0DS | FDCAN_RXESC_F1DS | FDCAN_RXESC_RBDS));
+ // Configure TX element data size
+ CANx->TXESC |= 0x7U << FDCAN_TXESC_TBDS_Pos; // 64 bytes
+ //Configure RX FIFO0 element data size
+ CANx->RXESC |= 0x7U << FDCAN_RXESC_F0DS_Pos;
// Disable filtering, accept all valid frames received
CANx->XIDFC &= ~(FDCAN_XIDFC_LSE); // No extended filters
CANx->SIDFC &= ~(FDCAN_SIDFC_LSS); // No standard filters
@@ -154,13 +147,13 @@ bool llcan_init(FDCAN_GlobalTypeDef *CANx) {
uint32_t TxFIFOSA = RxFIFO0SA + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE);
// RX FIFO 0
- CANx->RXF0C = (FDCAN_RX_FIFO_0_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_RXF0C_F0SA_Pos;
+ CANx->RXF0C |= (FDCAN_RX_FIFO_0_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_RXF0C_F0SA_Pos;
CANx->RXF0C |= FDCAN_RX_FIFO_0_EL_CNT << FDCAN_RXF0C_F0S_Pos;
// RX FIFO 0 switch to non-blocking (overwrite) mode
CANx->RXF0C |= FDCAN_RXF0C_F0OM;
// TX FIFO (mode set earlier)
- CANx->TXBC = (FDCAN_TX_FIFO_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_TXBC_TBSA_Pos;
+ CANx->TXBC |= (FDCAN_TX_FIFO_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_TXBC_TBSA_Pos;
CANx->TXBC |= FDCAN_TX_FIFO_EL_CNT << FDCAN_TXBC_TFQS_Pos;
// Flush allocated RAM
diff --git a/panda/board/stm32h7/stm32h7_config.h b/panda/board/stm32h7/stm32h7_config.h
index a295d18ab..cdc2a9dcc 100644
--- a/panda/board/stm32h7/stm32h7_config.h
+++ b/panda/board/stm32h7/stm32h7_config.h
@@ -34,6 +34,10 @@
#define PROVISION_CHUNK_ADDRESS 0x080FFFE0U
#define DEVICE_SERIAL_NUMBER_ADDRESS 0x080FFFC0U
+#define CANPACKET_DATA_SIZE_MAX 64U
+
+#include "can_definitions.h"
+
#ifndef BOOTSTUB
#include "main_declarations.h"
#else
diff --git a/panda/board/stm32h7/stm32h7x5_flash.ld b/panda/board/stm32h7/stm32h7x5_flash.ld
index f03600b5e..aeaa4e3be 100644
--- a/panda/board/stm32h7/stm32h7x5_flash.ld
+++ b/panda/board/stm32h7/stm32h7x5_flash.ld
@@ -180,6 +180,12 @@ SECTIONS
. = ALIGN(8);
} >DTCMRAM
+ .ram_d1 (NOLOAD) :
+ {
+ . = ALIGN(4);
+ *(.ram_d1*)
+ } >RAM_D1
+
.ARM.attributes 0 : { *(.ARM.attributes) }
}
diff --git a/panda/board/usb_protocol.h b/panda/board/usb_protocol.h
new file mode 100644
index 000000000..0e50aa7a0
--- /dev/null
+++ b/panda/board/usb_protocol.h
@@ -0,0 +1,108 @@
+typedef struct {
+ uint8_t ptr;
+ uint8_t 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 usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
+ UNUSED(hardwired);
+ uint8_t 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) {
+ (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;
+ }
+ }
+
+ if (total_rx_size > MAX_EP1_CHUNK_PER_BULK_TRANSFER) {
+ total_rx_size = 0;
+ ep1_buffer.counter = 0;
+ } 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];
+ if ((pos + pckt_len) <= len) {
+ (void)memcpy(&usbdata8[pos], &can_packet, pckt_len);
+ pos += pckt_len;
+ } else {
+ (void)memcpy(&usbdata8[pos], &can_packet, len - pos);
+ ep1_buffer.ptr = pckt_len - (len - pos);
+ //(void)memcpy(ep1_buffer.data, ((uint8_t*)&can_packet + (len - pos)), ep1_buffer.ptr);
+ // cppcheck-suppress objectIndex
+ (void)memcpy(ep1_buffer.data, &((uint8_t*)&can_packet)[(len - pos)], ep1_buffer.ptr);
+ pos = len;
+ }
+ }
+ ep1_buffer.counter++;
+ total_rx_size += pos;
+ }
+ if (pos != len) {
+ ep1_buffer.counter = 0;
+ total_rx_size = 0;
+ }
+ if (pos <= 1) { pos = 0; }
+ return pos;
+}
+
+usb_asm_buffer ep3_buffer = {.ptr = 0, .tail_size = 0, .counter = 0};
+
+// send on CAN
+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;
+ 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;
+ ep3_buffer.counter++;
+ if (ep3_buffer.ptr != 0) {
+ if (ep3_buffer.tail_size <= 63U) {
+ 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);
+ can_send(&to_push, to_push.bus, false);
+ pos += ep3_buffer.tail_size;
+ ep3_buffer.ptr = 0;
+ 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;
+ }
+ }
+
+ while (pos < len) {
+ uint8_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(usbdata8[pos] >> 4U)];
+ if ((pos + pckt_len) <= (uint8_t)len) {
+ CANPacket_t to_push;
+ (void)memcpy(&to_push, &usbdata8[pos], pckt_len);
+ can_send(&to_push, to_push.bus, false);
+ pos += pckt_len;
+ } else {
+ (void)memcpy(ep3_buffer.data, &usbdata8[pos], len - pos);
+ ep3_buffer.ptr = len - pos;
+ ep3_buffer.tail_size = pckt_len - ep3_buffer.ptr;
+ pos += ep3_buffer.ptr;
+ }
+ }
+ }
+}
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index be9ef492d..e491f3104 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -8,6 +8,7 @@ import os
import time
import traceback
import sys
+from functools import wraps
from .dfu import PandaDFU, MCU_TYPE_F2, MCU_TYPE_F4, MCU_TYPE_H7 # pylint: disable=import-error
from .flash_release import flash_release # noqa pylint: disable=import-error
from .update import ensure_st_up_to_date # noqa pylint: disable=import-error
@@ -15,28 +16,105 @@ from .serial import PandaSerial # noqa pylint: disable=import-error
from .isotp import isotp_send, isotp_recv # pylint: disable=import-error
from .config import DEFAULT_FW_FN, DEFAULT_H7_FW_FN # noqa pylint: disable=import-error
-__version__ = '0.0.9'
+__version__ = '0.0.10'
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
DEBUG = os.getenv("PANDADEBUG") is not None
-def parse_can_buffer(dat):
- ret = []
- for j in range(0, len(dat), 0x10):
- ddat = dat[j:j + 0x10]
- f1, f2 = struct.unpack("II", ddat[0:8])
- extended = 4
- if f1 & extended:
- address = f1 >> 3
- else:
- address = f1 >> 21
- dddat = ddat[8:8 + (f2 & 0xF)]
+CANPACKET_HEAD_SIZE = 0x5
+DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64]
+LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)}
+
+def pack_can_buffer(arr):
+ snds = [b'']
+ idx = 0
+ for address, _, dat, bus in arr:
+ assert len(dat) in LEN_TO_DLC
if DEBUG:
- print(f" R 0x{address:x}: 0x{dddat.hex()}")
- ret.append((address, f2 >> 16, dddat, (f2 >> 4) & 0xFF))
+ print(f" W 0x{address:x}: 0x{dat.hex()}")
+ extended = 1 if address >= 0x800 else 0
+ data_len_code = LEN_TO_DLC[len(dat)]
+ header = bytearray(5)
+ word_4b = address << 3 | extended << 2
+ header[0] = (data_len_code << 4) | (bus << 1)
+ header[1] = word_4b & 0xFF
+ header[2] = (word_4b >> 8) & 0xFF
+ header[3] = (word_4b >> 16) & 0xFF
+ header[4] = (word_4b >> 24) & 0xFF
+ snds[idx] += header + dat
+ if len(snds[idx]) > 256: # Limit chunks to 256 bytes
+ snds.append(b'')
+ idx += 1
+
+ #Apply counter to each 64 byte packet
+ for idx in range(len(snds)):
+ tx = b''
+ counter = 0
+ for i in range (0, len(snds[idx]), 63):
+ tx += bytes([counter]) + snds[idx][i:i+63]
+ counter += 1
+ snds[idx] = tx
+ return snds
+
+def unpack_can_buffer(dat):
+ ret = []
+ counter = 0
+ tail = bytearray()
+ for i in range(0, len(dat), 64):
+ if counter != dat[i]:
+ print("CAN: LOST RECV PACKET COUNTER")
+ break
+ counter+=1
+ chunk = tail + dat[i+1:i+64]
+ tail = bytearray()
+ pos = 0
+ while pos>4)]
+ pckt_len = CANPACKET_HEAD_SIZE + data_len
+ if pckt_len <= len(chunk[pos:]):
+ header = chunk[pos:pos+CANPACKET_HEAD_SIZE]
+ if len(header) < 5:
+ print("CAN: MALFORMED USB RECV PACKET")
+ break
+ bus = (header[0] >> 1) & 0x7
+ address = (header[4] << 24 | header[3] << 16 | header[2] << 8 | header[1]) >> 3
+ returned = (header[1] >> 1) & 0x1
+ rejected = header[1] & 0x1
+ data = chunk[pos + CANPACKET_HEAD_SIZE:pos + CANPACKET_HEAD_SIZE + data_len]
+ if returned:
+ bus += 128
+ if rejected:
+ bus += 192
+ if DEBUG:
+ print(f" R 0x{address:x}: 0x{data.hex()}")
+ ret.append((address, 0, data, bus))
+ pos += pckt_len
+ else:
+ tail = chunk[pos:]
+ break
return ret
+def ensure_health_packet_version(fn):
+ @wraps(fn)
+ def wrapper(self, *args, **kwargs):
+ if self.health_version < self.HEALTH_PACKET_VERSION:
+ raise RuntimeError("Panda firmware has outdated health packet definition. Reflash panda firmware.")
+ elif self.health_version > self.HEALTH_PACKET_VERSION:
+ raise RuntimeError("Panda python library has outdated health packet definition. Update panda python library.")
+ return fn(self, *args, **kwargs)
+ return wrapper
+
+def ensure_can_packet_version(fn):
+ @wraps(fn)
+ def wrapper(self, *args, **kwargs):
+ if self.can_version < self.CAN_PACKET_VERSION:
+ raise RuntimeError("Panda firmware has outdated CAN packet definition. Reflash panda firmware.")
+ elif self.can_version > self.CAN_PACKET_VERSION:
+ raise RuntimeError("Panda python library has outdated CAN packet definition. Update panda python library.")
+ return fn(self, *args, **kwargs)
+ return wrapper
+
class PandaWifiStreaming(object):
def __init__(self, ip="192.168.0.10", port=1338):
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -55,7 +133,7 @@ class PandaWifiStreaming(object):
try:
dat, addr = self.sock.recvfrom(0x200 * 0x10)
if addr == (self.ip, self.port):
- ret += parse_can_buffer(dat)
+ ret += unpack_can_buffer(dat)
except socket.error as e:
if e.errno != 35 and e.errno != 11:
traceback.print_exc()
@@ -140,6 +218,9 @@ class Panda(object):
HW_TYPE_DOS = b'\x06'
HW_TYPE_RED_PANDA = b'\x07'
+ CAN_PACKET_VERSION = 2
+ HEALTH_PACKET_VERSION = 1
+
F2_DEVICES = [HW_TYPE_PEDAL]
F4_DEVICES = [HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS]
H7_DEVICES = [HW_TYPE_RED_PANDA]
@@ -150,7 +231,13 @@ class Panda(object):
FLAG_HONDA_ALT_BRAKE = 1
FLAG_HONDA_BOSCH_LONG = 2
+ FLAG_HONDA_NIDEC_ALT = 4
+
+ FLAG_HYUNDAI_EV_GAS = 1
+ FLAG_HYUNDAI_HYBRID_GAS = 2
FLAG_HYUNDAI_LONG = 4
+ FLAG_TESLA_POWERTRAIN = 1
+ FLAG_TESLA_LONG_CONTROL = 2
def __init__(self, serial=None, claim=True):
self._serial = serial
@@ -201,6 +288,7 @@ class Panda(object):
break
context = usb1.USBContext() # New context needed so new devices show up
assert(self._handle is not None)
+ self.health_version, self.can_version = self.get_packets_versions()
print("connected")
def reset(self, enter_bootstub=False, enter_bootloader=False):
@@ -239,6 +327,8 @@ class Panda(object):
if not success:
raise Exception("reconnect failed")
+
+
@staticmethod
def flash_static(handle, code):
# confirm flasher is present
@@ -342,6 +432,7 @@ class Panda(object):
# ******************* health *******************
+ @ensure_health_packet_version
def health(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 44)
a = struct.unpack("= 0x800:
- rir = (addr << 3) | transmit | extended
- else:
- rir = (addr << 21) | transmit
- snd = struct.pack("II", rir, len(dat) | (bus << 4)) + dat
- snd = snd.ljust(0x10, b'\x00')
- snds.append(snd)
-
+ snds = pack_can_buffer(arr)
while True:
try:
if self.wifi:
for s in snds:
self._handle.bulkWrite(3, s)
else:
- self._handle.bulkWrite(3, b''.join(snds), timeout=timeout)
+ for tx in snds:
+ while True:
+ bs = self._handle.bulkWrite(3, tx, timeout=timeout)
+ tx = tx[bs:]
+ if len(tx) == 0:
+ break
+ print("CAN: PARTIAL SEND MANY, RETRYING")
break
except (usb1.USBErrorIO, usb1.USBErrorOverflow):
print("CAN: BAD SEND MANY, RETRYING")
@@ -535,16 +639,17 @@ class Panda(object):
def can_send(self, addr, dat, bus, timeout=CAN_SEND_TIMEOUT_MS):
self.can_send_many([[addr, None, dat, bus]], timeout=timeout)
+ @ensure_can_packet_version
def can_recv(self):
dat = bytearray()
while True:
try:
- dat = self._handle.bulkRead(1, 0x10 * 256)
+ dat = self._handle.bulkRead(1, 16384) # Max receive batch size + 2 extra reserve frames
break
except (usb1.USBErrorIO, usb1.USBErrorOverflow):
print("CAN: BAD RECV, RETRYING")
time.sleep(0.1)
- return parse_can_buffer(dat)
+ return unpack_can_buffer(dat)
def can_clear(self, bus):
"""Clears all messages from the specified internal CAN ringbuffer as
diff --git a/panda/python/ccp.py b/panda/python/ccp.py
new file mode 100644
index 000000000..a8c00169f
--- /dev/null
+++ b/panda/python/ccp.py
@@ -0,0 +1,362 @@
+#!/usr/bin/env python3
+import sys
+import time
+import struct
+from enum import IntEnum, Enum
+
+class COMMAND_CODE(IntEnum):
+ CONNECT = 0x01
+ SET_MTA = 0x02
+ DNLOAD = 0x03
+ UPLOAD = 0x04
+ TEST = 0x05
+ START_STOP = 0x06
+ DISCONNECT = 0x07
+ START_STOP_ALL = 0x08
+ GET_ACTIVE_CAL_PAGE = 0x09
+ SET_S_STATUS = 0x0C
+ GET_S_STATUS = 0x0D
+ BUILD_CHKSUM = 0x0E
+ SHORT_UP = 0x0F
+ CLEAR_MEMORY = 0x10
+ SELECT_CAL_PAGE = 0x11
+ GET_SEED = 0x12
+ UNLOCK = 0x13
+ GET_DAQ_SIZE = 0x14
+ SET_DAQ_PTR = 0x15
+ WRITE_DAQ = 0x16
+ EXCHANGE_ID = 0x17
+ PROGRAM = 0x18
+ MOVE = 0x19
+ GET_CCP_VERSION = 0x1B
+ DIAG_SERVICE = 0x20
+ ACTION_SERVICE = 0x21
+ PROGRAM_6 = 0x22
+ DNLOAD_6 = 0x23
+
+COMMAND_RETURN_CODES = {
+ 0x00: "acknowledge / no error",
+ 0x01: "DAQ processor overload",
+ 0x10: "command processor busy",
+ 0x11: "DAQ processor busy",
+ 0x12: "internal timeout",
+ 0x18: "key request",
+ 0x19: "session status request",
+ 0x20: "cold start request",
+ 0x21: "cal. data init. request",
+ 0x22: "DAQ list init. request",
+ 0x23: "code update request",
+ 0x30: "unknown command",
+ 0x31: "command syntax",
+ 0x32: "parameter(s) out of range",
+ 0x33: "access denied",
+ 0x34: "overload",
+ 0x35: "access locked",
+ 0x36: "resource/function not available",
+}
+
+class BYTE_ORDER(Enum):
+ LITTLE_ENDIAN = '<'
+ BIG_ENDIAN = '>'
+
+class CommandTimeoutError(Exception):
+ pass
+
+class CommandCounterError(Exception):
+ pass
+
+class CommandResponseError(Exception):
+ def __init__(self, message, return_code):
+ super().__init__()
+ self.message = message
+ self.return_code = return_code
+
+ def __str__(self):
+ return self.message
+
+class CcpClient():
+ def __init__(self, panda, tx_addr: int, rx_addr: int, bus: int=0, byte_order: BYTE_ORDER=BYTE_ORDER.BIG_ENDIAN, debug=False):
+ self.tx_addr = tx_addr
+ self.rx_addr = rx_addr
+ self.can_bus = bus
+ self.byte_order = byte_order
+ self.debug = debug
+ self._panda = panda
+ self._command_counter = -1
+
+ def _send_cro(self, cmd: int, dat: bytes = b"") -> None:
+ self._command_counter = (self._command_counter + 1) & 0xFF
+ tx_data = (bytes([cmd, self._command_counter]) + dat).ljust(8, b"\x00")
+ if self.debug:
+ print(f"CAN-TX: {hex(self.tx_addr)} - 0x{bytes.hex(tx_data)}")
+ assert len(tx_data) == 8, "data is not 8 bytes"
+ self._panda.can_clear(self.can_bus)
+ self._panda.can_clear(0xFFFF)
+ self._panda.can_send(self.tx_addr, tx_data, self.can_bus)
+
+ def _recv_dto(self, timeout: float) -> bytes:
+ start_time = time.time()
+ while time.time() - start_time < timeout:
+ msgs = self._panda.can_recv() or []
+ if len(msgs) >= 256:
+ print("CAN RX buffer overflow!!!", file=sys.stderr)
+ for rx_addr, _, rx_data, rx_bus in msgs:
+ if rx_bus == self.can_bus and rx_addr == self.rx_addr:
+ rx_data = bytes(rx_data) # convert bytearray to bytes
+ if self.debug:
+ print(f"CAN-RX: {hex(rx_addr)} - 0x{bytes.hex(rx_data)}")
+ assert len(rx_data) == 8, f"message length not 8: {len(rx_data)}"
+
+ pid = rx_data[0]
+ if pid == 0xFF or pid == 0xFE:
+ err = rx_data[1]
+ err_desc = COMMAND_RETURN_CODES.get(err, "unknown error")
+ ctr = rx_data[2]
+ dat = rx_data[3:]
+
+ if pid == 0xFF and self._command_counter != ctr:
+ raise CommandCounterError(f"counter invalid: {ctr} != {self._command_counter}")
+
+ if err >= 0x10 and err <= 0x12:
+ if self.debug:
+ print(f"CCP-WAIT: {hex(err)} - {err_desc}")
+ start_time = time.time()
+ continue
+
+ if err >= 0x30:
+ raise CommandResponseError(f"{hex(err)} - {err_desc}", err)
+ else:
+ dat = rx_data[1:]
+
+ return dat
+ time.sleep(0.001)
+
+ raise CommandTimeoutError("timeout waiting for response")
+
+ # commands
+ def connect(self, station_addr: int) -> None:
+ if station_addr > 65535:
+ raise ValueError("station address must be less than 65536")
+ # NOTE: station address is always little endian
+ self._send_cro(COMMAND_CODE.CONNECT, struct.pack(" dict:
+ self._send_cro(COMMAND_CODE.EXCHANGE_ID, device_id_info)
+ resp = self._recv_dto(0.025)
+ return { # TODO: define a type
+ "id_length": resp[0],
+ "data_type": resp[1],
+ "available": resp[2],
+ "protected": resp[3],
+ }
+
+ def get_seed(self, resource_mask: int) -> bytes:
+ if resource_mask > 255:
+ raise ValueError("resource mask must be less than 256")
+ self._send_cro(COMMAND_CODE.GET_SEED)
+ resp = self._recv_dto(0.025)
+ # protected = resp[0] == 0
+ seed = resp[1:]
+ return seed
+
+ def unlock(self, key: bytes) -> int:
+ if len(key) > 6:
+ raise ValueError("max key size is 6 bytes")
+ self._send_cro(COMMAND_CODE.UNLOCK, key)
+ resp = self._recv_dto(0.025)
+ status = resp[0]
+ return status
+
+ def set_memory_transfer_address(self, mta_num: int, addr_ext: int, addr: int) -> None:
+ if mta_num > 255:
+ raise ValueError("MTA number must be less than 256")
+ if addr_ext > 255:
+ raise ValueError("address extension must be less than 256")
+ self._send_cro(COMMAND_CODE.SET_MTA, bytes([mta_num, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
+ self._recv_dto(0.025)
+
+ def download(self, data: bytes) -> int:
+ if len(data) > 5:
+ raise ValueError("max data size is 5 bytes")
+ self._send_cro(COMMAND_CODE.DNLOAD, bytes([len(data)]) + data)
+ resp = self._recv_dto(0.025)
+ # mta_addr_ext = resp[0]
+ mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
+ return mta_addr
+
+ def download_6_bytes(self, data: bytes) -> int:
+ if len(data) != 6:
+ raise ValueError("data size must be 6 bytes")
+ self._send_cro(COMMAND_CODE.DNLOAD_6, data)
+ resp = self._recv_dto(0.025)
+ # mta_addr_ext = resp[0]
+ mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
+ return mta_addr
+
+ def upload(self, size: int) -> bytes:
+ if size > 5:
+ raise ValueError("size must be less than 6")
+ self._send_cro(COMMAND_CODE.UPLOAD, bytes([size]))
+ return self._recv_dto(0.025)
+
+ def short_upload(self, size: int, addr_ext: int, addr: int) -> bytes:
+ if size > 5:
+ raise ValueError("size must be less than 6")
+ if addr_ext > 255:
+ raise ValueError("address extension must be less than 256")
+ self._send_cro(COMMAND_CODE.SHORT_UP, bytes([size, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
+ return self._recv_dto(0.025)
+
+ def select_calibration_page(self) -> None:
+ self._send_cro(COMMAND_CODE.SELECT_CAL_PAGE)
+ self._recv_dto(0.025)
+
+ def get_daq_list_size(self, list_num: int, can_id: int = 0) -> dict:
+ if list_num > 255:
+ raise ValueError("list number must be less than 256")
+ self._send_cro(COMMAND_CODE.GET_DAQ_SIZE, bytes([list_num, 0]) + struct.pack(f"{self.byte_order.value}I", can_id))
+ resp = self._recv_dto(0.025)
+ return { # TODO: define a type
+ "list_size": resp[0],
+ "first_pid": resp[1],
+ }
+
+ def set_daq_list_pointer(self, list_num: int, odt_num: int, element_num: int) -> None:
+ if list_num > 255:
+ raise ValueError("list number must be less than 256")
+ if odt_num > 255:
+ raise ValueError("ODT number must be less than 256")
+ if element_num > 255:
+ raise ValueError("element number must be less than 256")
+ self._send_cro(COMMAND_CODE.SET_DAQ_PTR, bytes([list_num, odt_num, element_num]))
+ self._recv_dto(0.025)
+
+ def write_daq_list_entry(self, size: int, addr_ext: int, addr: int) -> None:
+ if size > 255:
+ raise ValueError("size must be less than 256")
+ if addr_ext > 255:
+ raise ValueError("address extension must be less than 256")
+ self._send_cro(COMMAND_CODE.WRITE_DAQ, bytes([size, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
+ self._recv_dto(0.025)
+
+ def start_stop_transmission(self, mode: int, list_num: int, odt_num: int, channel_num: int, rate_prescaler: int = 0) -> None:
+ if mode > 255:
+ raise ValueError("mode must be less than 256")
+ if list_num > 255:
+ raise ValueError("list number must be less than 256")
+ if odt_num > 255:
+ raise ValueError("ODT number must be less than 256")
+ if channel_num > 255:
+ raise ValueError("channel number must be less than 256")
+ if rate_prescaler > 65535:
+ raise ValueError("rate prescaler must be less than 65536")
+ self._send_cro(COMMAND_CODE.START_STOP, bytes([mode, list_num, odt_num, channel_num]) + struct.pack(f"{self.byte_order.value}H", rate_prescaler))
+ self._recv_dto(0.025)
+
+ def disconnect(self, station_addr: int, temporary: bool = False) -> None:
+ if station_addr > 65535:
+ raise ValueError("station address must be less than 65536")
+ # NOTE: station address is always little endian
+ self._send_cro(COMMAND_CODE.DISCONNECT, bytes([int(not temporary), 0x00]) + struct.pack(" None:
+ if status > 255:
+ raise ValueError("status must be less than 256")
+ self._send_cro(COMMAND_CODE.SET_S_STATUS, bytes([status]))
+ self._recv_dto(0.025)
+
+ def get_session_status(self) -> dict:
+ self._send_cro(COMMAND_CODE.GET_S_STATUS)
+ resp = self._recv_dto(0.025)
+ return { # TODO: define a type
+ "status": resp[0],
+ "info": resp[2] if resp[1] else None,
+ }
+
+ def build_checksum(self, size: int) -> bytes:
+ self._send_cro(COMMAND_CODE.BUILD_CHKSUM, struct.pack(f"{self.byte_order.value}I", size))
+ resp = self._recv_dto(30.0)
+ chksum_size = resp[0]
+ assert chksum_size <= 4, "checksum more than 4 bytes"
+ chksum = resp[1:1+chksum_size]
+ return chksum
+
+ def clear_memory(self, size: int) -> None:
+ self._send_cro(COMMAND_CODE.CLEAR_MEMORY, struct.pack(f"{self.byte_order.value}I", size))
+ self._recv_dto(30.0)
+
+ def program(self, size: int, data: bytes) -> int:
+ if size > 5:
+ raise ValueError("size must be less than 6")
+ if len(data) > 5:
+ raise ValueError("max data size is 5 bytes")
+ self._send_cro(COMMAND_CODE.PROGRAM, bytes([size]) + data)
+ resp = self._recv_dto(0.1)
+ # mta_addr_ext = resp[0]
+ mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
+ return mta_addr
+
+ def program_6_bytes(self, data: bytes) -> int:
+ if len(data) != 6:
+ raise ValueError("data size must be 6 bytes")
+ self._send_cro(COMMAND_CODE.PROGRAM_6, data)
+ resp = self._recv_dto(0.1)
+ # mta_addr_ext = resp[0]
+ mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
+ return mta_addr
+
+ def move_memory_block(self, size: int) -> None:
+ self._send_cro(COMMAND_CODE.MOVE, struct.pack(f"{self.byte_order.value}I", size))
+ self._recv_dto(0.025)
+
+ def diagnostic_service(self, service_num: int, data: bytes = b"") -> dict:
+ if service_num > 65535:
+ raise ValueError("service number must be less than 65536")
+ if len(data) > 4:
+ raise ValueError("max data size is 4 bytes")
+ self._send_cro(COMMAND_CODE.DIAG_SERVICE, struct.pack(f"{self.byte_order.value}H", service_num) + data)
+ resp = self._recv_dto(0.025)
+ return { # TODO: define a type
+ "length": resp[0],
+ "type": resp[1],
+ }
+
+ def action_service(self, service_num: int, data: bytes = b"") -> dict:
+ if service_num > 65535:
+ raise ValueError("service number must be less than 65536")
+ if len(data) > 4:
+ raise ValueError("max data size is 4 bytes")
+ self._send_cro(COMMAND_CODE.ACTION_SERVICE, struct.pack(f"{self.byte_order.value}H", service_num) + data)
+ resp = self._recv_dto(0.025)
+ return { # TODO: define a type
+ "length": resp[0],
+ "type": resp[1],
+ }
+
+ def test_availability(self, station_addr: int) -> None:
+ if station_addr > 65535:
+ raise ValueError("station address must be less than 65536")
+ # NOTE: station address is always little endian
+ self._send_cro(COMMAND_CODE.TEST, struct.pack(" None:
+ if mode > 255:
+ raise ValueError("mode must be less than 256")
+ self._send_cro(COMMAND_CODE.START_STOP_ALL, bytes([mode]))
+ self._recv_dto(0.025)
+
+ def get_active_calibration_page(self):
+ self._send_cro(COMMAND_CODE.GET_ACTIVE_CAL_PAGE)
+ resp = self._recv_dto(0.025)
+ # cal_addr_ext = resp[0]
+ cal_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
+ return cal_addr
+
+ def get_version(self, desired_version: float = 2.1) -> float:
+ major, minor = map(int, str(desired_version).split("."))
+ self._send_cro(COMMAND_CODE.GET_CCP_VERSION, bytes([major, minor]))
+ resp = self._recv_dto(0.025)
+ return float(f"{resp[0]}.{resp[1]}")
diff --git a/panda/python/uds.py b/panda/python/uds.py
index 48fcf5322..d0119612c 100644
--- a/panda/python/uds.py
+++ b/panda/python/uds.py
@@ -586,13 +586,16 @@ class UdsClient():
power_down_time = resp[0]
return power_down_time
- def security_access(self, access_type: ACCESS_TYPE, security_key: bytes = None):
+ def security_access(self, access_type: ACCESS_TYPE, security_key: bytes = b'', data_record: bytes = b''):
request_seed = access_type % 2 != 0
- if request_seed and security_key is not None:
+ if request_seed and len(security_key) != 0:
raise ValueError('security_key not allowed')
- if not request_seed and security_key is None:
+ if not request_seed and len(security_key) == 0:
raise ValueError('security_key is missing')
- resp = self._uds_request(SERVICE_TYPE.SECURITY_ACCESS, subfunction=access_type, data=security_key)
+ if not request_seed and len(data_record) != 0:
+ raise ValueError('data_record not allowed')
+ data = security_key + data_record
+ resp = self._uds_request(SERVICE_TYPE.SECURITY_ACCESS, subfunction=access_type, data=data)
if request_seed:
security_seed = resp
return security_seed
@@ -792,7 +795,7 @@ class UdsClient():
return resp
def input_output_control_by_identifier(self, data_identifier_type: DATA_IDENTIFIER_TYPE, control_parameter_type: CONTROL_PARAMETER_TYPE,
- control_option_record: bytes, control_enable_mask_record: bytes = b''):
+ control_option_record: bytes = b'', control_enable_mask_record: bytes = b''):
data = struct.pack('!H', data_identifier_type) + bytes([control_parameter_type]) + control_option_record + control_enable_mask_record
resp = self._uds_request(SERVICE_TYPE.INPUT_OUTPUT_CONTROL_BY_IDENTIFIER, subfunction=None, data=data)
resp_id = struct.unpack('!H', resp[0:2])[0] if len(resp) >= 2 else None
diff --git a/pyextra/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json
index f4e332351..16fd45233 100644
--- a/pyextra/acados_template/acados_layout.json
+++ b/pyextra/acados_template/acados_layout.json
@@ -666,6 +666,9 @@
"nlp_solver_type": [
"str"
],
+ "collocation_type": [
+ "str"
+ ],
"globalization": [
"str"
],
diff --git a/pyextra/acados_template/acados_model.py b/pyextra/acados_template/acados_model.py
index 379ade0d9..4871a2c0d 100644
--- a/pyextra/acados_template/acados_model.py
+++ b/pyextra/acados_template/acados_model.py
@@ -96,6 +96,9 @@ class AcadosModel():
self.cost_expr_ext_cost = None #: CasADi expression for external cost; Default: :code:`None`
self.cost_expr_ext_cost_e = None #: CasADi expression for external cost, terminal; Default: :code:`None`
self.cost_expr_ext_cost_0 = None #: CasADi expression for external cost, initial; Default: :code:`None`
+ self.cost_expr_ext_cost_custom_hess = None #: CasADi expression for custom hessian (only for external cost); Default: :code:`None`
+ self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None`
+ self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None`
def acados_model_strip_casadi_symbolics(model):
@@ -147,5 +150,11 @@ def acados_model_strip_casadi_symbolics(model):
del out['cost_expr_ext_cost_e']
if 'cost_expr_ext_cost_0' in out.keys():
del out['cost_expr_ext_cost_0']
+ if 'cost_expr_ext_cost_custom_hess' in out.keys():
+ del out['cost_expr_ext_cost_custom_hess']
+ if 'cost_expr_ext_cost_custom_hess_e' in out.keys():
+ del out['cost_expr_ext_cost_custom_hess_e']
+ if 'cost_expr_ext_cost_custom_hess_0' in out.keys():
+ del out['cost_expr_ext_cost_custom_hess_0']
return out
diff --git a/pyextra/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py
index 9c2f02aa8..198ab033d 100644
--- a/pyextra/acados_template/acados_ocp.py
+++ b/pyextra/acados_template/acados_ocp.py
@@ -2120,6 +2120,7 @@ class AcadosOcpOptions:
self.__globalization = 'FIXED_STEP'
self.__nlp_solver_step_length = 1.0 # fixed Newton step length
self.__levenberg_marquardt = 0.0
+ self.__collocation_type = 'GAUSS_LEGENDRE'
self.__sim_method_num_stages = 4 # number of stages in the integrator
self.__sim_method_num_steps = 1 # number of steps in the integrator
self.__sim_method_newton_iter = 3 # number of Newton iterations in simulation method
@@ -2195,6 +2196,15 @@ class AcadosOcpOptions:
"""
return self.__globalization
+ @property
+ def collocation_type(self):
+ """Collocation type: relevant for implicit integrators
+ -- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}.
+
+ Default: GAUSS_LEGENDRE
+ """
+ return self.__collocation_type
+
@property
def regularize_method(self):
"""Regularization method for the Hessian.
@@ -2481,6 +2491,15 @@ class AcadosOcpOptions:
raise Exception('Invalid regularize_method value. Possible values are:\n\n' \
+ ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.')
+ @collocation_type.setter
+ def collocation_type(self, collocation_type):
+ collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE')
+ if collocation_type in collocation_types:
+ self.__collocation_type = collocation_type
+ else:
+ raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
+ + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.')
+
@hessian_approx.setter
def hessian_approx(self, hessian_approx):
hessian_approxs = ('GAUSS_NEWTON', 'EXACT')
diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py
index 4e80e1205..b34b8fa84 100644
--- a/pyextra/acados_template/acados_ocp_solver.py
+++ b/pyextra/acados_template/acados_ocp_solver.py
@@ -53,7 +53,7 @@ from .acados_ocp import AcadosOcp
from .acados_model import acados_model_strip_casadi_symbolics
from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
- set_up_imported_gnsf_model, get_acados_path
+ set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path
def make_ocp_dims_consistent(acados_ocp):
@@ -102,6 +102,7 @@ def make_ocp_dims_consistent(acados_ocp):
cost.cost_ext_fun_type_0 = cost.cost_ext_fun_type
model.cost_y_expr_0 = model.cost_y_expr
model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost
+ model.cost_expr_ext_cost_custom_hess_0 = model.cost_expr_ext_cost_custom_hess
if cost.cost_type_0 == 'LINEAR_LS':
ny_0 = cost.W_0.shape[0]
@@ -433,9 +434,19 @@ def make_ocp_dims_consistent(acados_ocp):
if np.shape(opts.shooting_nodes)[0] != dims.N+1:
raise Exception('inconsistent dimension N, regarding shooting_nodes.')
+ # time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
+ # # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
+ # delta_time_steps = time_steps[1:] - time_steps[0:-1]
+ # avg_time_steps = np.average(time_steps)
+ # # criterion for constant time-step detection: the min/max difference in values normalized by the average
+ # check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
+ # # if the criterion is small, we have a constant time-step
+ # if check_const_time_step < 1e-9:
+ # time_steps[:] = avg_time_steps # if we have a constant time-step: apply the average time-step
time_steps = np.zeros((dims.N,))
for i in range(dims.N):
- time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i]
+ time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above
+
opts.time_steps = time_steps
elif (not is_empty(opts.time_steps)) and (not is_empty(opts.shooting_nodes)):
@@ -483,13 +494,12 @@ def make_ocp_dims_consistent(acados_ocp):
raise Exception("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).")
-
-def get_ocp_nlp_layout():
- current_module = sys.modules[__name__]
- acados_path = os.path.dirname(current_module.__file__)
- with open(acados_path + '/acados_layout.json', 'r') as f:
- ocp_nlp_layout = json.load(f)
- return ocp_nlp_layout
+def get_simulink_default_opts():
+ python_interface_path = get_python_interface_path()
+ abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
+ with open(abs_path , 'r') as f:
+ simulink_default_opts = json.load(f)
+ return simulink_default_opts
def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'):
@@ -622,9 +632,7 @@ def ocp_render_templates(acados_ocp, json_file):
name = acados_ocp.model.name
# setting up loader and environment
- json_path = '{cwd}/{json_file}'.format(
- cwd=os.getcwd(),
- json_file=json_file)
+ json_path = os.path.join(os.getcwd(), json_file)
if not os.path.exists(json_path):
raise Exception('{} not found!'.format(json_path))
@@ -645,6 +653,10 @@ def ocp_render_templates(acados_ocp, json_file):
out_file = f'acados_solver_{name}.h'
render_template(in_file, out_file, template_dir, json_path)
+ in_file = 'acados_solver.in.pxd'
+ out_file = f'acados_solver.pxd'
+ render_template(in_file, out_file, template_dir, json_path)
+
in_file = 'Makefile.in'
out_file = 'Makefile'
render_template(in_file, out_file, template_dir, json_path)
@@ -671,8 +683,7 @@ def ocp_render_templates(acados_ocp, json_file):
render_template(in_file, out_file, template_dir, json_path)
## folder model
- template_dir = f'{code_export_dir}/{name}_model/'
-
+ template_dir = os.path.join(code_export_dir, name + '_model')
in_file = 'model.in.h'
out_file = f'{name}_model.h'
render_template(in_file, out_file, template_dir, json_path)
@@ -680,7 +691,7 @@ def ocp_render_templates(acados_ocp, json_file):
# constraints on convex over nonlinear function
if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0:
# constraints on outer function
- template_dir = f'{code_export_dir}/{name}_constraints/'
+ template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'phi_constraint.in.h'
out_file = f'{name}_phi_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
@@ -688,62 +699,62 @@ def ocp_render_templates(acados_ocp, json_file):
# terminal constraints on convex over nonlinear function
if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0:
# terminal constraints on outer function
- template_dir = f'{code_export_dir}/{name}_constraints/'
+ template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'phi_e_constraint.in.h'
out_file = f'{name}_phi_e_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
# nonlinear constraints
if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0:
- template_dir = f'{code_export_dir}/{name}_constraints/'
+ template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'h_constraint.in.h'
out_file = f'{name}_h_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
# terminal nonlinear constraints
if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0:
- template_dir = f'{code_export_dir}/{name}_constraints/'
+ template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'h_e_constraint.in.h'
out_file = f'{name}_h_e_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
# initial stage Nonlinear LS cost function
if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
- template_dir = f'{code_export_dir}/{name}_cost/'
+ template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'cost_y_0_fun.in.h'
out_file = f'{name}_cost_y_0_fun.h'
render_template(in_file, out_file, template_dir, json_path)
# external cost - terminal
elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
- template_dir = f'{code_export_dir}/{name}_cost/'
+ template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'external_cost_0.in.h'
out_file = f'{name}_external_cost_0.h'
render_template(in_file, out_file, template_dir, json_path)
# path Nonlinear LS cost function
if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
- template_dir = f'{code_export_dir}/{name}_cost/'
+ template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'cost_y_fun.in.h'
out_file = f'{name}_cost_y_fun.h'
render_template(in_file, out_file, template_dir, json_path)
# terminal Nonlinear LS cost function
if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
- template_dir = f'{code_export_dir}/{name}_cost/'
+ template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'cost_y_e_fun.in.h'
out_file = f'{name}_cost_y_e_fun.h'
render_template(in_file, out_file, template_dir, json_path)
# external cost
if acados_ocp.cost.cost_type == 'EXTERNAL':
- template_dir = f'{code_export_dir}/{name}_cost/'
+ template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'external_cost.in.h'
out_file = f'{name}_external_cost.h'
render_template(in_file, out_file, template_dir, json_path)
# external cost - terminal
if acados_ocp.cost.cost_type_e == 'EXTERNAL':
- template_dir = f'{code_export_dir}/{name}_cost/'
+ template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'external_cost_e.in.h'
out_file = f'{name}_external_cost_e.h'
render_template(in_file, out_file, template_dir, json_path)
@@ -775,9 +786,7 @@ class AcadosOcpSolver:
model = acados_ocp.model
if simulink_opts is None:
- json_path = os.path.dirname(os.path.realpath(__file__))
- with open(json_path + '/simulink_default_opts.json', 'r') as f:
- simulink_opts = json.load(f)
+ simulink_opts = get_simulink_default_opts()
# make dims consistent
make_ocp_dims_consistent(acados_ocp)
@@ -830,6 +839,14 @@ class AcadosOcpSolver:
assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0
self.solver_created = True
+ # get pointers solver
+ self.__get_pointers_solver()
+
+
+ def __get_pointers_solver(self):
+ """
+ Private function to get the pointers for solver
+ """
# get pointers solver
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p
@@ -863,14 +880,10 @@ class AcadosOcpSolver:
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
- self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
- [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
- self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
- [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
@@ -881,15 +894,6 @@ class AcadosOcpSolver:
self.shared_lib.ocp_nlp_set.argtypes = \
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
- self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
- [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
- self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
- self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
- [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
- self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
- [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-
-
def solve(self):
"""
Solve the ocp with current input.
@@ -901,46 +905,118 @@ class AcadosOcpSolver:
return status
- def get_slice(self, start_stage_, end_stage_, field_):
- dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
- self.nlp_dims, self.nlp_out, start_stage_, field_)
- out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
- self.fill_in_slice(start_stage_, end_stage_, field_, out)
- return out
+ def set_new_time_steps(self, new_time_steps):
+ """
+ Set new time steps before solving. Only reload library without code generation but with new time steps.
- def fill_in_slice(self, start_stage_, end_stage_, field_, arr):
- out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
- mem_fields = ['sl', 'su']
+ :param new_time_steps: vector of new time steps for the solver
+
+ .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
+ the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
+ do not require a new code export and compilation.
+ """
+
+ # unlikely but still possible
+ if not self.solver_created:
+ raise Exception('Solver was not yet created!')
+
+ # check if time steps really changed in value
+ if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
+ return
+
+ N = new_time_steps.size
+ model = self.acados_ocp.model
+ new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))
+
+ # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
+ if self.acados_ocp.solver_options.time_steps.size == N:
+ getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
+ getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
+ assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
+ else: # recreate the solver with the new time steps
+ self.solver_created = False
+
+ # delete old memory (analog to __del__)
+ getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p]
+ getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
+ getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)
+
+ # store N and new time steps
+ self.N = self.acados_ocp.dims.N = N
+ self.acados_ocp.solver_options.time_steps = new_time_steps
+ self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]
+
+ # create solver with new time steps
+ getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
+ getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
+ assert getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization")(self.capsule, N, new_time_steps_data) == 0
+
+ self.solver_created = True
+
+ # get pointers solver
+ self.__get_pointers_solver()
+
+
+ def get(self, stage_, field_):
+ """
+ Get the last solution of the solver:
+
+ :param stage: integer corresponding to shooting node
+ :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
+
+ .. note:: regarding lam, t: \n
+ the inequalities are internally organized in the following order: \n
+ [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
+ lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+ .. note:: pi: multipliers for dynamics equality constraints \n
+ lam: multipliers for inequalities \n
+ t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
+ sl: slack variables of soft lower inequality constraints \n
+ su: slack variables of soft upper inequality constraints \n
+ """
+
+ out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
+ # mem_fields = ['sl', 'su']
field = field_
field = field.encode('utf-8')
- if (field_ not in out_fields + mem_fields):
- raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
+ if (field_ not in out_fields):
+ raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
\n Possible values are {}. Exiting.'.format(field_, out_fields))
- if not isinstance(start_stage_, int):
- raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
+ if not isinstance(stage_, int):
+ raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
- if not isinstance(end_stage_, int):
- raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
+ if stage_ < 0 or stage_ > self.N:
+ raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
- if start_stage_ >= end_stage_:
- raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
+ if stage_ == self.N and field_ == 'pi':
+ raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
+ .format(field_, stage_))
- if start_stage_ < 0 or end_stage_ > self.N + 1:
- raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
+ self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
+ [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+ self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
- out_data = cast(arr.ctypes.data, POINTER(c_double))
+ dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage_, field)
+
+ out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64)
+ out_data = cast(out.ctypes.data, POINTER(c_double))
if (field_ in out_fields):
- self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
- self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
- elif field_ in mem_fields:
- self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
- self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
+ self.shared_lib.ocp_nlp_out_get.argtypes = \
+ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+ self.shared_lib.ocp_nlp_out_get(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage_, field, out_data)
+ # elif field_ in mem_fields:
+ # self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
+ # [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+ # self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
+ # self.nlp_dims, self.nlp_solver, stage_, field, out_data)
- def get(self, stage_, field_):
- return self.get_slice(stage_, stage_ + 1, field_)[0]
+ return out
def print_statistics(self):
@@ -1176,8 +1252,7 @@ class AcadosOcpSolver:
"""
cost_fields = ['y_ref', 'yref']
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
- out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
- mem_fields = ['sl', 'su']
+ out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
# cast value_ to avoid conversion issues
if isinstance(value_, (float, int)):
@@ -1189,47 +1264,52 @@ class AcadosOcpSolver:
stage = c_int(stage_)
- if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
- raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
- \nPossible values are {}. Exiting.".format(field, \
- constraints_fields + cost_fields + out_fields + ['p']))
+ # treat parameters separately
+ if field_ == 'p':
+ getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
+ getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
- self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
- [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
- self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+ value_data = cast(value_.ctypes.data, POINTER(c_double))
- dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
- self.nlp_dims, self.nlp_out, stage_, field)
+ assert getattr(self.shared_lib, f"{self.model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
+ else:
+ if field_ not in constraints_fields + cost_fields + out_fields:
+ raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
+ \nPossible values are {}. Exiting.".format(field, \
+ constraints_fields + cost_fields + out_fields + ['p']))
- if value_.shape[0] != dims:
- msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
- msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
- raise Exception(msg)
+ self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
+ [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+ self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+
+ dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage_, field)
+
+ if value_.shape[0] != dims:
+ msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+ msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
+ raise Exception(msg)
+
+ value_data = cast(value_.ctypes.data, POINTER(c_double))
+ value_data_p = cast((value_data), c_void_p)
+
+ if field_ in constraints_fields:
+ self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
+ self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+ elif field_ in cost_fields:
+ self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
+ self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+ elif field_ in out_fields:
+ self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage, field, value_data_p)
+ # elif field_ in mem_fields:
+ # self.shared_lib.ocp_nlp_set(self.nlp_config, \
+ # self.nlp_solver, stage, field, value_data_p)
- value_data_p = cast(value_.ctypes.data, c_void_p)
- if field_ in constraints_fields:
- self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
- self.nlp_dims, self.nlp_in, stage, field, value_data_p)
- elif field_ in cost_fields:
- self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
- self.nlp_dims, self.nlp_in, stage, field, value_data_p)
- elif field_ in out_fields:
- self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
- self.nlp_dims, self.nlp_out, stage, field, value_data_p)
- elif field_ in mem_fields:
- self.shared_lib.ocp_nlp_set(self.nlp_config, \
- self.nlp_solver, stage, field, value_data_p)
return
- def set_param(self, stage_, value_):
- value_data = cast(value_.ctypes.data, POINTER(c_double))
- self._set_param(self.capsule, stage_, value_data, value_.shape[0])
-
- def cost_set(self, start_stage_, field_, value_, api='warn'):
- self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-
- def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
+ def cost_set(self, stage_, field_, value_, api='warn'):
"""
Set numerical data in the cost module of the solver.
@@ -1238,39 +1318,133 @@ class AcadosOcpSolver:
:param value: of appropriate size
"""
# cast value_ to avoid conversion issues
- field = field_.encode('utf-8')
- if len(value_.shape) > 2:
- dim = value_.shape[1]*value_.shape[2]
- else:
- dim = value_.shape[1]
+ if isinstance(value_, (float, int)):
+ value_ = np.array([value_])
+ value_ = value_.astype(float)
- self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \
- self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
- cast(value_.ctypes.data, c_void_p), dim)
+ field = field_
+ field = field.encode('utf-8')
+
+ stage = c_int(stage_)
+ self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
+ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
+ self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
+
+ dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+ dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+ self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage_, field, dims_data)
+
+ value_shape = value_.shape
+ if len(value_shape) == 1:
+ value_shape = (value_shape[0], 0)
+
+ elif len(value_shape) == 2:
+ if api=='old':
+ pass
+ elif api=='warn':
+ if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
+ raise Exception("Ambiguity in API detected.\n"
+ "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
+ "Are you seeing this error suddenly in previously running code? Read on.\n"
+ " You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
+ " acados_template now correctly passes on any matrices to acados in column major format.\n" +
+ " Two options to fix this error: \n" +
+ " * Add api='old' to cost_set to restore old incorrect behaviour\n" +
+ " * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
+ "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
+ "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
+ # Get elements in column major order
+ value_ = np.ravel(value_, order='F')
+ elif api=='new':
+ # Get elements in column major order
+ value_ = np.ravel(value_, order='F')
+ else:
+ raise Exception("Unknown api: '{}'".format(api))
+
+ if value_shape != tuple(dims):
+ raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
+ ' for field "{}" with dimension {} (you have {})'.format( \
+ field_, tuple(dims), value_shape))
+
+ value_data = cast(value_.ctypes.data, POINTER(c_double))
+ value_data_p = cast((value_data), c_void_p)
+
+ self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
+ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+ self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
+ self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+
+ return
- def constraints_set(self, start_stage_, field_, value_, api='warn'):
- self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-
-
- def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
+ def constraints_set(self, stage_, field_, value_, api='warn'):
"""
Set numerical data in the constraint module of the solver.
:param stage: integer corresponding to shooting node
- :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
+ :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D']
:param value: of appropriate size
"""
+ # cast value_ to avoid conversion issues
+ if isinstance(value_, (float, int)):
+ value_ = np.array([value_])
+ value_ = value_.astype(float)
- field = field_.encode('utf-8')
- if len(value_.shape) > 2:
- dim = value_.shape[1]*value_.shape[2]
- else:
- dim = value_.shape[1]
+ field = field_
+ field = field.encode('utf-8')
- self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
- self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
- cast(value_.ctypes.data, c_void_p), dim)
+ stage = c_int(stage_)
+ self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
+ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
+ self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
+
+ dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+ dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+ self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage_, field, dims_data)
+
+ value_shape = value_.shape
+ if len(value_shape) == 1:
+ value_shape = (value_shape[0], 0)
+ elif len(value_shape) == 2:
+ if api=='old':
+ pass
+ elif api=='warn':
+ if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
+ raise Exception("Ambiguity in API detected.\n"
+ "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
+ "Are you seeing this error suddenly in previously running code? Read on.\n"
+ " You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
+ " acados_template now correctly passes on any matrices to acados in column major format.\n" +
+ " Two options to fix this error: \n" +
+ " * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
+ " * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
+ "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
+ "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
+ # Get elements in column major order
+ value_ = np.ravel(value_, order='F')
+ elif api=='new':
+ # Get elements in column major order
+ value_ = np.ravel(value_, order='F')
+ else:
+ raise Exception("Unknown api: '{}'".format(api))
+
+ if value_shape != tuple(dims):
+ raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
+ ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+
+ value_data = cast(value_.ctypes.data, POINTER(c_double))
+ value_data_p = cast((value_data), c_void_p)
+
+ self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
+ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+ self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
+ self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+
+ return
def dynamics_get(self, stage_, field_):
diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx
new file mode 100644
index 000000000..b950d880b
--- /dev/null
+++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx
@@ -0,0 +1,427 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+# cython: language_level=3
+# cython: profile=False
+# distutils: language=c
+
+cimport cython
+from libc cimport string
+
+cimport acados_solver_common
+cimport acados_solver
+
+cimport numpy as cnp
+
+import os
+import numpy as np
+
+
+cdef class AcadosOcpSolverFast:
+ """
+ Class to interact with the acados ocp solver C object.
+
+ :param acados_ocp: type AcadosOcp - description of the OCP for acados
+ :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
+ :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
+ """
+
+ cdef acados_solver.nlp_solver_capsule *capsule
+ cdef void *nlp_opts
+ cdef acados_solver_common.ocp_nlp_dims *nlp_dims
+ cdef acados_solver_common.ocp_nlp_config *nlp_config
+ cdef acados_solver_common.ocp_nlp_out *nlp_out
+ cdef acados_solver_common.ocp_nlp_in *nlp_in
+ cdef acados_solver_common.ocp_nlp_solver *nlp_solver
+
+ cdef str model_name
+ cdef int N
+ cdef bint solver_created
+
+ def __cinit__(self, str model_name, int N, str code_export_dir):
+ self.model_name = model_name
+ self.N = N
+
+ self.solver_created = False
+
+ # create capsule
+ self.capsule = acados_solver.acados_create_capsule()
+
+ # create solver
+ assert acados_solver.acados_create(self.capsule) == 0
+ self.solver_created = True
+
+ # get pointers solver
+ self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
+ self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
+ self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
+ self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
+ self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
+ self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
+
+
+ def solve(self):
+ """
+ Solve the ocp with current input.
+ """
+ return acados_solver.acados_solve(self.capsule)
+
+
+ def set_new_time_steps(self, new_time_steps):
+ """
+ Set new time steps before solving. Only reload library without code generation but with new time steps.
+
+ :param new_time_steps: vector of new time steps for the solver
+
+ .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
+ the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
+ do not require a new code export and compilation.
+ """
+ raise NotImplementedError()
+
+
+ def get(self, int stage, str field_):
+ """
+ Get the last solution of the solver:
+
+ :param stage: integer corresponding to shooting node
+ :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
+
+ .. note:: regarding lam, t: \n
+ the inequalities are internally organized in the following order: \n
+ [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
+ lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+ .. note:: pi: multipliers for dynamics equality constraints \n
+ lam: multipliers for inequalities \n
+ t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
+ sl: slack variables of soft lower inequality constraints \n
+ su: slack variables of soft upper inequality constraints \n
+ """
+
+ out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
+ field = field_.encode('utf-8')
+
+ if field_ not in out_fields:
+ raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
+ \n Possible values are {}. Exiting.'.format(field_, out_fields))
+
+ if stage < 0 or stage > self.N:
+ raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
+
+ if stage == self.N and field_ == 'pi':
+ raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
+ .format(field_, stage))
+
+ cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
+ self.nlp_dims, self.nlp_out, stage, field)
+
+ cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims,))
+ acados_solver_common.ocp_nlp_out_get(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage, field, out.data)
+
+ return out
+
+
+ def print_statistics(self):
+ """
+ prints statistics of previous solver run as a table:
+ - iter: iteration number
+ - res_stat: stationarity residual
+ - res_eq: residual wrt equality constraints (dynamics)
+ - res_ineq: residual wrt inequality constraints (constraints)
+ - res_comp: residual wrt complementarity conditions
+ - qp_stat: status of QP solver
+ - qp_iter: number of QP iterations
+ - qp_res_stat: stationarity residual of the last QP solution
+ - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
+ - qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution
+ - qp_res_comp: residual wrt complementarity conditions of the last QP solution
+ """
+ raise NotImplementedError()
+
+
+ def store_iterate(self, filename='', overwrite=False):
+ """
+ Stores the current iterate of the ocp solver in a json file.
+
+ :param filename: if not set, use model_name + timestamp + '.json'
+ :param overwrite: if false and filename exists add timestamp to filename
+ """
+ raise NotImplementedError()
+
+
+ def load_iterate(self, filename):
+ """
+ Loads the iterate stored in json file with filename into the ocp solver.
+ """
+ raise NotImplementedError()
+
+
+ def get_stats(self, field_):
+ """
+ Get the information of the last solver call.
+
+ :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+ """
+ raise NotImplementedError()
+
+
+ def get_cost(self):
+ """
+ Returns the cost value of the current solution.
+ """
+ # compute cost internally
+ acados_solver_common.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+ # create output
+ cdef double out
+
+ # call getter
+ acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, "cost_value", &out)
+
+ return out
+
+
+ def get_residuals(self):
+ """
+ Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
+ """
+ raise NotImplementedError()
+
+
+ # Note: this function should not be used anymore, better use cost_set, constraints_set
+ def set(self, int stage, str field_, value_):
+
+ """
+ Set numerical data inside the solver.
+
+ :param stage: integer corresponding to shooting node
+ :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
+
+ .. note:: regarding lam, t: \n
+ the inequalities are internally organized in the following order: \n
+ [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
+ lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+ .. note:: pi: multipliers for dynamics equality constraints \n
+ lam: multipliers for inequalities \n
+ t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
+ sl: slack variables of soft lower inequality constraints \n
+ su: slack variables of soft upper inequality constraints \n
+ """
+ cost_fields = ['y_ref', 'yref']
+ constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
+ out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
+
+ field = field_.encode('utf-8')
+
+ cdef double[::1] value
+
+ # treat parameters separately
+ if field_ == 'p':
+ value = np.ascontiguousarray(value_, dtype=np.double)
+ assert acados_solver.acados_update_params(self.capsule, stage, &value[0], value.shape[0]) == 0
+ else:
+ if field_ not in constraints_fields + cost_fields + out_fields:
+ raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
+ \nPossible values are {}. Exiting.".format(field, \
+ constraints_fields + cost_fields + out_fields + ['p']))
+
+ dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
+ self.nlp_dims, self.nlp_out, stage, field)
+
+ if value_.shape[0] != dims:
+ msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+ msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
+ raise Exception(msg)
+
+ value = np.ascontiguousarray(value_, dtype=np.double)
+ if field_ in constraints_fields:
+ acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
+ self.nlp_dims, self.nlp_in, stage, field, &value[0])
+ elif field_ in cost_fields:
+ acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
+ self.nlp_dims, self.nlp_in, stage, field, &value[0])
+ elif field_ in out_fields:
+ acados_solver_common.ocp_nlp_out_set(self.nlp_config,
+ self.nlp_dims, self.nlp_out, stage, field, &value[0])
+
+
+ def cost_set(self, int stage, str field_, value_):
+ """
+ Set numerical data in the cost module of the solver.
+
+ :param stage: integer corresponding to shooting node
+ :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
+ :param value: of appropriate size
+ """
+ field = field_.encode('utf-8')
+
+ cdef int dims[2]
+ acados_solver_common.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage, field, &dims[0])
+
+ cdef double[::1,:] value
+
+ value_shape = value_.shape
+ if len(value_shape) == 1:
+ value_shape = (value_shape[0], 0)
+ value = np.asfortranarray(value_[None,:])
+
+ elif len(value_shape) == 2:
+ # Get elements in column major order
+ value = np.asfortranarray(value_)
+
+ if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
+ raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
+ ' for field "{}" with dimension {} (you have {})'.format( \
+ field_, tuple(dims), value_shape))
+
+ acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
+ self.nlp_dims, self.nlp_in, stage, field, &value[0][0])
+
+
+ def constraints_set(self, int stage, str field_, value_):
+ """
+ Set numerical data in the constraint module of the solver.
+
+ :param stage: integer corresponding to shooting node
+ :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D']
+ :param value: of appropriate size
+ """
+ field = field_.encode('utf-8')
+
+ cdef int dims[2]
+ acados_solver_common.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
+ self.nlp_dims, self.nlp_out, stage, field, &dims[0])
+
+ cdef double[::1,:] value
+
+ value_shape = value_.shape
+ if len(value_shape) == 1:
+ value_shape = (value_shape[0], 0)
+ value = np.asfortranarray(value_[None,:])
+
+ elif len(value_shape) == 2:
+ # Get elements in column major order
+ value = np.asfortranarray(value_)
+
+ if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
+ raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
+ ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+
+ acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
+ self.nlp_dims, self.nlp_in, stage, field, &value[0][0])
+
+ return
+
+
+ def dynamics_get(self, int stage, str field_):
+ """
+ Get numerical data from the dynamics module of the solver:
+
+ :param stage: integer corresponding to shooting node
+ :param field: string, e.g. 'A'
+ """
+ field = field_.encode('utf-8')
+
+ # get dims
+ cdef int[2] dims
+ acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
+
+ # create output data
+ out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
+
+ # call getter
+ acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, out.data)
+
+ return out
+
+
+ def options_set(self, str field_, value_):
+ """
+ Set options of the solver.
+
+ :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
+ :param value: of type int, float
+ """
+ int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
+ double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+ string_fields = ['globalization']
+
+ # encode
+ field = field_.encode('utf-8')
+
+ cdef int int_value
+ cdef double double_value
+ cdef unsigned char[::1] string_value
+
+ # check field availability and type
+ if field_ in int_fields:
+ if not isinstance(value_, int):
+ raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
+
+ if field_ == 'rti_phase':
+ if value_ < 0 or value_ > 2:
+ raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+ 'take only values 0, 1, 2 for SQP-RTI-type solvers')
+ if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
+ raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+ 'take only value 0 for SQP-type solvers')
+
+ int_value = value_
+ acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &int_value)
+
+ elif field_ in double_fields:
+ if not isinstance(value_, float):
+ raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
+
+ double_value = value_
+ acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &double_value)
+
+ elif field_ in string_fields:
+ if not isinstance(value_, bytes):
+ raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
+
+ string_value = value_.encode('utf-8')
+ acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &string_value[0])
+
+ raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+ '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
+
+
+ def __del__(self):
+ if self.solver_created:
+ acados_solver.acados_free(self.capsule)
+ acados_solver.acados_free_capsule(self.capsule)
diff --git a/pyextra/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py
index 3ae198865..d7ad1487d 100644
--- a/pyextra/acados_template/acados_sim.py
+++ b/pyextra/acados_template/acados_sim.py
@@ -106,7 +106,8 @@ class AcadosSimOpts:
"""
def __init__(self):
self.__integrator_type = 'ERK'
- self.__tf = None
+ self.__collocation_type = 'GAUSS_LEGENDRE'
+ self.__Tsim = None
# ints
self.__sim_method_num_stages = 1
self.__sim_method_num_steps = 1
@@ -174,6 +175,15 @@ class AcadosSimOpts:
"""Time horizon"""
return self.__Tsim
+ @property
+ def collocation_type(self):
+ """Collocation type: relevant for implicit integrators
+ -- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}
+
+ Default: GAUSS_LEGENDRE
+ """
+ return self.__collocation_type
+
@integrator_type.setter
def integrator_type(self, integrator_type):
integrator_types = ('ERK', 'IRK', 'GNSF')
@@ -183,6 +193,15 @@ class AcadosSimOpts:
raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
+ ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.')
+ @collocation_type.setter
+ def collocation_type(self, collocation_type):
+ collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE')
+ if collocation_type in collocation_types:
+ self.__collocation_type = collocation_type
+ else:
+ raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
+ + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.')
+
@T.setter
def T(self, T):
self.__Tsim = T
@@ -262,6 +281,8 @@ class AcadosSim:
- :py:attr:`acados_include_path` (set automatically)
- :py:attr:`acados_lib_path` (set automatically)
+ - :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
+
"""
def __init__(self, acados_path=''):
if acados_path == '':
@@ -281,6 +302,21 @@ class AcadosSim:
self.code_export_directory = 'c_generated_code'
"""Path to where code will be exported. Default: `c_generated_code`."""
+ self.__parameter_values = np.array([])
+
+ @property
+ def parameter_values(self):
+ """:math:`p` - initial values for parameter - can be updated"""
+ return self.__parameter_values
+
+ @parameter_values.setter
+ def parameter_values(self, parameter_values):
+ if isinstance(parameter_values, np.ndarray):
+ self.__parameter_values = parameter_values
+ else:
+ raise Exception('Invalid parameter_values value. ' +
+ f'Expected numpy array, got {type(parameter_values)}.')
+
def set(self, attr, value):
# tokenize string
tokens = attr.split('_', 1)
diff --git a/pyextra/acados_template/acados_sim_layout.json b/pyextra/acados_template/acados_sim_layout.json
index c8f4280b8..25b149613 100644
--- a/pyextra/acados_template/acados_sim_layout.json
+++ b/pyextra/acados_template/acados_sim_layout.json
@@ -28,6 +28,9 @@
"integrator_type": [
"str"
],
+ "collocation_type": [
+ "str"
+ ],
"Tsim": [
"float"
],
diff --git a/pyextra/acados_template/acados_sim_solver.py b/pyextra/acados_template/acados_sim_solver.py
index a5447907b..145f90293 100644
--- a/pyextra/acados_template/acados_sim_solver.py
+++ b/pyextra/acados_template/acados_sim_solver.py
@@ -46,7 +46,7 @@ from .acados_sim import AcadosSim
from .acados_ocp import AcadosOcp
from .acados_model import acados_model_strip_casadi_symbolics
from .utils import is_column, render_template, format_class_dict, np_array_to_list,\
- make_model_consistent, set_up_imported_gnsf_model
+ make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path
def make_sim_dims_consistent(acados_sim):
@@ -84,14 +84,13 @@ def make_sim_dims_consistent(acados_sim):
def get_sim_layout():
- current_module = sys.modules[__name__]
- acados_path = os.path.dirname(current_module.__file__)
- with open(acados_path + '/acados_sim_layout.json', 'r') as f:
+ python_interface_path = get_python_interface_path()
+ abs_path = os.path.join(python_interface_path, 'acados_sim_layout.json')
+ with open(abs_path, 'r') as f:
sim_layout = json.load(f)
return sim_layout
-
def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
# Load acados_sim structure description
sim_layout = get_sim_layout()
@@ -114,9 +113,7 @@ def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
def sim_render_templates(json_file, model_name, code_export_dir):
# setting up loader and environment
- json_path = '{cwd}/{json_file}'.format(
- cwd=os.getcwd(),
- json_file=json_file)
+ json_path = os.path.join(os.getcwd(), json_file)
if not os.path.exists(json_path):
raise Exception(f"{json_path} not found!")
@@ -141,7 +138,7 @@ def sim_render_templates(json_file, model_name, code_export_dir):
render_template(in_file, out_file, template_dir, json_path)
## folder model
- template_dir = f'{code_export_dir}/{model_name}_model/'
+ template_dir = os.path.join(code_export_dir, model_name + '_model')
in_file = 'model.in.h'
out_file = f'{model_name}_model.h'
diff --git a/pyextra/acados_template/acados_solver_common.pxd b/pyextra/acados_template/acados_solver_common.pxd
new file mode 100644
index 000000000..9314802e6
--- /dev/null
+++ b/pyextra/acados_template/acados_solver_common.pxd
@@ -0,0 +1,102 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+cdef extern from "acados/ocp_nlp/ocp_nlp_common.h":
+ ctypedef struct ocp_nlp_config:
+ pass
+
+ ctypedef struct ocp_nlp_dims:
+ pass
+
+ ctypedef struct ocp_nlp_in:
+ pass
+
+ ctypedef struct ocp_nlp_out:
+ pass
+
+
+cdef extern from "acados_c/ocp_nlp_interface.h":
+ ctypedef enum ocp_nlp_solver_t:
+ pass
+
+ ctypedef enum ocp_nlp_cost_t:
+ pass
+
+ ctypedef enum ocp_nlp_dynamics_t:
+ pass
+
+ ctypedef enum ocp_nlp_constraints_t:
+ pass
+
+ ctypedef enum ocp_nlp_reg_t:
+ pass
+
+ ctypedef struct ocp_nlp_plan:
+ pass
+
+ ctypedef struct ocp_nlp_solver:
+ pass
+
+ int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_,
+ int start_stage, const char *field, void *value)
+ int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims,
+ ocp_nlp_in *in_, int stage, const char *field, void *value)
+
+ # out
+ void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+ int stage, const char *field, void *value)
+ void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+ int stage, const char *field, void *value)
+ void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver,
+ int stage, const char *field, void *value)
+ int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+ int stage, const char *field)
+ void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+ int stage, const char *field, int *dims_out)
+ void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+ int stage, const char *field, int *dims_out)
+ void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+ int stage, const char *field, int *dims_out)
+
+ # opts
+ void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value)
+
+ # solver
+ void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
+ void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
+
+ # get/set
+ void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, const char *field, void *return_value_)
+ void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, int stage, const char *field, void *value)
diff --git a/pyextra/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in
index 330a5aba1..487e66ab0 100644
--- a/pyextra/acados_template/c_templates_tera/Makefile.in
+++ b/pyextra/acados_template/c_templates_tera/Makefile.in
@@ -279,12 +279,12 @@ all: clean casadi_fun example_sim example
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
{%- endif %}
-CASADI_MODEL_SOURCE=
+CASADI_MODEL_SOURCE=
{%- if solver_options.integrator_type == "ERK" %}
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
{%- endif %}
{%- elif solver_options.integrator_type == "IRK" %}
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
@@ -453,7 +453,7 @@ ocp_shared_lib: casadi_fun ocp_solver
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
{{ link_libs }} \
-lm \
-
+
{%- else %}
ocp_shared_lib: casadi_fun ocp_solver
@@ -465,9 +465,34 @@ ocp_shared_lib: casadi_fun ocp_solver
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
{{ link_libs }} \
-lm \
-
+
{%- endif %}
+ocp_cython_c: ocp_shared_lib
+ cython \
+ -o acados_ocp_solver_pyx.c \
+ -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+ $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+
+ocp_cython_o: ocp_cython_c
+ clang $(ACADOS_FLAGS) -c -O2 \
+ -o acados_ocp_solver_pyx.o \
+ -I /usr/include/python3.8 \
+ -I $(INCLUDE_PATH)/blasfeo/include/ \
+ -I $(INCLUDE_PATH)/hpipm/include/ \
+ -I $(INCLUDE_PATH) \
+ acados_ocp_solver_pyx.c \
+
+ocp_cython: ocp_cython_o
+ clang $(ACADOS_FLAGS) -shared \
+ -o acados_ocp_solver_pyx.so \
+ -Wl,-rpath=$(LIB_PATH) \
+ acados_ocp_solver_pyx.o \
+ $(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
+ -L $(LIB_PATH) -lacados -lhpipm -lblasfeo -lqpOASES_e \
+ {{ link_libs }} \
+ -lm \
+
sim_shared_lib: casadi_fun sim_solver
gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
index 7fa08d8f7..251b249b2 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
@@ -52,7 +52,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
int status = 0;
// create solver
- nlp_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
+ {{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
status = {{ model.name }}_acados_create(acados_ocp_capsule);
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c
index 843440297..560adb0b9 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c
@@ -51,7 +51,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
const mxArray *C_ocp = prhs[0];
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
- nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
+ {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
status = {{ model.name }}_acados_free(capsule);
if (status)
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
index ee86a3116..ae8c31155 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
@@ -66,7 +66,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
const mxArray *C_ocp = prhs[2];
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
- nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
+ {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
// plan
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c
index 9fd4feab2..c673d4bf2 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c
@@ -51,7 +51,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
- nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
+ {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
// solve
{{ model.name }}_acados_solve(capsule);
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
index 390506420..971fb8a78 100644
--- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
@@ -78,9 +78,10 @@ int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule
int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
- int nx = {{ dims.nx }};
- int nu = {{ dims.nu }};
- int nz = {{ dims.nz }};
+ const int nx = {{ model.name | upper }}_NX;
+ const int nu = {{ model.name | upper }}_NU;
+ const int nz = {{ model.name | upper }}_NZ;
+ const int np = {{ model.name | upper }}_NP;
bool tmp_bool;
{#// double Tsim = {{ solver_options.tf / dims.N }};#}
@@ -98,7 +99,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
- external_function_param_casadi_create(capsule->sim_impl_dae_fun, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_impl_dae_fun, np);
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
@@ -106,7 +107,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
- external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np);
// external_function_param_casadi impl_dae_jac_x_xdot_u_z;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
@@ -115,7 +116,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
- external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np);
{%- if hessian_approx == "EXACT" %}
capsule->sim_impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
@@ -126,7 +127,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
- external_function_param_casadi_create(capsule->sim_impl_dae_hess, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_impl_dae_hess, np);
{%- endif %}
{% elif solver_options.integrator_type == "ERK" %}
@@ -140,7 +141,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work;
- external_function_param_casadi_create(capsule->sim_forw_vde_casadi, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
@@ -148,7 +149,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work;
- external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
{%- if hessian_approx == "EXACT" %}
capsule->sim_expl_ode_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
@@ -159,7 +160,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
- external_function_param_casadi_create(capsule->sim_expl_ode_hess, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_expl_ode_hess, np);
{%- endif %}
{% elif solver_options.integrator_type == "GNSF" -%}
@@ -175,7 +176,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
- external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, np);
capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
@@ -183,7 +184,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
- external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, np);
capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
@@ -191,7 +192,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
- external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
@@ -199,7 +200,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
- external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
@@ -207,7 +208,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
- external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, {{ dims.np }});
+ external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, np);
{% endif %}
// sim plan & config
@@ -243,6 +244,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->acados_sim_opts = {{ model.name }}_sim_opts;
int tmp_int = {{ solver_options.sim_method_newton_iter }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int);
+ sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
+ sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type);
{% if problem_class == "SIM" %}
tmp_int = {{ solver_options.sim_method_num_stages }};
@@ -321,35 +324,18 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
{{ model.name }}_sim_dims, {{ model.name }}_sim_opts);
capsule->acados_sim_solver = {{ model.name }}_sim_solver;
+{% if dims.np > 0 %}
/* initialize parameter values */
- {% if dims.np > 0 %}
- // initialize parameters to nominal value
- double p[{{ dims.np }}];
- {% for i in range(end=dims.np) %}
- p[{{ i }}] = {{ parameter_values[i] }};
+ double* p = calloc(np, sizeof(double));
+ {% for item in parameter_values %}
+ {%- if item != 0 %}
+ p[{{ loop.index0 }}] = {{ item }};
+ {%- endif %}
{%- endfor %}
-{%- if solver_options.integrator_type == "ERK" %}
- capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
- capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
-{%- if hessian_approx == "EXACT" %}
- capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
-{%- endif %}
-{%- elif solver_options.integrator_type == "IRK" %}
- capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p);
- capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p);
- capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p);
-{%- if hessian_approx == "EXACT" %}
- capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
-{%- endif %}
-{%- elif solver_options.integrator_type == "GNSF" %}
- capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
- capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
- capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
- capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
- capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
-{% endif %}
- {% endif %}{# if dims.np #}
+ {{ model.name }}_acados_sim_update_params(capsule, p, np);
+ free(p);
+{% endif %}{# if dims.np #}
/* initialize input */
// x
@@ -437,7 +423,7 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
- int casadi_np = {{ dims.np }};
+ int casadi_np = {{ model.name | upper }}_NP;
if (casadi_np != np) {
printf("{{ model.name }}_acados_sim_update_params: trying to set %i parameters for external functions."
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h
index de332e835..4da8202d3 100644
--- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h
@@ -37,6 +37,11 @@
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
+#define {{ model.name | upper }}_NX {{ dims.nx }}
+#define {{ model.name | upper }}_NZ {{ dims.nz }}
+#define {{ model.name | upper }}_NU {{ dims.nu }}
+#define {{ model.name | upper }}_NP {{ dims.np }}
+
#ifdef __cplusplus
extern "C" {
#endif
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
index da73f6e29..6e13242a4 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
@@ -71,59 +71,97 @@
#include "acados_solver_{{ model.name }}.h"
-#define NX {{ dims.nx }}
-#define NZ {{ dims.nz }}
-#define NU {{ dims.nu }}
-#define NP {{ dims.np }}
-#define NBX {{ dims.nbx }}
-#define NBX0 {{ dims.nbx_0 }}
-#define NBU {{ dims.nbu }}
-#define NSBX {{ dims.nsbx }}
-#define NSBU {{ dims.nsbu }}
-#define NSH {{ dims.nsh }}
-#define NSG {{ dims.nsg }}
-#define NSPHI {{ dims.nsphi }}
-#define NSHN {{ dims.nsh_e }}
-#define NSGN {{ dims.nsg_e }}
-#define NSPHIN {{ dims.nsphi_e }}
-#define NSBXN {{ dims.nsbx_e }}
-#define NS {{ dims.ns }}
-#define NSN {{ dims.ns_e }}
-#define NG {{ dims.ng }}
-#define NBXN {{ dims.nbx_e }}
-#define NGN {{ dims.ng_e }}
-#define NY0 {{ dims.ny_0 }}
-#define NY {{ dims.ny }}
-#define NYN {{ dims.ny_e }}
-#define N {{ dims.N }}
-#define NH {{ dims.nh }}
-#define NPHI {{ dims.nphi }}
-#define NHN {{ dims.nh_e }}
-#define NPHIN {{ dims.nphi_e }}
-#define NR {{ dims.nr }}
+#define NX {{ model.name | upper }}_NX
+#define NZ {{ model.name | upper }}_NZ
+#define NU {{ model.name | upper }}_NU
+#define NP {{ model.name | upper }}_NP
+#define NBX {{ model.name | upper }}_NBX
+#define NBX0 {{ model.name | upper }}_NBX0
+#define NBU {{ model.name | upper }}_NBU
+#define NSBX {{ model.name | upper }}_NSBX
+#define NSBU {{ model.name | upper }}_NSBU
+#define NSH {{ model.name | upper }}_NSH
+#define NSG {{ model.name | upper }}_NSG
+#define NSPHI {{ model.name | upper }}_NSPHI
+#define NSHN {{ model.name | upper }}_NSHN
+#define NSGN {{ model.name | upper }}_NSGN
+#define NSPHIN {{ model.name | upper }}_NSPHIN
+#define NSBXN {{ model.name | upper }}_NSBXN
+#define NS {{ model.name | upper }}_NS
+#define NSN {{ model.name | upper }}_NSN
+#define NG {{ model.name | upper }}_NG
+#define NBXN {{ model.name | upper }}_NBXN
+#define NGN {{ model.name | upper }}_NGN
+#define NY0 {{ model.name | upper }}_NY0
+#define NY {{ model.name | upper }}_NY
+#define NYN {{ model.name | upper }}_NYN
+// #define N {{ model.name | upper }}_N
+#define NH {{ model.name | upper }}_NH
+#define NPHI {{ model.name | upper }}_NPHI
+#define NHN {{ model.name | upper }}_NHN
+#define NPHIN {{ model.name | upper }}_NPHIN
+#define NR {{ model.name | upper }}_NR
// ** solver data **
-nlp_solver_capsule * {{ model.name }}_acados_create_capsule()
+{{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void)
{
- void* capsule_mem = malloc(sizeof(nlp_solver_capsule));
- nlp_solver_capsule *capsule = (nlp_solver_capsule *) capsule_mem;
+ void* capsule_mem = malloc(sizeof({{ model.name }}_solver_capsule));
+ {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) capsule_mem;
return capsule;
}
-int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule)
+int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule)
{
free(capsule);
return 0;
}
-int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
+int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule)
+{
+ int N_shooting_intervals = {{ model.name | upper }}_N;
+ double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps
+ return {{ model.name }}_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps);
+}
+
+int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
+{
+ if (N != capsule->nlp_solver_plan->N) {
+ fprintf(stderr, "{{ model.name }}_acados_update_time_steps: given number of time steps (= %d) " \
+ "differs from the currently allocated number of " \
+ "time steps (= %d)!\n" \
+ "Please recreate with new discretization and provide a new vector of time_stamps!\n",
+ N, capsule->nlp_solver_plan->N);
+ return 1;
+ }
+
+ ocp_nlp_config * nlp_config = capsule->nlp_config;
+ ocp_nlp_dims * nlp_dims = capsule->nlp_dims;
+ ocp_nlp_in * nlp_in = capsule->nlp_in;
+
+ for (int i = 0; i < N; i++)
+ {
+ ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]);
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]);
+ }
+ return 0;
+}
+
+int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
{
int status = 0;
+ // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given.
+ if (N != {{ model.name | upper }}_N && !new_time_steps) {
+ fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \
+ "but the number of shooting intervals (= %d) differs from the number of " \
+ "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \
+ N, {{ model.name | upper }}_N);
+ return 1;
+ }
// number of expected runtime parameters
capsule->nlp_np = NP;
@@ -895,28 +933,27 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
{%- endif %}
{%- endfor %}
+ if (new_time_steps) {
+ {{ model.name }}_acados_update_time_steps(capsule, N, new_time_steps);
+ } else {
{%- if all_equal == true -%}
- // all time_steps are identical
- double time_step = {{ solver_options.time_steps[0] }};
- for (int i = 0; i < N; i++)
- {
- ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step);
- ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_step);
- }
+ // all time_steps are identical
+ double time_step = {{ solver_options.time_steps[0] }};
+ for (int i = 0; i < N; i++)
+ {
+ ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step);
+ ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_step);
+ }
{%- else -%}
- // time_steps are different
- double* time_steps = malloc(N*sizeof(double));
- {%- for j in range(end=dims.N) %}
- time_steps[{{ j }}] = {{ solver_options.time_steps[j] }};
- {%- endfor %}
-
- for (int i = 0; i < N; i++)
- {
- ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_steps[i]);
- ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_steps[i]);
- }
- free(time_steps);
+ // time_steps are different
+ double* time_steps = malloc(N*sizeof(double));
+ {%- for j in range(end=dims.N) %}
+ time_steps[{{ j }}] = {{ solver_options.time_steps[j] }};
+ {%- endfor %}
+ {{ model.name }}_acados_update_time_steps(capsule, N, time_steps);
+ free(time_steps);
{%- endif %}
+ }
/**** Dynamics ****/
for (int i = 0; i < N; i++)
@@ -1879,6 +1916,11 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
{%- if solver_options.integrator_type != "DISCRETE" %}
+ // set collocation type (relevant for implicit integrators)
+ sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
+ for (int i = 0; i < N; i++)
+ ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_collocation_type", &collocation_type);
+
// set up sim_method_num_steps
{%- set all_equal = true %}
{%- set val = solver_options.sim_method_num_steps[0] %}
@@ -2113,7 +2155,7 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
}
-int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *p, int np)
+int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *p, int np)
{
int solver_status = 0;
@@ -2125,7 +2167,8 @@ int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stag
}
{%- if dims.np > 0 %}
- if (stage < {{ dims.N }} && stage >= 0)
+ const int N = capsule->nlp_solver_plan->N;
+ if (stage < N && stage >= 0)
{
{%- if solver_options.integrator_type == "IRK" %}
capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p);
@@ -2228,7 +2271,7 @@ int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stag
-int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule)
+int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule)
{
// solve NLP
int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
@@ -2237,8 +2280,10 @@ int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule)
}
-int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
+int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
{
+ // before destroying, keep some info
+ const int N = capsule->nlp_solver_plan->N;
// free memory
ocp_nlp_solver_opts_destroy(capsule->nlp_opts);
ocp_nlp_in_destroy(capsule->nlp_in);
@@ -2251,7 +2296,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
/* free external function */
// dynamics
{%- if solver_options.integrator_type == "IRK" %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->impl_dae_fun[i]);
external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]);
@@ -2268,7 +2313,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
{%- endif %}
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->impl_dae_fun[i]);
external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]);
@@ -2277,7 +2322,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
free(capsule->impl_dae_fun_jac_x_xdot_u);
{%- elif solver_options.integrator_type == "ERK" %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->forw_vde_casadi[i]);
external_function_param_casadi_free(&capsule->expl_ode_fun[i]);
@@ -2292,7 +2337,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
{%- endif %}
{%- elif solver_options.integrator_type == "GNSF" %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]);
external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]);
@@ -2306,7 +2351,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z);
free(capsule->gnsf_get_matrices_fun);
{%- elif solver_options.integrator_type == "DISCRETE" %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun[i]);
external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt[i]);
@@ -2333,7 +2378,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac_hess);
{%- endif %}
{%- if cost.cost_type == "NONLINEAR_LS" %}
- for (int i = 0; i < {{ dims.N }} - 1; i++)
+ for (int i = 0; i < N - 1; i++)
{
external_function_param_casadi_free(&capsule->cost_y_fun[i]);
external_function_param_casadi_free(&capsule->cost_y_fun_jac_ut_xt[i]);
@@ -2343,7 +2388,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
free(capsule->cost_y_fun_jac_ut_xt);
free(capsule->cost_y_hess);
{%- elif cost.cost_type == "EXTERNAL" %}
- for (int i = 0; i < {{ dims.N }} - 1; i++)
+ for (int i = 0; i < N - 1; i++)
{
external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun[i]);
external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac[i]);
@@ -2365,13 +2410,13 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
// constraints
{%- if constraints.constr_type == "BGH" and dims.nh > 0 %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac[i]);
external_function_param_casadi_free(&capsule->nl_constr_h_fun[i]);
}
{%- if solver_options.hessian_approx == "EXACT" %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac_hess[i]);
}
@@ -2383,7 +2428,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
{%- endif %}
{%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %}
- for (int i = 0; i < {{ dims.N }}; i++)
+ for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->phi_constraint[i]);
}
@@ -2403,16 +2448,16 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
return 0;
}
-ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule) { return capsule->nlp_in; }
-ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule) { return capsule->nlp_out; }
-ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule) { return capsule->nlp_solver; }
-ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule) { return capsule->nlp_config; }
-void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule) { return capsule->nlp_opts; }
-ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule) { return capsule->nlp_dims; }
-ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule) { return capsule->nlp_solver_plan; }
+ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_in; }
+ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_out; }
+ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_solver; }
+ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_config; }
+void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_opts; }
+ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_dims; }
+ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_solver_plan; }
-void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule)
+void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule)
{
int sqp_iter, stat_m, stat_n, tmp_int;
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter);
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
index e8966c254..e8c0a38ca 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.h
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
@@ -37,12 +37,43 @@
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
+#define {{ model.name | upper }}_NX {{ dims.nx }}
+#define {{ model.name | upper }}_NZ {{ dims.nz }}
+#define {{ model.name | upper }}_NU {{ dims.nu }}
+#define {{ model.name | upper }}_NP {{ dims.np }}
+#define {{ model.name | upper }}_NBX {{ dims.nbx }}
+#define {{ model.name | upper }}_NBX0 {{ dims.nbx_0 }}
+#define {{ model.name | upper }}_NBU {{ dims.nbu }}
+#define {{ model.name | upper }}_NSBX {{ dims.nsbx }}
+#define {{ model.name | upper }}_NSBU {{ dims.nsbu }}
+#define {{ model.name | upper }}_NSH {{ dims.nsh }}
+#define {{ model.name | upper }}_NSG {{ dims.nsg }}
+#define {{ model.name | upper }}_NSPHI {{ dims.nsphi }}
+#define {{ model.name | upper }}_NSHN {{ dims.nsh_e }}
+#define {{ model.name | upper }}_NSGN {{ dims.nsg_e }}
+#define {{ model.name | upper }}_NSPHIN {{ dims.nsphi_e }}
+#define {{ model.name | upper }}_NSBXN {{ dims.nsbx_e }}
+#define {{ model.name | upper }}_NS {{ dims.ns }}
+#define {{ model.name | upper }}_NSN {{ dims.ns_e }}
+#define {{ model.name | upper }}_NG {{ dims.ng }}
+#define {{ model.name | upper }}_NBXN {{ dims.nbx_e }}
+#define {{ model.name | upper }}_NGN {{ dims.ng_e }}
+#define {{ model.name | upper }}_NY0 {{ dims.ny_0 }}
+#define {{ model.name | upper }}_NY {{ dims.ny }}
+#define {{ model.name | upper }}_NYN {{ dims.ny_e }}
+#define {{ model.name | upper }}_N {{ dims.N }}
+#define {{ model.name | upper }}_NH {{ dims.nh }}
+#define {{ model.name | upper }}_NPHI {{ dims.nphi }}
+#define {{ model.name | upper }}_NHN {{ dims.nh_e }}
+#define {{ model.name | upper }}_NPHIN {{ dims.nphi_e }}
+#define {{ model.name | upper }}_NR {{ dims.nr }}
+
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
-typedef struct nlp_solver_capsule
+typedef struct {{ model.name }}_solver_capsule
{
// acados objects
ocp_nlp_in *nlp_in;
@@ -58,73 +89,115 @@ typedef struct nlp_solver_capsule
/* external functions */
// dynamics
+{% if solver_options.integrator_type == "ERK" %}
external_function_param_casadi *forw_vde_casadi;
external_function_param_casadi *expl_ode_fun;
+{% if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi *hess_vde_casadi;
+{%- endif %}
+{% elif solver_options.integrator_type == "IRK" %}
external_function_param_casadi *impl_dae_fun;
external_function_param_casadi *impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi *impl_dae_jac_x_xdot_u_z;
- external_function_param_casadi *impl_dae_fun_jac_x_xdot_u;
+{% if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi *impl_dae_hess;
+{%- endif %}
+{% elif solver_options.integrator_type == "LIFTED_IRK" %}
+ external_function_param_casadi *impl_dae_fun;
+ external_function_param_casadi *impl_dae_fun_jac_x_xdot_u;
+{% elif solver_options.integrator_type == "GNSF" %}
external_function_param_casadi *gnsf_phi_fun;
external_function_param_casadi *gnsf_phi_fun_jac_y;
external_function_param_casadi *gnsf_phi_jac_y_uhat;
external_function_param_casadi *gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi *gnsf_get_matrices_fun;
+{% elif solver_options.integrator_type == "DISCRETE" %}
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun;
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt;
+{%- if solver_options.hessian_approx == "EXACT" %}
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess;
+{%- endif %}
+{%- endif %}
+
// cost
+{% if cost.cost_type == "NONLINEAR_LS" %}
external_function_param_casadi *cost_y_fun;
external_function_param_casadi *cost_y_fun_jac_ut_xt;
external_function_param_casadi *cost_y_hess;
+{%- elif cost.cost_type == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun;
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac_hess;
+{% endif %}
+{% if cost.cost_type_0 == "NONLINEAR_LS" %}
external_function_param_casadi cost_y_0_fun;
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
external_function_param_casadi cost_y_0_hess;
+{% elif cost.cost_type_0 == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun;
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac_hess;
+{%- endif %}
+{% if cost.cost_type_e == "NONLINEAR_LS" %}
external_function_param_casadi cost_y_e_fun;
external_function_param_casadi cost_y_e_fun_jac_ut_xt;
external_function_param_casadi cost_y_e_hess;
+{% elif cost.cost_type_e == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun;
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac_hess;
+{%- endif %}
// constraints
+{%- if constraints.constr_type == "BGP" %}
external_function_param_casadi *phi_constraint;
+{% elif constraints.constr_type == "BGH" and dims.nh > 0 %}
external_function_param_casadi *nl_constr_h_fun_jac;
external_function_param_casadi *nl_constr_h_fun;
external_function_param_casadi *nl_constr_h_fun_jac_hess;
+{%- endif %}
+
+{% if constraints.constr_type_e == "BGP" %}
external_function_param_casadi phi_e_constraint;
+{% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
external_function_param_casadi nl_constr_h_e_fun_jac;
external_function_param_casadi nl_constr_h_e_fun;
external_function_param_casadi nl_constr_h_e_fun_jac_hess;
-} nlp_solver_capsule;
+{%- endif %}
-nlp_solver_capsule * {{ model.name }}_acados_create_capsule(void);
-int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule);
+} {{ model.name }}_solver_capsule;
-int {{ model.name }}_acados_create(nlp_solver_capsule * capsule);
-int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *value, int np);
-int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule);
-int {{ model.name }}_acados_free(nlp_solver_capsule * capsule);
-void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule);
+{{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void);
+int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule);
-ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule);
-ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule);
-ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule);
-ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule);
-void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule);
-ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule);
-ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule);
+int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule);
+/**
+ * Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than
+ * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
+ * generation, the time-steps from code generation is used.
+ */
+int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
+/**
+ * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
+ * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
+ */
+int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
+int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
+int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
+int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
+void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule);
+
+ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
+void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
#ifdef __cplusplus
} /* extern "C" */
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
new file mode 100644
index 000000000..8387980c2
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
@@ -0,0 +1,22 @@
+cimport acados_solver_common
+
+cdef extern from "acados_solver_{{ model.name }}.h":
+ ctypedef struct nlp_solver_capsule "{{ model.name }}_solver_capsule":
+ pass
+
+ nlp_solver_capsule * acados_create_capsule "{{ model.name }}_acados_create_capsule"()
+ int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
+
+ int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
+ int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
+ int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
+ int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
+ void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule)
+
+ acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
+ acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+ acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
+ acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
+ void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
+ acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "{{ model.name }}_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
+ acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "{{ model.name }}_acados_get_nlp_plan"(nlp_solver_capsule * capsule)
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
index e70c5cafe..a6cd1faa8 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
@@ -377,7 +377,7 @@ static void mdlInitializeSampleTimes(SimStruct *S)
static void mdlStart(SimStruct *S)
{
- nlp_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
+ {{ model.name }}_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
{{ model.name }}_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
@@ -386,7 +386,7 @@ static void mdlStart(SimStruct *S)
static void mdlOutputs(SimStruct *S, int_T tid)
{
- nlp_solver_capsule *capsule = ssGetUserData(S);
+ {{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
@@ -747,7 +747,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
static void mdlTerminate(SimStruct *S)
{
- nlp_solver_capsule *capsule = ssGetUserData(S);
+ {{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
{{ model.name }}_acados_free(capsule);
{{ model.name }}_acados_free_capsule(capsule);
diff --git a/pyextra/acados_template/c_templates_tera/main.in.c b/pyextra/acados_template/c_templates_tera/main.in.c
index 1cae69aea..3348eea5c 100644
--- a/pyextra/acados_template/c_templates_tera/main.in.c
+++ b/pyextra/acados_template/c_templates_tera/main.in.c
@@ -42,12 +42,46 @@
#include "acados_c/external_function_interface.h"
#include "acados_solver_{{ model.name }}.h"
+#define NX {{ model.name | upper }}_NX
+#define NZ {{ model.name | upper }}_NZ
+#define NU {{ model.name | upper }}_NU
+#define NP {{ model.name | upper }}_NP
+#define NBX {{ model.name | upper }}_NBX
+#define NBX0 {{ model.name | upper }}_NBX0
+#define NBU {{ model.name | upper }}_NBU
+#define NSBX {{ model.name | upper }}_NSBX
+#define NSBU {{ model.name | upper }}_NSBU
+#define NSH {{ model.name | upper }}_NSH
+#define NSG {{ model.name | upper }}_NSG
+#define NSPHI {{ model.name | upper }}_NSPHI
+#define NSHN {{ model.name | upper }}_NSHN
+#define NSGN {{ model.name | upper }}_NSGN
+#define NSPHIN {{ model.name | upper }}_NSPHIN
+#define NSBXN {{ model.name | upper }}_NSBXN
+#define NS {{ model.name | upper }}_NS
+#define NSN {{ model.name | upper }}_NSN
+#define NG {{ model.name | upper }}_NG
+#define NBXN {{ model.name | upper }}_NBXN
+#define NGN {{ model.name | upper }}_NGN
+#define NY0 {{ model.name | upper }}_NY0
+#define NY {{ model.name | upper }}_NY
+#define NYN {{ model.name | upper }}_NYN
+#define NH {{ model.name | upper }}_NH
+#define NPHI {{ model.name | upper }}_NPHI
+#define NHN {{ model.name | upper }}_NHN
+#define NPHIN {{ model.name | upper }}_NPHIN
+#define NR {{ model.name | upper }}_NR
+
int main()
{
- nlp_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
- int status = {{ model.name }}_acados_create(acados_ocp_capsule);
+ {{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
+ // there is an opportunity to change the number of shooting intervals in C without new code generation
+ int N = {{ model.name | upper }}_N;
+ // allocate the array and fill it accordingly
+ double* new_time_steps = NULL;
+ int status = {{ model.name }}_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
if (status)
{
@@ -63,13 +97,13 @@ int main()
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
// initial condition
- int idxbx0[{{ dims.nbx_0 }}];
+ int idxbx0[NBX0];
{%- for i in range(end=dims.nbx_0) %}
idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
{%- endfor %}
- double lbx0[{{ dims.nbx_0 }}];
- double ubx0[{{ dims.nbx_0 }}];
+ double lbx0[NBX0];
+ double ubx0[NBX0];
{%- for i in range(end=dims.nbx_0) %}
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
@@ -80,13 +114,13 @@ int main()
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
// initialization for state values
- double x_init[{{ dims.nx }}];
+ double x_init[NX];
{%- for i in range(end=dims.nx) %}
x_init[{{ i }}] = 0.0;
{%- endfor %}
// initial value for control input
- double u0[{{ dims.nu }}];
+ double u0[NU];
{%- for i in range(end=dims.nu) %}
u0[{{ i }}] = 0.0;
{%- endfor %}
@@ -94,14 +128,14 @@ int main()
{%- if dims.np > 0 %}
// set parameters
- double p[{{ dims.np }}];
- {% for item in parameter_values %}
+ double p[NP];
+ {%- for item in parameter_values %}
p[{{ loop.index0 }}] = {{ item }};
- {% endfor %}
+ {%- endfor %}
- for (int ii = 0; ii <= {{ dims.N }}; ii++)
+ for (int ii = 0; ii <= N; ii++)
{
- {{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, {{ dims.np }});
+ {{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, NP);
}
{% endif %}{# if np > 0 #}
@@ -112,8 +146,8 @@ int main()
double elapsed_time;
int sqp_iter;
- double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)];
- double utraj[{{ dims.nu }} * ({{ dims.N }})];
+ double xtraj[NX * (N+1)];
+ double utraj[NU * N];
// solve ocp in loop
@@ -135,14 +169,14 @@ int main()
/* print solution and statistics */
for (int ii = 0; ii <= nlp_dims->N; ii++)
- ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]);
+ ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
for (int ii = 0; ii < nlp_dims->N; ii++)
- ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]);
+ ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
printf("\n--- xtraj ---\n");
- d_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} );
+ d_print_exp_tran_mat( NX, N+1, xtraj, NX);
printf("\n--- utraj ---\n");
- d_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} );
+ d_print_exp_tran_mat( NU, N, utraj, NU );
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
diff --git a/pyextra/acados_template/c_templates_tera/main_sim.in.c b/pyextra/acados_template/c_templates_tera/main_sim.in.c
index b62bf8f7f..743e81d59 100644
--- a/pyextra/acados_template/c_templates_tera/main_sim.in.c
+++ b/pyextra/acados_template/c_templates_tera/main_sim.in.c
@@ -41,6 +41,11 @@
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_{{ model.name }}.h"
+#define NX {{ model.name | upper }}_NX
+#define NZ {{ model.name | upper }}_NZ
+#define NU {{ model.name | upper }}_NU
+#define NP {{ model.name | upper }}_NP
+
int main()
{
@@ -60,7 +65,7 @@ int main()
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
// initial condition
- double x_current[{{ dims.nx }}];
+ double x_current[NX];
{%- for i in range(end=dims.nx) %}
x_current[{{ i }}] = 0.0;
{%- endfor %}
@@ -78,19 +83,19 @@ int main()
// initial value for control input
- double u0[{{ dims.nu }}];
+ double u0[NU];
{%- for i in range(end=dims.nu) %}
u0[{{ i }}] = 0.0;
{%- endfor %}
{%- if dims.np > 0 %}
// set parameters
- double p[{{ dims.np }}];
- {% for item in parameter_values %}
+ double p[NP];
+ {%- for item in parameter_values %}
p[{{ loop.index0 }}] = {{ item }};
- {% endfor %}
+ {%- endfor %}
- {{ model.name }}_acados_sim_update_params(capsule, p, {{ dims.np }});
+ {{ model.name }}_acados_sim_update_params(capsule, p, NP);
{% endif %}{# if np > 0 #}
int n_sim_steps = 3;
@@ -110,7 +115,7 @@ int main()
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
- for (int jj = 0; jj < {{ dims.nx }}; jj++)
+ for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
diff --git a/pyextra/acados_template/generate_c_code_constraint.py b/pyextra/acados_template/generate_c_code_constraint.py
index 93e919c56..c79ddc129 100644
--- a/pyextra/acados_template/generate_c_code_constraint.py
+++ b/pyextra/acados_template/generate_c_code_constraint.py
@@ -94,7 +94,7 @@ def generate_c_code_constraint( model, con_name, is_terminal, opts ):
gen_dir = con_name + '_constraints'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
- gen_dir_location = './' + gen_dir
+ gen_dir_location = os.path.join('.', gen_dir)
os.chdir(gen_dir_location)
# export casadi functions
diff --git a/pyextra/acados_template/generate_c_code_discrete_dynamics.py b/pyextra/acados_template/generate_c_code_discrete_dynamics.py
index 531b5ed9d..334e18dab 100644
--- a/pyextra/acados_template/generate_c_code_discrete_dynamics.py
+++ b/pyextra/acados_template/generate_c_code_discrete_dynamics.py
@@ -80,7 +80,7 @@ def generate_c_code_discrete_dynamics( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
- model_dir_location = './' + model_dir
+ model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
# set up & generate Functions
diff --git a/pyextra/acados_template/generate_c_code_explicit_ode.py b/pyextra/acados_template/generate_c_code_explicit_ode.py
index fb0c020b0..76e640053 100644
--- a/pyextra/acados_template/generate_c_code_explicit_ode.py
+++ b/pyextra/acados_template/generate_c_code_explicit_ode.py
@@ -105,7 +105,7 @@ def generate_c_code_explicit_ode( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
- model_dir_location = './' + model_dir
+ model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
fun_name = model_name + '_expl_ode_fun'
expl_ode_fun.generate(fun_name, casadi_opts)
diff --git a/pyextra/acados_template/generate_c_code_external_cost.py b/pyextra/acados_template/generate_c_code_external_cost.py
index bc21f85f7..839652261 100644
--- a/pyextra/acados_template/generate_c_code_external_cost.py
+++ b/pyextra/acados_template/generate_c_code_external_cost.py
@@ -58,6 +58,7 @@ def generate_c_code_external_cost(model, stage_type, opts):
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
u = symbol("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
+ custom_hess = model.cost_expr_ext_cost_custom_hess_e
elif stage_type == 'path':
suffix_name = "_cost_ext_cost_fun"
@@ -65,6 +66,7 @@ def generate_c_code_external_cost(model, stage_type, opts):
suffix_name_jac = "_cost_ext_cost_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost
+ custom_hess = model.cost_expr_ext_cost_custom_hess
elif stage_type == 'initial':
suffix_name = "_cost_ext_cost_0_fun"
@@ -72,6 +74,7 @@ def generate_c_code_external_cost(model, stage_type, opts):
suffix_name_jac = "_cost_ext_cost_0_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost_0
+ custom_hess = model.cost_expr_ext_cost_custom_hess_0
# set up functions to be exported
fun_name = model.name + suffix_name
@@ -81,6 +84,9 @@ def generate_c_code_external_cost(model, stage_type, opts):
# generate expression for full gradient and Hessian
full_hess, grad = hessian(ext_cost, vertcat(u, x))
+ if custom_hess is not None:
+ full_hess = custom_hess
+
ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
ext_cost_fun_jac_hess = Function(
fun_name_hess, [x, u, p], [ext_cost, grad, full_hess]
diff --git a/pyextra/acados_template/generate_c_code_gnsf.py b/pyextra/acados_template/generate_c_code_gnsf.py
index 3203cbbcc..97acb8e33 100644
--- a/pyextra/acados_template/generate_c_code_gnsf.py
+++ b/pyextra/acados_template/generate_c_code_gnsf.py
@@ -54,7 +54,7 @@ def generate_c_code_gnsf( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
- model_dir_location = './' + model_dir
+ model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
# obtain gnsf dimensions
diff --git a/pyextra/acados_template/generate_c_code_implicit_ode.py b/pyextra/acados_template/generate_c_code_implicit_ode.py
index e0138c69f..f2d50b43a 100644
--- a/pyextra/acados_template/generate_c_code_implicit_ode.py
+++ b/pyextra/acados_template/generate_c_code_implicit_ode.py
@@ -114,7 +114,7 @@ def generate_c_code_implicit_ode( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
- model_dir_location = './' + model_dir
+ model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
fun_name = model_name + '_impl_dae_fun'
diff --git a/pyextra/acados_template/generate_c_code_nls_cost.py b/pyextra/acados_template/generate_c_code_nls_cost.py
index cdb0bf001..ffcd78ca7 100644
--- a/pyextra/acados_template/generate_c_code_nls_cost.py
+++ b/pyextra/acados_template/generate_c_code_nls_cost.py
@@ -76,7 +76,7 @@ def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
gen_dir = cost_name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
- gen_dir_location = './' + gen_dir
+ gen_dir_location = os.path.join('.', gen_dir)
os.chdir(gen_dir_location)
# set up expressions
diff --git a/pyextra/acados_template/utils.py b/pyextra/acados_template/utils.py
index 3f002b98c..bf8ae4d5d 100644
--- a/pyextra/acados_template/utils.py
+++ b/pyextra/acados_template/utils.py
@@ -45,7 +45,7 @@ def get_acados_path():
ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR')
if not ACADOS_PATH:
acados_template_path = os.path.dirname(os.path.abspath(__file__))
- acados_path = os.path.join(acados_template_path, '../../../')
+ acados_path = os.path.join(acados_template_path, '..','..','..')
ACADOS_PATH = os.path.realpath(acados_path)
msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, '
msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH)
@@ -54,10 +54,20 @@ def get_acados_path():
return ACADOS_PATH
+def get_python_interface_path():
+ ACADOS_PYTHON_INTERFACE_PATH = os.environ.get('ACADOS_PYTHON_INTERFACE_PATH')
+ if not ACADOS_PYTHON_INTERFACE_PATH:
+ acados_path = get_acados_path()
+ ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template')
+ return ACADOS_PYTHON_INTERFACE_PATH
+
+
def get_tera_exec_path():
TERA_PATH = os.environ.get('TERA_PATH')
if not TERA_PATH:
- TERA_PATH = os.path.join(get_acados_path(), 'bin/t_renderer')
+ TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer')
+ if os.name == 'nt':
+ TERA_PATH += '.exe'
return TERA_PATH
@@ -199,9 +209,7 @@ def render_template(in_file, out_file, template_dir, json_path):
# setting up loader and environment
acados_path = os.path.dirname(os.path.abspath(__file__))
-
- template_glob = acados_path + '/c_templates_tera/*'
- acados_template_path = acados_path + '/c_templates_tera'
+ template_glob = os.path.join(acados_path, 'c_templates_tera', '*')
# call tera as system cmd
os_cmd = "{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'".format(
@@ -213,7 +221,7 @@ def render_template(in_file, out_file, template_dir, json_path):
)
status = os.system(os_cmd)
if (status != 0):
- raise Exception('Rendering of {} failed! Exiting.\n'.format(in_file))
+ raise Exception('Rendering of {} failed!\n\nAttempted to execute OS command:\n{}\n\nExiting.\n'.format(in_file, os_cmd))
os.chdir(cwd)
@@ -262,6 +270,14 @@ def acados_class2dict(class_instance):
return out
+def get_ocp_nlp_layout():
+ python_interface_path = get_python_interface_path()
+ abs_path = os.path.join(python_interface_path, 'acados_layout.json')
+ with open(abs_path, 'r') as f:
+ ocp_nlp_layout = json.load(f)
+ return ocp_nlp_layout
+
+
def ocp_check_against_layout(ocp_nlp, ocp_dims):
"""
Check dimensions against layout
@@ -273,11 +289,7 @@ def ocp_check_against_layout(ocp_nlp, ocp_dims):
ocp_dims : instance of AcadosOcpDims
"""
- # load JSON layout
- current_module = sys.modules[__name__]
- acados_path = os.path.dirname(current_module.__file__)
- with open(acados_path + '/acados_layout.json', 'r') as f:
- ocp_nlp_layout = json.load(f)
+ ocp_nlp_layout = get_ocp_nlp_layout()
ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout)
return
diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc
index be3f51609..8a2f2b88a 100644
--- a/rednose/helpers/ekf_sym.cc
+++ b/rednose/helpers/ekf_sym.cc
@@ -43,10 +43,10 @@ EKFSym::EKFSym(std::string name, Map Q, Map x_initial, Map<
this->init_state(x_initial, P_initial, NAN);
}
-void EKFSym::init_state(Map state, Map covs, double filter_time) {
+void EKFSym::init_state(Map state, Map covs, double init_filter_time) {
this->x = state;
this->P = covs;
- this->filter_time = filter_time;
+ this->filter_time = init_filter_time;
this->augment_times = VectorXd::Zero(this->N);
this->reset_rewind();
}
diff --git a/release/files_common b/release/files_common
index 68c2600c2..aeae351df 100644
--- a/release/files_common
+++ b/release/files_common
@@ -9,7 +9,10 @@ SConstruct
README.md
RELEASES.md
-docs/*
+docs/CARS.md
+docs/CONTRIBUTING.md
+docs/INTEGRATION.md
+docs/LIMITATIONS.md
site_scons/site_tools/cython.py
common/.gitignore
@@ -22,8 +25,8 @@ common/ffi_wrapper.py
common/file_helpers.py
common/logging_extra.py
common/numpy_fast.py
+common/markdown.py
common/params.py
-common/params_pxd.pxd
common/params_pyx.pyx
common/xattr.py
common/profiler.py
@@ -60,6 +63,8 @@ models/dmonitoring_model_q.dlc
release/*
+tools/lib/*
+
installer/updater/updater
selfdrive/version.py
@@ -125,6 +130,7 @@ selfdrive/car/hyundai/carcontroller.py
selfdrive/car/hyundai/hyundaican.py
selfdrive/car/toyota/__init__.py
selfdrive/car/toyota/carstate.py
+selfdrive/car/toyota/tunes.py
selfdrive/car/toyota/interface.py
selfdrive/car/toyota/radar_interface.py
selfdrive/car/toyota/values.py
@@ -335,7 +341,10 @@ selfdrive/ui/*.h
selfdrive/ui/ui
selfdrive/ui/text
selfdrive/ui/spinner
-selfdrive/ui/soundd
+selfdrive/ui/soundd/*.cc
+selfdrive/ui/soundd/*.h
+selfdrive/ui/soundd/soundd
+selfdrive/ui/soundd/.gitignore
selfdrive/ui/qt/*.cc
selfdrive/ui/qt/*.h
@@ -347,8 +356,8 @@ selfdrive/ui/qt/widgets/*.h
selfdrive/ui/qt/spinner_aarch64
selfdrive/ui/qt/text_aarch64
-selfdrive/ui/replay/framereader.cc
-selfdrive/ui/replay/framereader.h
+selfdrive/ui/replay/*.cc
+selfdrive/ui/replay/*.h
selfdrive/camerad/SConscript
selfdrive/camerad/main.cc
@@ -607,3 +616,4 @@ opendbc/vw_mqb_2010.dbc
opendbc/tesla_can.dbc
opendbc/tesla_radar.dbc
+opendbc/tesla_powertrain.dbc
diff --git a/release/files_pc b/release/files_pc
index 2f55599c2..d00de7e47 100644
--- a/release/files_pc
+++ b/release/files_pc
@@ -1,3 +1,5 @@
+selfdrive/ui/replay/*
+
third_party/mapbox-gl-native-qt/x86_64/**
third_party/qt-plugins/x86_64/**
diff --git a/release/files_tici b/release/files_tici
index b5a2e5b7a..29c8353b8 100644
--- a/release/files_tici
+++ b/release/files_tici
@@ -23,3 +23,9 @@ selfdrive/ui/qt/spinner_larch64
selfdrive/ui/qt/text_larch64
selfdrive/ui/qt/maps/*.cc
selfdrive/ui/qt/maps/*.h
+
+selfdrive/ui/navd/*.cc
+selfdrive/ui/navd/*.h
+selfdrive/ui/navd/navd
+selfdrive/ui/navd/.gitignore
+
diff --git a/selfdrive/assets/fonts/Inter-Black.ttf b/selfdrive/assets/fonts/Inter-Black.ttf
new file mode 100644
index 000000000..565375773
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-Black.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-Bold.ttf b/selfdrive/assets/fonts/Inter-Bold.ttf
new file mode 100644
index 000000000..e98b84ce8
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-Bold.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-ExtraBold.ttf b/selfdrive/assets/fonts/Inter-ExtraBold.ttf
new file mode 100644
index 000000000..7f16a0f0f
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-ExtraBold.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-ExtraLight.ttf b/selfdrive/assets/fonts/Inter-ExtraLight.ttf
new file mode 100644
index 000000000..69426a3eb
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-ExtraLight.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-Light.ttf b/selfdrive/assets/fonts/Inter-Light.ttf
new file mode 100644
index 000000000..a5f073690
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-Light.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-Medium.ttf b/selfdrive/assets/fonts/Inter-Medium.ttf
new file mode 100644
index 000000000..721147d83
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-Medium.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-Regular.ttf b/selfdrive/assets/fonts/Inter-Regular.ttf
new file mode 100644
index 000000000..96fd6a12d
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-Regular.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-SemiBold.ttf b/selfdrive/assets/fonts/Inter-SemiBold.ttf
new file mode 100644
index 000000000..ddb279290
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-SemiBold.ttf differ
diff --git a/selfdrive/assets/fonts/Inter-Thin.ttf b/selfdrive/assets/fonts/Inter-Thin.ttf
new file mode 100644
index 000000000..76be6252b
Binary files /dev/null and b/selfdrive/assets/fonts/Inter-Thin.ttf differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index fadbad0bf..779045bf9 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -26,7 +26,7 @@ from common.file_helpers import CallbackReader
from common.basedir import PERSIST
from common.params import Params
from common.realtime import sec_since_boot
-from selfdrive.hardware import HARDWARE, PC, TICI
+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
@@ -268,9 +268,6 @@ def cancelUpload(upload_id):
@dispatcher.add_method
def primeActivated(activated):
- dongle_id = Params().get("DongleId", encoding='utf-8')
- api = Api(dongle_id)
- manage_tokens(api)
return {"success": 1}
@@ -523,21 +520,6 @@ def backoff(retries):
return random.randrange(0, min(128, int(2 ** retries)))
-def manage_tokens(api):
- if not TICI:
- return
-
- try:
- params = Params()
- mapbox = api.get(f"/v1/tokens/mapbox/{api.dongle_id}/", timeout=5.0, access_token=api.get_token())
- if mapbox.status_code == 200:
- params.put("MapboxToken", mapbox.json()["token"])
- else:
- params.delete("MapboxToken")
- except Exception:
- cloudlog.exception("Failed to update tokens")
-
-
def main():
params = Params()
dongle_id = params.get("DongleId", encoding='utf-8')
@@ -556,8 +538,6 @@ def main():
cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri)
params.delete("PrimeRedirected")
- manage_tokens(api)
-
conn_retries = 0
cur_upload_items.clear()
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index edf5e5e8e..ce73662e4 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -31,6 +31,25 @@
#include "selfdrive/boardd/panda.h"
#include "selfdrive/boardd/pigeon.h"
+// -- Multi-panda conventions --
+// Ordering:
+// - The internal panda will always be the first panda
+// - Consecutive pandas will be sorted based on panda type, and then serial number
+// Connecting:
+// - If a panda connection is dropped, boardd wil reconnect to all pandas
+// - If a panda is added, we will only reconnect when we are offroad
+// CAN buses:
+// - Each panda will have it's block of 4 buses. E.g.: the second panda will use
+// bus numbers 4, 5, 6 and 7
+// - The internal panda will always be used for accessing the OBD2 port,
+// and thus firmware queries
+// Safety:
+// - SafetyConfig is a list, which is mapped to the connected pandas
+// - If there are more pandas connected than there are SafetyConfigs,
+// the excess pandas will remain in "silent" ot "noOutput" mode
+// Ignition:
+// - If any of the ignition sources in any panda is high, ignition is high
+
#define MAX_IR_POWER 0.5f
#define MIN_IR_POWER 0.0f
#define CUTOFF_IL 200
@@ -39,6 +58,7 @@
using namespace std::chrono_literals;
std::atomic ignition(false);
+std::atomic pigeon_active(false);
ExitHandler do_exit;
@@ -48,18 +68,30 @@ std::string get_time_str(const struct tm &time) {
return s;
}
-bool safety_setter_thread(Panda *panda) {
+bool check_all_connected(const std::vector &pandas) {
+ for (const auto& panda : pandas) {
+ if (!panda->connected) return false;
+ }
+ return true;
+}
+
+bool safety_setter_thread(std::vector pandas) {
LOGD("Starting safety setter thread");
- // diagnostic only is the default, needed for VIN query
- panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
+
+ // there should be at least one panda connected
+ if (pandas.size() == 0) {
+ return false;
+ }
+
+ pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
Params p = Params();
// switch to SILENT when CarVin param is read
while (true) {
- if (do_exit || !panda->connected || !ignition) {
+ if (do_exit || !check_all_connected(pandas) || !ignition) {
return false;
- };
+ }
std::string value_vin = p.get("CarVin");
if (value_vin.size() > 0) {
@@ -71,15 +103,16 @@ bool safety_setter_thread(Panda *panda) {
util::sleep_for(20);
}
- // VIN query done, stop listening to OBDII
- panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
+ pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
std::string params;
LOGW("waiting for params to set safety model");
while (true) {
- if (do_exit || !panda->connected || !ignition) {
- return false;
- };
+ for (const auto& panda : pandas) {
+ if (do_exit || !panda->connected || !ignition) {
+ return false;
+ }
+ }
if (p.getBool("ControlsReady")) {
params = p.get("CarParams");
@@ -96,58 +129,39 @@ bool safety_setter_thread(Panda *panda) {
int safety_param;
auto safety_configs = car_params.getSafetyConfigs();
- if (safety_configs.size() > 0) {
- safety_model = safety_configs[0].getSafetyModel();
- safety_param = safety_configs[0].getSafetyParam();
- } else {
- // If no safety mode is set, default to silent
- safety_model = cereal::CarParams::SafetyModel::SILENT;
- safety_param = 0;
+ for (uint32_t i=0; i i) {
+ safety_model = safety_configs[i].getSafetyModel();
+ safety_param = safety_configs[i].getSafetyParam();
+ } else {
+ // If no safety mode is specified, default to silent
+ safety_model = cereal::CarParams::SafetyModel::SILENT;
+ safety_param = 0;
+ }
+
+ LOGW("panda %d: setting safety model: %d with param %d", i, (int)safety_model, safety_param);
+
+ panda->set_unsafe_mode(0); // see safety_declarations.h for allowed values
+ panda->set_safety_model(safety_model, safety_param);
}
- panda->set_unsafe_mode(0); // see safety_declarations.h for allowed values
-
- LOGW("setting safety model: %d with param %d", (int)safety_model, safety_param);
- panda->set_safety_model(safety_model, safety_param);
return true;
}
-
-Panda *usb_connect() {
+Panda *usb_connect(std::string serial="", uint32_t index=0) {
std::unique_ptr panda;
try {
- panda = std::make_unique();
+ panda = std::make_unique(serial, (index * PANDA_BUS_CNT));
} catch (std::exception &e) {
return nullptr;
}
- Params params = Params();
-
if (getenv("BOARDD_LOOPBACK")) {
panda->set_loopback(true);
}
- if (auto fw_sig = panda->get_firmware_version(); fw_sig) {
- params.put("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size());
-
- // Convert to hex for offroad
- char fw_sig_hex_buf[16] = {0};
- const uint8_t *fw_sig_buf = fw_sig->data();
- for (size_t i = 0; i < 8; i++) {
- fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] >> 4);
- fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF);
- }
-
- params.put("PandaFirmwareHex", fw_sig_hex_buf, 16);
- LOGW("fw signature: %.*s", 16, fw_sig_hex_buf);
- } else { return nullptr; }
-
- // get panda serial
- if (auto serial = panda->get_serial(); serial) {
- params.put("PandaDongleId", serial->c_str(), serial->length());
- LOGW("panda serial: %s", serial->c_str());
- } else { return nullptr; }
-
// power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
#ifndef __x86_64__
static std::once_flag connected_once;
@@ -170,14 +184,7 @@ Panda *usb_connect() {
return panda.release();
}
-void can_recv(Panda *panda, PubMaster &pm) {
- kj::Array can_data;
- panda->can_receive(can_data);
- auto bytes = can_data.asBytes();
- pm.send("can", bytes.begin(), bytes.size());
-}
-
-void can_send_thread(Panda *panda, bool fake_send) {
+void can_send_thread(std::vector pandas, bool fake_send) {
LOGD("start send thread");
AlignedBuffer aligned_buf;
@@ -187,7 +194,12 @@ void can_send_thread(Panda *panda, bool fake_send) {
subscriber->setTimeout(100);
// run as fast as messages come in
- while (!do_exit && panda->connected) {
+ while (!do_exit) {
+ if (!check_all_connected(pandas)) {
+ do_exit = true;
+ break;
+ }
+
Message * msg = subscriber->receive();
if (!msg) {
@@ -203,7 +215,9 @@ void can_send_thread(Panda *panda, bool fake_send) {
//Dont send if older than 1 second
if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
if (!fake_send) {
- panda->can_send(event.getSendcan());
+ for (const auto& panda : pandas) {
+ panda->can_send(event.getSendcan());
+ }
}
}
@@ -214,7 +228,7 @@ void can_send_thread(Panda *panda, bool fake_send) {
delete context;
}
-void can_recv_thread(Panda *panda) {
+void can_recv_thread(std::vector pandas) {
LOGD("start recv thread");
// can = 8006
@@ -223,9 +237,31 @@ void can_recv_thread(Panda *panda) {
// run at 100hz
const uint64_t dt = 10000000ULL;
uint64_t next_frame_time = nanos_since_boot() + dt;
+ std::vector raw_can_data;
- while (!do_exit && panda->connected) {
- can_recv(panda, pm);
+ while (!do_exit) {
+ if (!check_all_connected(pandas)){
+ do_exit = true;
+ break;
+ }
+
+ bool comms_healthy = true;
+ raw_can_data.clear();
+ for (const auto& panda : pandas) {
+ comms_healthy &= panda->can_receive(raw_can_data);
+ }
+
+ MessageBuilder msg;
+ auto evt = msg.initEvent();
+ evt.setValid(comms_healthy);
+ auto canData = evt.initCan(raw_can_data.size());
+ for (uint i = 0; isend("pandaStates", msg);
}
-bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) {
- health_t pandaState = panda->get_state();
-
- if (spoofing_started) {
- pandaState.ignition_line = 1;
- }
-
- // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
- if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
- panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
- }
-
- bool ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
-
-#ifndef __x86_64__
- bool power_save_desired = !ignition;
- if (pandaState.power_save_enabled != power_save_desired) {
- panda->set_power_saving(power_save_desired);
- }
-
- // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
- if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
- panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
- }
-#endif
+bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool spoofing_started) {
+ bool ignition_local = false;
// build msg
MessageBuilder msg;
auto evt = msg.initEvent();
- evt.setValid(panda->comms_healthy);
+ auto pss = evt.initPandaStates(pandas.size());
- // TODO: this has to be adapted to merge in multipanda support
- auto ps = evt.initPandaStates(1);
- ps[0].setUptime(pandaState.uptime);
- ps[0].setIgnitionLine(pandaState.ignition_line);
- ps[0].setIgnitionCan(pandaState.ignition_can);
- ps[0].setControlsAllowed(pandaState.controls_allowed);
- ps[0].setGasInterceptorDetected(pandaState.gas_interceptor_detected);
- ps[0].setCanRxErrs(pandaState.can_rx_errs);
- ps[0].setCanSendErrs(pandaState.can_send_errs);
- ps[0].setCanFwdErrs(pandaState.can_fwd_errs);
- ps[0].setGmlanSendErrs(pandaState.gmlan_send_errs);
- ps[0].setPandaType(panda->hw_type);
- ps[0].setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
- ps[0].setSafetyParam(pandaState.safety_param);
- ps[0].setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
- ps[0].setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
- ps[0].setHeartbeatLost((bool)(pandaState.heartbeat_lost));
- ps[0].setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
+ std::vector pandaStates;
+ for (const auto& panda : pandas){
+ health_t pandaState = panda->get_state();
- // Convert faults bitset to capnp list
- std::bitset fault_bits(pandaState.faults);
- auto faults = ps[0].initFaults(fault_bits.count());
+ if (spoofing_started) {
+ pandaState.ignition_line = 1;
+ }
- size_t i = 0;
- for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
- f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
- if (fault_bits.test(f)) {
- faults.set(i, cereal::PandaState::FaultType(f));
- i++;
+ ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
+
+ pandaStates.push_back(pandaState);
+ }
+
+ for (uint32_t i=0; iset_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+ }
+
+ #ifndef __x86_64__
+ bool power_save_desired = !ignition_local && !pigeon_active;
+ if (pandaState.power_save_enabled != power_save_desired) {
+ panda->set_power_saving(power_save_desired);
+ }
+
+ // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
+ if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
+ panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+ }
+ #endif
+
+ // TODO: do we still need this?
+ if (!panda->comms_healthy) {
+ evt.setValid(false);
+ }
+
+ auto ps = pss[i];
+ ps.setUptime(pandaState.uptime);
+ ps.setIgnitionLine(pandaState.ignition_line);
+ ps.setIgnitionCan(pandaState.ignition_can);
+ ps.setControlsAllowed(pandaState.controls_allowed);
+ ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
+ ps.setCanRxErrs(pandaState.can_rx_errs);
+ ps.setCanSendErrs(pandaState.can_send_errs);
+ ps.setCanFwdErrs(pandaState.can_fwd_errs);
+ ps.setGmlanSendErrs(pandaState.gmlan_send_errs);
+ ps.setPandaType(panda->hw_type);
+ ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
+ ps.setSafetyParam(pandaState.safety_param);
+ ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
+ ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
+ ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost));
+ ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
+
+ // Convert faults bitset to capnp list
+ std::bitset fault_bits(pandaState.faults);
+ auto faults = ps.initFaults(fault_bits.count());
+
+ size_t j = 0;
+ for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
+ f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
+ if (fault_bits.test(f)) {
+ faults.set(j, cereal::PandaState::FaultType(f));
+ j++;
+ }
}
}
- pm->send("pandaStates", msg);
- return ignition;
+ pm->send("pandaStates", msg);
+ return ignition_local;
}
void send_peripheral_state(PubMaster *pm, Panda *panda) {
@@ -354,23 +406,36 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
pm->send("peripheralState", msg);
}
-void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, bool spoofing_started) {
+void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofing_started) {
Params params;
+ Panda *peripheral_panda = pandas[0];
bool ignition_last = false;
std::future safety_future;
LOGD("start panda state thread");
// run at 2hz
- while (!do_exit && panda->connected) {
+ while (!do_exit) {
+ if(!check_all_connected(pandas)) {
+ do_exit = true;
+ break;
+ }
+
send_peripheral_state(pm, peripheral_panda);
- ignition = send_panda_state(pm, panda, spoofing_started);
+ ignition = send_panda_states(pm, pandas, spoofing_started);
+
+ // check if we have new pandas and are offroad
+ if (!ignition && (pandas.size() != Panda::list().size())) {
+ LOGW("Reconnecting to changed amount of pandas!");
+ do_exit = true;
+ break;
+ }
// clear VIN, CarParams, 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) {
- safety_future = std::async(std::launch::async, safety_setter_thread, panda);
+ safety_future = std::async(std::launch::async, safety_setter_thread, pandas);
} else {
LOGW("Safety setter thread already running");
}
@@ -380,7 +445,9 @@ void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, b
ignition_last = ignition;
- panda->send_heartbeat();
+ for (const auto &panda : pandas) {
+ panda->send_heartbeat();
+ }
util::sleep_for(500);
}
}
@@ -498,10 +565,11 @@ void pigeon_thread(Panda *panda) {
while (!do_exit && panda->connected) {
bool need_reset = false;
+ bool ignition_local = ignition;
std::string recv = pigeon->receive();
// Parse message header
- if (ignition && recv.length() >= 3) {
+ if (ignition_local && recv.length() >= 3) {
if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2) {
const char msg_cls = recv[2];
uint64_t t = nanos_since_boot();
@@ -514,7 +582,7 @@ void pigeon_thread(Panda *panda) {
// Check based on message frequency
for (const auto& [msg_cls, max_dt] : cls_max_dt) {
int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls];
- if (ignition_last && ignition && dt > max_dt) {
+ if (ignition_last && ignition_local && dt > max_dt) {
LOGD("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt);
// TODO: turn on reset after verification of logs
// need_reset = true;
@@ -522,7 +590,7 @@ void pigeon_thread(Panda *panda) {
}
// Check based on null bytes
- if (ignition && recv.length() > 0 && recv[0] == (char)0x00) {
+ if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) {
need_reset = true;
LOGW("received invalid ublox message while onroad, resetting panda GPS");
}
@@ -533,7 +601,8 @@ void pigeon_thread(Panda *panda) {
// init pigeon on rising ignition edge
// since it was turned off in low power mode
- if((ignition && !ignition_last) || need_reset) {
+ if((ignition_local && !ignition_last) || need_reset) {
+ pigeon_active = true;
pigeon->init();
// Set receive times to current time
@@ -541,14 +610,15 @@ void pigeon_thread(Panda *panda) {
for (const auto& [msg_cls, dt] : cls_max_dt) {
last_recv_time[msg_cls] = t;
}
- } else if (!ignition && ignition_last) {
+ } else if (!ignition_local && ignition_last) {
// power off on falling edge of ignition
LOGD("powering off pigeon\n");
pigeon->stop();
pigeon->set_power(false);
+ pigeon_active = false;
}
- ignition_last = ignition;
+ ignition_last = ignition_local;
// 10ms - 100 Hz
util::sleep_for(10);
@@ -557,44 +627,56 @@ void pigeon_thread(Panda *panda) {
delete pigeon;
}
-int main() {
+int main(int argc, char *argv[]) {
LOGW("starting boardd");
- // set process priority and affinity
- int err = set_realtime_priority(54);
- LOG("set priority returns %d", err);
-
- err = set_core_affinity({Hardware::TICI() ? 4 : 3});
- LOG("set affinity returns %d", err);
+ if (!Hardware::PC()) {
+ int err;
+ err = set_realtime_priority(54);
+ assert(err == 0);
+ err = set_core_affinity({Hardware::TICI() ? 4 : 3});
+ assert(err == 0);
+ }
LOGW("attempting to connect");
PubMaster pm({"pandaStates", "peripheralState"});
- while (!do_exit) {
- Panda *panda = usb_connect();
- Panda *peripheral_panda = panda;
+ std::vector serials(argv + 1, argv + argc);
+ if (serials.size() == 0) serials.push_back("");
- // Send empty pandaState & peripheralState and try again
- if (panda == nullptr || peripheral_panda == nullptr) {
+ // connect to all provided serials
+ std::vector pandas;
+ for (int i = 0; i < serials.size() && !do_exit; /**/) {
+ Panda *p = usb_connect(serials[i], i);
+ if (!p) {
+ // send empty pandaState & peripheralState and try again
send_empty_panda_state(&pm);
send_empty_peripheral_state(&pm);
util::sleep_for(500);
continue;
}
- LOGW("connected to board");
+ pandas.push_back(p);
+ ++i;
+ }
+ if (!do_exit) {
+ LOGW("connected to board");
+ Panda *peripheral_panda = pandas[0];
std::vector threads;
- threads.emplace_back(panda_state_thread, &pm, peripheral_panda, panda, getenv("STARTED") != nullptr);
+
+ threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
threads.emplace_back(peripheral_control_thread, peripheral_panda);
threads.emplace_back(pigeon_thread, peripheral_panda);
- threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
- threads.emplace_back(can_recv_thread, panda);
+ threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
+ threads.emplace_back(can_recv_thread, pandas);
for (auto &t : threads) t.join();
+ }
+ // we have exited, clean up pandas
+ for (Panda *panda : pandas) {
delete panda;
- panda = nullptr;
}
}
diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc
index a4c9f3f6c..faa0e3737 100644
--- a/selfdrive/boardd/can_list_to_can_capnp.cc
+++ b/selfdrive/boardd/can_list_to_can_capnp.cc
@@ -1,11 +1,5 @@
#include "cereal/messaging/messaging.h"
-
-typedef struct {
- long address;
- std::string dat;
- long busTime;
- long src;
-} can_frame;
+#include "panda.h"
extern "C" {
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index c123d0c80..78b123f69 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -7,6 +7,7 @@
#include
#include "cereal/messaging/messaging.h"
+#include "panda/board/dlc_to_len.h"
#include "selfdrive/common/gpio.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
@@ -30,7 +31,7 @@ static int init_usb_ctx(libusb_context **context) {
}
-Panda::Panda(std::string serial) {
+Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
// init libusb
ssize_t num_devices;
libusb_device **dev_list = NULL;
@@ -343,33 +344,80 @@ void Panda::send_heartbeat() {
usb_write(0xf3, 1, 0);
}
-void Panda::can_send(capnp::List::Reader can_data_list) {
- const int msg_count = can_data_list.size();
- const int buf_size = msg_count*0x10;
-
- if (send.size() < buf_size) {
- send.resize(buf_size);
- }
-
- for (int i = 0; i < msg_count; i++) {
- auto cmsg = can_data_list[i];
- if (cmsg.getAddress() >= 0x800) { // extended
- send[i*4] = (cmsg.getAddress() << 3) | 5;
- } else { // normal
- send[i*4] = (cmsg.getAddress() << 21) | 1;
- }
- auto can_data = cmsg.getDat();
- assert(can_data.size() <= 8);
- send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4);
- memcpy(&send[i*4+2], can_data.begin(), can_data.size());
- }
-
- usb_bulk_write(3, (unsigned char*)send.data(), buf_size, 5);
+void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {
+ usb_write(0xde, bus, (speed * 10));
}
-int Panda::can_receive(kj::Array& out_buf) {
- uint32_t data[RECV_SIZE/4];
- int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE);
+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) {
+ if (len <= 8) {
+ return len;
+ }
+ if (len <= 24) {
+ return 8 + ((len - 8) / 4) + ((len % 4) ? 1 : 0);
+ } else {
+ return 11 + (len / 16) + ((len % 16) ? 1 : 0);
+ }
+}
+
+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);
+ }
+ }
+}
+
+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;
@@ -378,27 +426,51 @@ int Panda::can_receive(kj::Array& out_buf) {
LOGW("Receive buffer full");
}
- size_t num_msg = recv / 0x10;
- MessageBuilder msg;
- auto evt = msg.initEvent();
- evt.setValid(comms_healthy);
-
- // populate message
- auto canData = evt.initCan(num_msg);
- for (int i = 0; i < num_msg; i++) {
- if (data[i*4] & 4) {
- // extended
- canData[i].setAddress(data[i*4] >> 3);
- //printf("got extended: %x\n", data[i*4] >> 3);
- } else {
- // normal
- canData[i].setAddress(data[i*4] >> 21);
- }
- canData[i].setBusTime(data[i*4+1] >> 16);
- int len = data[i*4+1]&0xF;
- canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
- canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
+ if (!comms_healthy) {
+ return false;
}
- out_buf = capnp::messageToFlatArray(msg);
- return recv;
+
+ 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]) {
+ LOGE("CAN: MALFORMED USB RECV PACKET");
+ break;
+ }
+ 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);
+
+ 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; }
+ canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], data_len);
+
+ pos += pckt_len;
+ } else {
+ // Keep partial CAN packet until next USB packet
+ tail_size = (chunk_len - pos);
+ memcpy(tail, &chunk[pos], tail_size);
+ break;
+ }
+ }
+ }
+ return true;
}
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 36fd699be..4be9454cf 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -3,6 +3,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -12,9 +13,15 @@
#include "cereal/gen/cpp/car.capnp.h"
#include "cereal/gen/cpp/log.capnp.h"
-// double the FIFO size
-#define RECV_SIZE (0x1000)
#define TIMEOUT 0
+#define PANDA_BUS_CNT 4
+#define RECV_SIZE (0x4000U)
+#define USB_TX_SOFT_LIMIT (0x100U)
+#define USBPACKET_MAX_SIZE (0x40U)
+#define CANPACKET_HEAD_SIZE 5U
+#define CANPACKET_MAX_SIZE 72U
+#define CANPACKET_REJECTED (0xC0U)
+#define CANPACKET_RETURNED (0x80U)
// copied from panda/board/main.c
struct __attribute__((packed)) health_t {
@@ -39,18 +46,34 @@ struct __attribute__((packed)) health_t {
uint8_t heartbeat_lost;
};
+struct __attribute__((packed)) can_header {
+ uint8_t reserved : 1;
+ uint8_t bus : 3;
+ uint8_t data_len_code : 4;
+ uint8_t rejected : 1;
+ uint8_t returned : 1;
+ uint8_t extended : 1;
+ uint32_t addr : 29;
+};
+
+struct can_frame {
+ long address;
+ std::string dat;
+ long busTime;
+ long src;
+};
class Panda {
private:
libusb_context *ctx = NULL;
libusb_device_handle *dev_handle = NULL;
std::mutex usb_lock;
- std::vector send;
+ std::vector send;
void handle_usb_issue(int err, const char func[]);
void cleanup();
public:
- Panda(std::string serial="");
+ Panda(std::string serial="", uint32_t bus_offset=0);
~Panda();
std::string usb_serial;
@@ -58,6 +81,7 @@ class Panda {
std::atomic comms_healthy = true;
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
bool has_rtc = false;
+ const uint32_t bus_offset;
// Static functions
static std::vector list();
@@ -84,6 +108,9 @@ class Panda {
void set_power_saving(bool power_saving);
void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
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);
- int can_receive(kj::Array& out_buf);
+ bool can_receive(std::vector& out_vec);
};
diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc
index b063b9e17..b8735cc3b 100644
--- a/selfdrive/boardd/pigeon.cc
+++ b/selfdrive/boardd/pigeon.cc
@@ -41,18 +41,19 @@ Pigeon * Pigeon::connect(const char * tty) {
return pigeon;
}
-bool Pigeon::wait_for_ack(const std::string &ack, const std::string &nack) {
+bool Pigeon::wait_for_ack(const std::string &ack_, const std::string &nack_, int timeout_ms) {
std::string s;
+ const double start_t = millis_since_boot();
while (!do_exit) {
s += receive();
- if (s.find(ack) != std::string::npos) {
+ if (s.find(ack_) != std::string::npos) {
LOGD("Received ACK from ublox");
return true;
- } else if (s.find(nack) != std::string::npos) {
+ } else if (s.find(nack_) != std::string::npos) {
LOGE("Received NACK from ublox");
return false;
- } else if (s.size() > 0x1000) {
+ } else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) {
LOGE("No response from ublox");
return false;
}
diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h
index ecd42cb7b..0a82bcdd9 100644
--- a/selfdrive/boardd/pigeon.h
+++ b/selfdrive/boardd/pigeon.h
@@ -16,7 +16,7 @@ class Pigeon {
void init();
void stop();
bool wait_for_ack();
- bool wait_for_ack(const std::string &ack, const std::string &nack);
+ bool wait_for_ack(const std::string &ack, const std::string &nack, int timeout_ms = 1000);
bool send_with_ack(const std::string &cmd);
virtual void set_baud(int baud) = 0;
virtual void send(const std::string &s) = 0;
diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript
index 65be54aa4..d018704d6 100644
--- a/selfdrive/camerad/SConscript
+++ b/selfdrive/camerad/SConscript
@@ -21,8 +21,12 @@ else:
if USE_FRAME_STREAM:
cameras = ['cameras/camera_frame_stream.cc']
else:
- libs += ['avutil', 'avcodec', 'avformat', 'swscale']
- cameras = ['cameras/camera_replay.cc', env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc')]
+ 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')]
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 313d71c60..4db6eaa12 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -30,25 +30,56 @@
const int YUV_COUNT = 100;
-static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b, const CameraState *s) {
- char args[4096];
- snprintf(args, sizeof(args),
- "-cl-fast-relaxed-math -cl-denorms-are-zero "
- "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d "
- "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d "
- "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d",
- ci->frame_width, ci->frame_height, ci->frame_stride,
- b->rgb_width, b->rgb_height, b->rgb_stride,
- ci->bayer_flip, ci->hdr, s->camera_num);
- const char *cl_file = Hardware::TICI() ? "cameras/real_debayer.cl" : "cameras/debayer.cl";
- return cl_program_from_file(context, device_id, cl_file, args);
-}
+class Debayer {
+public:
+ Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) {
+ char args[4096];
+ const CameraInfo *ci = &s->ci;
+ snprintf(args, sizeof(args),
+ "-cl-fast-relaxed-math -cl-denorms-are-zero "
+ "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d "
+ "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d "
+ "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d",
+ ci->frame_width, ci->frame_height, ci->frame_stride,
+ b->rgb_width, b->rgb_height, b->rgb_stride,
+ ci->bayer_flip, ci->hdr, s->camera_num);
+ const char *cl_file = Hardware::TICI() ? "cameras/real_debayer.cl" : "cameras/debayer.cl";
+ cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args);
+ krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err));
+ CL_CHECK(clReleaseProgram(prg_debayer));
+ }
-void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType rgb_type, VisionStreamType yuv_type, release_cb release_callback) {
+ void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, cl_event *debayer_event) {
+ CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl));
+ CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl));
+
+ if (Hardware::TICI()) {
+ const int debayer_local_worksize = 16;
+ constexpr int localMemSize = (debayer_local_worksize + 2 * (3 / 2)) * (debayer_local_worksize + 2 * (3 / 2)) * sizeof(short int);
+ const size_t globalWorkSize[] = {size_t(width), size_t(height)};
+ const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
+ CL_CHECK(clSetKernelArg(krnl_, 2, localMemSize, 0));
+ CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
+ } else {
+ const size_t debayer_work_size = height; // doesn't divide evenly, is this okay?
+ CL_CHECK(clSetKernelArg(krnl_, 2, sizeof(float), &gain));
+ CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 1, NULL, &debayer_work_size, NULL, 0, 0, debayer_event));
+ }
+ }
+
+ ~Debayer() {
+ CL_CHECK(clReleaseKernel(krnl_));
+ }
+
+private:
+ cl_kernel krnl_;
+};
+
+void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType init_rgb_type, VisionStreamType init_yuv_type, release_cb init_release_callback) {
vipc_server = v;
- this->rgb_type = rgb_type;
- this->yuv_type = yuv_type;
- this->release_callback = release_callback;
+ this->rgb_type = init_rgb_type;
+ this->yuv_type = init_yuv_type;
+ this->release_callback = init_release_callback;
const CameraInfo *ci = &s->ci;
camera_state = s;
@@ -81,11 +112,8 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
vipc_server->create_buffers(yuv_type, YUV_COUNT, false, rgb_width, rgb_height);
if (ci->bayer) {
- cl_program prg_debayer = build_debayer_program(device_id, context, ci, this, s);
- krnl_debayer = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err));
- CL_CHECK(clReleaseProgram(prg_debayer));
+ debayer = new Debayer(device_id, context, this, s);
}
-
rgb2yuv = std::make_unique(context, device_id, rgb_width, rgb_height, rgb_stride);
#ifdef __APPLE__
@@ -100,8 +128,7 @@ CameraBuf::~CameraBuf() {
for (int i = 0; i < frame_buf_count; i++) {
camera_bufs[i].free();
}
-
- if (krnl_debayer) CL_CHECK(clReleaseKernel(krnl_debayer));
+ if (debayer) delete debayer;
if (q) CL_CHECK(clReleaseCommandQueue(q));
}
@@ -116,37 +143,25 @@ bool CameraBuf::acquire() {
cur_frame_data = camera_bufs_metadata[cur_buf_idx];
cur_rgb_buf = vipc_server->get_buffer(rgb_type);
-
- cl_event debayer_event;
cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
- if (camera_state->ci.bayer) {
- CL_CHECK(clSetKernelArg(krnl_debayer, 0, sizeof(cl_mem), &camrabuf_cl));
- CL_CHECK(clSetKernelArg(krnl_debayer, 1, sizeof(cl_mem), &cur_rgb_buf->buf_cl));
-#ifdef QCOM2
- constexpr int localMemSize = (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * sizeof(short int);
- const size_t globalWorkSize[] = {size_t(camera_state->ci.frame_width), size_t(camera_state->ci.frame_height)};
- const size_t localWorkSize[] = {DEBAYER_LOCAL_WORKSIZE, DEBAYER_LOCAL_WORKSIZE};
- CL_CHECK(clSetKernelArg(krnl_debayer, 2, localMemSize, 0));
- CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 2, NULL, globalWorkSize, localWorkSize,
- 0, 0, &debayer_event));
-#else
- float digital_gain = camera_state->digital_gain;
- if ((int)digital_gain == 0) {
- digital_gain = 1.0;
- }
- CL_CHECK(clSetKernelArg(krnl_debayer, 2, sizeof(float), &digital_gain));
- const size_t debayer_work_size = rgb_height; // doesn't divide evenly, is this okay?
- CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 1, NULL,
- &debayer_work_size, NULL, 0, 0, &debayer_event));
+ cl_event event;
+
+ if (debayer) {
+ float gain = 0.0;
+
+#ifndef QCOM2
+ gain = camera_state->digital_gain;
+ if ((int)gain == 0) gain = 1.0;
#endif
+
+ debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, &event);
} else {
assert(rgb_stride == camera_state->ci.frame_stride);
- CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0,
- cur_rgb_buf->len, 0, 0, &debayer_event));
+ CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, cur_rgb_buf->len, 0, 0, &event));
}
- clWaitForEvents(1, &debayer_event);
- CL_CHECK(clReleaseEvent(debayer_event));
+ clWaitForEvents(1, &event);
+ CL_CHECK(clReleaseEvent(event));
cur_yuv_buf = vipc_server->get_buffer(yuv_type);
rgb2yuv->queue(q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
@@ -156,6 +171,8 @@ bool CameraBuf::acquire() {
cur_frame_data.timestamp_sof,
cur_frame_data.timestamp_eof,
};
+ cur_rgb_buf->set_frame_id(cur_frame_data.frame_id);
+ cur_yuv_buf->set_frame_id(cur_frame_data.frame_id);
vipc_server->send(cur_rgb_buf, &extra);
vipc_server->send(cur_yuv_buf, &extra);
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index 7b510058e..e994fc220 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -34,6 +34,7 @@ enum CameraType {
WideRoadCam
};
+// TODO: remove these once all the internal tools are moved to vipc
const bool env_send_driver = getenv("SEND_DRIVER") != NULL;
const bool env_send_road = getenv("SEND_ROAD") != NULL;
const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL;
@@ -94,13 +95,13 @@ typedef struct CameraExpInfo {
struct MultiCameraState;
struct CameraState;
+class Debayer;
class CameraBuf {
private:
VisionIpcServer *vipc_server;
CameraState *camera_state;
- cl_kernel krnl_debayer;
-
+ Debayer *debayer = nullptr;
std::unique_ptr rgb2yuv;
VisionStreamType rgb_type, yuv_type;
diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc
index 920ed1690..03138e353 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/selfdrive/camerad/cameras/camera_qcom2.cc
@@ -377,18 +377,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
buf_desc[0].mem_handle = buf0_mem_handle;
buf_desc[0].offset = buf0_offset;
- buf_desc[1].size = 324;
- if (io_mem_handle != 0) {
- buf_desc[1].length = 228; // 0 works here too
- buf_desc[1].offset = 0x60;
- } else {
- buf_desc[1].length = 324;
- }
- buf_desc[1].type = CAM_CMD_BUF_GENERIC;
- buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
- uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
-
- // cam_isp_packet_generic_blob_handler
+ // cam_isp_packet_generic_blob_handler
uint32_t tmp[] = {
// size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG)
0x2000,
@@ -410,6 +399,13 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+ buf_desc[1].size = sizeof(tmp);
+ buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0;
+ buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
+ buf_desc[1].type = CAM_CMD_BUF_GENERIC;
+ buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
+ uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
memcpy(buf2, tmp, sizeof(tmp));
if (io_mem_handle != 0) {
@@ -555,7 +551,6 @@ int open_v4l_by_name_and_index(const char name[], int index, int flags = O_RDWR
}
static void camera_open(CameraState *s) {
- int ret;
s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
assert(s->sensor_fd >= 0);
LOGD("opened sensor");
@@ -570,7 +565,7 @@ static void camera_open(CameraState *s) {
// create session
struct cam_req_mgr_session_info session_info = {};
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
+ int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
s->session_handle = session_info.session_hdl;
@@ -581,73 +576,59 @@ static void camera_open(CameraState *s) {
s->sensor_dev_handle = *sensor_dev_handle;
LOGD("acquire sensor dev");
- static struct cam_isp_resource isp_resource = {0};
- isp_resource.resource_id = CAM_ISP_RES_ID_PORT;
- isp_resource.length = sizeof(struct cam_isp_in_port_info) + sizeof(struct cam_isp_out_port_info)*(1-1);
- isp_resource.handle_type = CAM_HANDLE_USER_POINTER;
+ struct cam_isp_in_port_info in_port_info = {
+ .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[s->camera_num],
- struct cam_isp_in_port_info *in_port_info = (struct cam_isp_in_port_info *)malloc(isp_resource.length);
- isp_resource.res_hdl = (uint64_t)in_port_info;
+ .lane_type = CAM_ISP_LANE_TYPE_DPHY,
+ .lane_num = 4,
+ .lane_cfg = 0x3210,
- switch (s->camera_num) {
- case 0:
- in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_0;
- break;
- case 1:
- in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_1;
- break;
- case 2:
- in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_2;
- break;
- }
+ .vc = 0x0,
+ // .dt = 0x2C; //CSI_RAW12
+ .dt = 0x2B, //CSI_RAW10
+ .format = CAM_FORMAT_MIPI_RAW_10,
- in_port_info->lane_type = CAM_ISP_LANE_TYPE_DPHY;
- in_port_info->lane_num = 4;
- in_port_info->lane_cfg = 0x3210;
+ .test_pattern = 0x2, // 0x3?
+ .usage_type = 0x0,
- in_port_info->vc = 0x0;
- //in_port_info->dt = 0x2C; //CSI_RAW12
- //in_port_info->format = CAM_FORMAT_MIPI_RAW_12;
+ .left_start = 0,
+ .left_stop = FRAME_WIDTH - 1,
+ .left_width = FRAME_WIDTH,
- in_port_info->dt = 0x2B; //CSI_RAW10
- in_port_info->format = CAM_FORMAT_MIPI_RAW_10;
+ .right_start = 0,
+ .right_stop = FRAME_WIDTH - 1,
+ .right_width = FRAME_WIDTH,
- in_port_info->test_pattern = 0x2; // 0x3?
- in_port_info->usage_type = 0x0;
+ .line_start = 0,
+ .line_stop = FRAME_HEIGHT - 1,
+ .height = FRAME_HEIGHT,
- in_port_info->left_start = 0x0;
- in_port_info->left_stop = FRAME_WIDTH - 1;
- in_port_info->left_width = FRAME_WIDTH;
+ .pixel_clk = 0x0,
+ .batch_size = 0x0,
+ .dsp_mode = 0x0,
+ .hbi_cnt = 0x0,
+ .custom_csid = 0x0,
- in_port_info->right_start = 0x0;
- in_port_info->right_stop = FRAME_WIDTH - 1;
- in_port_info->right_width = FRAME_WIDTH;
-
- in_port_info->line_start = 0x0;
- in_port_info->line_stop = FRAME_HEIGHT - 1;
- in_port_info->height = FRAME_HEIGHT;
-
- in_port_info->pixel_clk = 0x0;
- in_port_info->batch_size = 0x0;
- in_port_info->dsp_mode = 0x0;
- in_port_info->hbi_cnt = 0x0;
- in_port_info->custom_csid = 0x0;
-
- in_port_info->num_out_res = 0x1;
- in_port_info->data[0] = (struct cam_isp_out_port_info){
- .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
- //.format = CAM_FORMAT_MIPI_RAW_12,
- .format = CAM_FORMAT_MIPI_RAW_10,
- .width = FRAME_WIDTH,
- .height = FRAME_HEIGHT,
- .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
+ .num_out_res = 0x1,
+ .data[0] = (struct cam_isp_out_port_info){
+ .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
+ .format = CAM_FORMAT_MIPI_RAW_10,
+ .width = FRAME_WIDTH,
+ .height = FRAME_HEIGHT,
+ .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
+ },
+ };
+ struct cam_isp_resource isp_resource = {
+ .resource_id = CAM_ISP_RES_ID_PORT,
+ .handle_type = CAM_HANDLE_USER_POINTER,
+ .res_hdl = (uint64_t)&in_port_info,
+ .length = sizeof(in_port_info),
};
auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
assert(isp_dev_handle);
s->isp_dev_handle = *isp_dev_handle;
LOGD("acquire isp dev");
- free(in_port_info);
struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
@@ -655,19 +636,14 @@ static void camera_open(CameraState *s) {
s->csiphy_dev_handle = *csiphy_dev_handle;
LOGD("acquire csiphy dev");
- // acquires done
-
// config ISP
alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu);
config_isp(s, 0, 0, 1, s->buf0_handle, 0);
LOG("-- Configuring sensor");
- sensors_i2c(s, init_array_ar0231, sizeof(init_array_ar0231)/sizeof(struct i2c_random_wr_payload),
- CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
- //sensors_i2c(s, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload),
- //CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
- //sensors_i2c(s, stop_reg_array, sizeof(stop_reg_array)/sizeof(struct i2c_random_wr_payload),
- //CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
+ sensors_i2c(s, init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+ //sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
+ //sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
// config csiphy
LOG("-- Config CSI PHY");
@@ -693,8 +669,8 @@ static void camera_open(CameraState *s) {
csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py
- int ret = device_config(s->csiphy_fd, s->session_handle, s->csiphy_dev_handle, cam_packet_handle);
- assert(ret == 0);
+ int ret_ = device_config(s->csiphy_fd, s->session_handle, s->csiphy_dev_handle, cam_packet_handle);
+ assert(ret_ == 0);
munmap(csiphy_info, buf_desc[0].size);
release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
@@ -704,7 +680,7 @@ static void camera_open(CameraState *s) {
// link devices
LOG("-- Link devices");
- static struct cam_req_mgr_link_info req_mgr_link_info = {0};
+ struct cam_req_mgr_link_info req_mgr_link_info = {0};
req_mgr_link_info.session_hdl = s->session_handle;
req_mgr_link_info.num_devices = 2;
req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
@@ -713,7 +689,7 @@ static void camera_open(CameraState *s) {
LOGD("link: %d", ret);
s->link_handle = req_mgr_link_info.link_hdl;
- static struct cam_req_mgr_link_control req_mgr_link_control = {0};
+ struct cam_req_mgr_link_control req_mgr_link_control = {0};
req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
req_mgr_link_control.session_hdl = s->session_handle;
req_mgr_link_control.num_links = 1;
diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h
index 483569c7a..f8ab2da8a 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.h
+++ b/selfdrive/camerad/cameras/camera_qcom2.h
@@ -8,7 +8,7 @@
#include "selfdrive/common/util.h"
#define FRAME_BUF_COUNT 4
-#define DEBAYER_LOCAL_WORKSIZE 16
+
typedef struct CameraState {
MultiCameraState *multi_cam_state;
CameraInfo ci;
diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc
index a1060ef17..6bc53d8c9 100644
--- a/selfdrive/camerad/cameras/camera_replay.cc
+++ b/selfdrive/camerad/cameras/camera_replay.cc
@@ -23,8 +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) {
- // TODO: cache url file
- s->frame = new FrameReader();
+ s->frame = new FrameReader(true);
if (!s->frame->load(url)) {
printf("failed to load stream from %s", url.c_str());
assert(0);
@@ -49,12 +48,13 @@ void run_camera(CameraState *s) {
uint32_t stream_frame_id = 0, frame_id = 0;
size_t buf_idx = 0;
std::unique_ptr rgb_buf = std::make_unique(s->frame->getRGBSize());
+ std::unique_ptr yuv_buf = std::make_unique(s->frame->getYUVSize());
while (!do_exit) {
if (stream_frame_id == s->frame->getFrameCount()) {
// loop stream
stream_frame_id = 0;
}
- if (s->frame->get(stream_frame_id++, rgb_buf.get(), nullptr)) {
+ if (s->frame->get(stream_frame_id++, rgb_buf.get(), yuv_buf.get())) {
s->buf.camera_bufs_metadata[buf_idx] = {.frame_id = frame_id};
auto &buf = s->buf.camera_bufs[buf_idx];
CL_CHECK(clEnqueueWriteBuffer(buf.copy_q, buf.buf_cl, CL_TRUE, 0, s->frame->getRGBSize(), rgb_buf.get(), 0, NULL, NULL));
diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc
index 4d9312688..1db713044 100644
--- a/selfdrive/camerad/main.cc
+++ b/selfdrive/camerad/main.cc
@@ -44,11 +44,13 @@ void party(cl_device_id device_id, cl_context context) {
#endif
int main(int argc, char *argv[]) {
- int ret;
- ret = set_realtime_priority(53);
- assert(ret == 0);
- ret = set_core_affinity({Hardware::EON() ? 2 : 6});
- assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
+ if (!Hardware::PC()) {
+ int ret;
+ ret = set_realtime_priority(53);
+ assert(ret == 0);
+ ret = set_core_affinity({Hardware::EON() ? 2 : 6});
+ assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
+ }
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py
index 787d62837..889e1579b 100755
--- a/selfdrive/camerad/snapshot/snapshot.py
+++ b/selfdrive/camerad/snapshot/snapshot.py
@@ -1,5 +1,4 @@
#!/usr/bin/env python3
-import os
import subprocess
import time
@@ -8,24 +7,29 @@ from PIL import Image
from typing import List
import cereal.messaging as messaging
+from cereal.visionipc.visionipc_pyx import VisionIpcClient, VisionStreamType # pylint: disable=no-name-in-module, import-error
from common.params import Params
from common.realtime import DT_MDL
-from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size
-from selfdrive.hardware import TICI
+from selfdrive.hardware import TICI, PC
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.manager.process_config import managed_processes
LM_THRESH = 120 # defined in selfdrive/camerad/imgproc/utils.h
+VISION_STREAMS = {
+ "roadCameraState": VisionStreamType.VISION_STREAM_RGB_BACK,
+ "driverCameraState": VisionStreamType.VISION_STREAM_RGB_FRONT,
+ "wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE,
+}
+
def jpeg_write(fn, dat):
img = Image.fromarray(dat)
img.save(fn, "JPEG")
-def extract_image(dat, frame_sizes):
- img = np.frombuffer(dat, dtype=np.uint8)
- w, h = frame_sizes[len(img) // 3]
+def extract_image(buf, w, h, stride):
+ img = np.hstack([buf[i * stride:i * stride + 3 * w] for i in range(h)])
b = img[::3].reshape(h, w)
g = img[1::3].reshape(h, w)
r = img[2::3].reshape(h, w)
@@ -33,45 +37,47 @@ def extract_image(dat, frame_sizes):
def rois_in_focus(lapres: List[float]) -> float:
- sz = len(lapres)
- return sum([1. / sz 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.):
- frame_sizes = [eon_f_frame_size, eon_d_frame_size, tici_f_frame_size]
- frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes}
-
- sockets = []
- if frame is not None:
- sockets.append(frame)
- if front_frame is not None:
- sockets.append(front_frame)
+ sockets = [s for s in (frame, front_frame) if s is not None]
+ sm = messaging.SubMaster(sockets)
+ vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets}
# wait 4 sec from camerad startup for focus and exposure
- sm = messaging.SubMaster(sockets)
while sm[sockets[0]].frameId < int(4. / DT_MDL):
sm.update()
+ for client in vipc_clients.values():
+ client.connect(True)
+
+ # wait for focus
start_t = time.monotonic()
while time.monotonic() - start_t < 10:
- sm.update()
+ sm.update(100)
if min(sm.rcv_frame.values()) > 1 and rois_in_focus(sm[frame].sharpnessScore) >= focus_perc_threshold:
break
- rear = extract_image(sm[frame].image, frame_sizes) if frame is not None else None
- front = extract_image(sm[front_frame].image, frame_sizes) if front_frame is not None else None
+ # grab images
+ rear, front = None, None
+ if frame is not None:
+ c = vipc_clients[frame]
+ rear = extract_image(c.recv(), c.width, c.height, c.stride)
+ if front_frame is not None:
+ c = vipc_clients[front_frame]
+ front = extract_image(c.recv(), c.width, c.height, c.stride)
return rear, front
def snapshot():
params = Params()
- front_camera_allowed = params.get_bool("RecordFront")
if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
print("Already taking snapshot")
return None, None
+ front_camera_allowed = params.get_bool("RecordFront")
params.put_bool("IsTakingSnapshot", True)
set_offroad_alert("Offroad_IsTakingSnapshot", True)
time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start
@@ -86,13 +92,11 @@ def snapshot():
except subprocess.CalledProcessError:
pass
- os.environ["SEND_ROAD"] = "1"
- os.environ["SEND_WIDE_ROAD"] = "1"
- if front_camera_allowed:
- os.environ["SEND_DRIVER"] = "1"
-
try:
- managed_processes['camerad'].start()
+ # Allow testing on replay on PC
+ if not PC:
+ managed_processes['camerad'].start()
+
frame = "wideRoadCameraState" if TICI else "roadCameraState"
front_frame = "driverCameraState" if front_camera_allowed else None
focus_perc_threshold = 0. if TICI else 10 / 12.
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index f2e1b37e3..980b207fa 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -79,10 +79,6 @@ interface_names = _get_interface_names()
interfaces = load_interfaces(interface_names)
-def only_toyota_left(candidate_cars):
- return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) and len(candidate_cars) > 0
-
-
# **** for use live only ****
def fingerprint(logcan, sendcan):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
@@ -126,27 +122,24 @@ def fingerprint(logcan, sendcan):
a = get_one_can(logcan)
for can in a.can:
- # need to independently try to fingerprint both bus 0 and 1 to work
- # for the combo black_panda and honda_bosch. Ignore extended messages
- # and VIN query response.
- # Include bus 2 for toyotas to disambiguate cars using camera messages
- # (ideally should be done for all cars but we can't for Honda Bosch)
- if can.src in range(0, 4):
+ # The fingerprint dict is generated for all buses, this way the car interface
+ # can use it to detect a (valid) multipanda setup and initialize accordingly
+ if can.src < 128:
+ if can.src not in finger.keys():
+ finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)
+
for b in candidate_cars:
- if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
- can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
+ # Ignore extended messages and VIN query response.
+ if can.src == b and can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
- # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
- if only_toyota_left(candidate_cars[b]):
- frame_fingerprint = 100 # 1s
if len(candidate_cars[b]) == 1 and frame > frame_fingerprint:
- # fingerprint done
- car_fingerprint = candidate_cars[b][0]
+ # fingerprint done
+ car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200
diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py
index 2c58b8e3a..2c451b7f4 100755
--- a/selfdrive/car/fw_versions.py
+++ b/selfdrive/car/fw_versions.py
@@ -71,6 +71,19 @@ MAZDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
MAZDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
+NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xc0])
+NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xc0])
+
+NISSAN_VERSION_REQUEST_KWP = b'\x21\x83'
+NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83'
+
+NISSAN_VERSION_REQUEST_STANDARD = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
+ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
+NISSAN_VERSION_RESPONSE_STANDARD = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
+ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
+
+NISSAN_RX_OFFSET = 0x20
+
# brand, request, response, response offset
REQUESTS = [
# Hyundai
@@ -131,7 +144,26 @@ REQUESTS = [
[MAZDA_VERSION_REQUEST],
[MAZDA_VERSION_RESPONSE],
DEFAULT_RX_OFFSET,
- )
+ ),
+ # Nissan
+ (
+ "nissan",
+ [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
+ [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
+ DEFAULT_RX_OFFSET,
+ ),
+ (
+ "nissan",
+ [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP],
+ [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
+ NISSAN_RX_OFFSET,
+ ),
+ (
+ "nissan",
+ [NISSAN_VERSION_REQUEST_STANDARD],
+ [NISSAN_VERSION_RESPONSE_STANDARD],
+ NISSAN_RX_OFFSET,
+ ),
]
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index c94f693c7..8f4d0f27c 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -14,6 +14,7 @@ class CarController():
def __init__(self, dbc_name, CP, VM):
self.start_time = 0.
self.apply_steer_last = 0
+ self.lka_steering_cmd_counter_last = -1
self.lka_icon_status_last = (False, False)
self.steer_rate_limited = False
@@ -31,8 +32,12 @@ class CarController():
# Send CAN commands.
can_sends = []
- # STEER
- if (frame % P.STEER_STEP) == 0:
+ # Steering (50Hz)
+ # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the
+ # next Panda loopback confirmation in the current CS frame.
+ if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
+ self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
+ elif (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = int(round(actuators.steer * P.STEER_MAX))
@@ -42,7 +47,9 @@ class CarController():
apply_steer = 0
self.apply_steer_last = apply_steer
- idx = (frame // P.STEER_STEP) % 4
+ # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the
+ # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks.
+ idx = (CS.lka_steering_cmd_counter + 1) % 4
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index f16f5e542..00f25c5fd 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -13,8 +13,9 @@ class CarState(CarStateBase):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
+ self.lka_steering_cmd_counter = 0
- def update(self, pt_cp):
+ def update(self, pt_cp, loopback_cp):
ret = car.CarState.new_message()
self.prev_cruise_buttons = self.cruise_buttons
@@ -42,6 +43,7 @@ class CarState(CarStateBase):
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
+ self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
@@ -131,3 +133,15 @@ class CarState(CarStateBase):
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN)
+
+ @staticmethod
+ def get_loopback_can_parser(CP):
+ signals = [
+ ("RollingCounter", "ASCMLKASteeringCmd", 0),
+ ]
+
+ checks = [
+ ("ASCMLKASteeringCmd", 50),
+ ]
+
+ return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK)
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 53692c25f..b06e5373a 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -16,18 +16,24 @@ class CarInterface(CarInterfaceBase):
params = CarControllerParams()
return params.ACCEL_MIN, params.ACCEL_MAX
- # Volt determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
+ # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
@staticmethod
def get_steer_feedforward_volt(desired_angle, v_ego):
- # maps [-inf,inf] to [-1,1]: sigmoid(34.4 deg) = sigmoid(1) = 0.5
- # 1 / 0.02904609 = 34.4 deg ~= 36 deg ~= 1/10 circle? Arbitrary?
desired_angle *= 0.02904609
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.10006696 * sigmoid * (v_ego + 3.12485927)
+ @staticmethod
+ def get_steer_feedforward_acadia(desired_angle, v_ego):
+ desired_angle *= 0.09760208
+ sigmoid = desired_angle / (1 + fabs(desired_angle))
+ return 0.04689655 * sigmoid * (v_ego + 10.028217)
+
def get_steer_feedforward_function(self):
- if self.CP.carFingerprint in [CAR.VOLT]:
+ if self.CP.carFingerprint == CAR.VOLT:
return self.get_steer_feedforward_volt
+ elif self.CP.carFingerprint == CAR.ACADIA:
+ return self.get_steer_feedforward_acadia
else:
return CarInterfaceBase.get_steer_feedforward_default
@@ -94,6 +100,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 14.4 # end to end is 13.46
ret.steerRatioRear = 0.
ret.centerToFront = ret.wheelbase * 0.4
+ ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia()
elif candidate == CAR.BUICK_REGAL:
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
@@ -111,6 +118,17 @@ class CarInterface(CarInterfaceBase):
ret.steerRatioRear = 0.
ret.centerToFront = ret.wheelbase * 0.49
+ elif candidate == CAR.ESCALADE_ESV:
+ ret.minEnableSpeed = -1. # engage speed is decided by pcm
+ ret.mass = 2739. + STD_CARGO_KG
+ ret.wheelbase = 3.302
+ ret.steerRatio = 17.3
+ ret.centerToFront = ret.wheelbase * 0.49
+ ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
+ ret.lateralTuning.pid.kf = 0.000045
+ tire_stiffness_factor = 1.0
+
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
@@ -133,10 +151,11 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def update(self, c, can_strings):
self.cp.update_strings(can_strings)
+ self.cp_loopback.update_strings(can_strings)
- ret = self.CS.update(self.cp)
+ ret = self.CS.update(self.cp, self.cp_loopback)
- ret.canValid = self.cp.can_valid
+ ret.canValid = self.cp.can_valid and self.cp_loopback.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
buttonEvents = []
diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py
index 7e475c568..4cb1e0781 100755
--- a/selfdrive/car/gm/radar_interface.py
+++ b/selfdrive/car/gm/radar_interface.py
@@ -16,7 +16,7 @@ NUM_SLOTS = 20
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
def create_radar_can_parser(car_fingerprint):
- if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
+ if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV):
return None
# C1A-ARS3-A by Continental
diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py
index cb4a68a69..d475b4bd6 100644
--- a/selfdrive/car/gm/values.py
+++ b/selfdrive/car/gm/values.py
@@ -47,6 +47,7 @@ class CAR:
MALIBU = "CHEVROLET MALIBU PREMIER 2017"
ACADIA = "GMC ACADIA DENALI 2018"
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
+ ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016"
class CruiseButtons:
INIT = 0
@@ -67,6 +68,7 @@ class CanBus:
OBSTACLE = 1
CHASSIS = 2
SW_GMLAN = 3
+ LOOPBACK = 128
FINGERPRINTS = {
# Astra BK MY17, ASCM unplugged
@@ -106,6 +108,10 @@ FINGERPRINTS = {
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
}],
+ CAR.ESCALADE_ESV: [
+ {
+ 309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8
+ }],
}
DBC = {
@@ -115,4 +121,5 @@ DBC = {
CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
+ CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
}
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 088e42476..1e0a35fdf 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -232,7 +232,7 @@ class CarController():
# This prevents unexpected pedal range rescaling
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
# when you do enable.
- if enabled:
+ if active:
apply_gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
else:
apply_gas = 0.0
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index c45573c72..8bf3ac58f 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_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
TransmissionType = car.CarParams.TransmissionType
@@ -161,8 +161,7 @@ class CarState(CarStateBase):
self.gearbox_msg = "GEARBOX_15T"
self.main_on_sig_msg = "SCM_FEEDBACK"
- if CP.carFingerprint in (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.RIDGELINE):
+ if CP.carFingerprint in HONDA_NIDEC_ALT_SCM_MESSAGES:
self.main_on_sig_msg = "SCM_BUTTONS"
self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
@@ -234,7 +233,7 @@ class CarState(CarStateBase):
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
- self.brake_hold = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"]
+ ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
@@ -288,7 +287,7 @@ class CarState(CarStateBase):
ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"])
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
- if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
+ if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE):
if ret.brake > 0.05:
ret.brakePressed = True
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index adeec6bd4..4aa6ec381 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_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, 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
@@ -252,6 +252,16 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
+ elif candidate == CAR.PASSPORT:
+ stop_and_go = False
+ ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
+ ret.wheelbase = 2.82
+ ret.centerToFront = ret.wheelbase * 0.428
+ ret.steerRatio = 17.25 # as spec
+ 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.444
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
+
elif candidate == CAR.RIDGELINE:
stop_and_go = False
ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG
@@ -289,6 +299,10 @@ class CarInterface(CarInterfaceBase):
if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
+ # These cars use alternate SCM messages (SCM_FEEDBACK AND SCM_BUTTON)
+ if candidate in HONDA_NIDEC_ALT_SCM_MESSAGES:
+ ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_NIDEC_ALT
+
if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
@@ -370,8 +384,6 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, pcm_enable=False)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
- if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
- events.add(EventName.brakeHold)
if self.CS.park_brake:
events.add(EventName.parkBrake)
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 26a6d2ce3..97e0ded66 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -76,30 +76,28 @@ class CAR:
ACURA_RDX_3G = "ACURA RDX 2020"
PILOT = "HONDA PILOT 2017"
PILOT_2019 = "HONDA PILOT 2019"
+ PASSPORT = "HONDA PASSPORT 2021"
RIDGELINE = "HONDA RIDGELINE 2017"
INSIGHT = "HONDA INSIGHT 2019"
HONDA_E = "HONDA E 2020"
-# diag message that in some Nidec cars only appear with 1s freq if VIN query is performed
-DIAG_MSGS = {1600: 5, 1601: 8}
-
-FINGERPRINTS = {
- CAR.ODYSSEY_CHN: [{
- 57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
- }],
-}
-
-# add DIAG_MSGS to fingerprints
-for c in FINGERPRINTS:
- for f, _ in enumerate(FINGERPRINTS[c]):
- for d in DIAG_MSGS:
- FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
-
-# TODO: Figure out what is relevant
FW_VERSIONS = {
CAR.ACCORD: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
+ b'37805-6A0-9520\x00\x00',
+ b'37805-6A0-9620\x00\x00',
+ b'37805-6A0-9720\x00\x00',
+ b'37805-6A0-A540\x00\x00',
+ b'37805-6A0-A550\x00\x00',
b'37805-6A0-A640\x00\x00',
+ b'37805-6A0-A650\x00\x00',
+ b'37805-6A0-A740\x00\x00',
+ b'37805-6A0-A750\x00\x00',
+ b'37805-6A0-A840\x00\x00',
+ b'37805-6A0-A850\x00\x00',
+ b'37805-6A0-AG30\x00\x00',
+ b'37805-6A0-C540\x00\x00',
+ b'37805-6A1-H650\x00\x00',
b'37805-6B2-A550\x00\x00',
b'37805-6B2-A560\x00\x00',
b'37805-6B2-A650\x00\x00',
@@ -109,24 +107,22 @@ FW_VERSIONS = {
b'37805-6B2-A820\x00\x00',
b'37805-6B2-A920\x00\x00',
b'37805-6B2-M520\x00\x00',
- b'37805-6A0-9520\x00\x00',
- b'37805-6A0-9620\x00\x00',
- b'37805-6A0-9720\x00\x00',
- b'37805-6A0-A540\x00\x00',
- b'37805-6A0-A550\x00\x00',
- b'37805-6A0-A650\x00\x00',
- b'37805-6A0-A740\x00\x00',
- b'37805-6A0-A750\x00\x00',
- b'37805-6A0-A840\x00\x00',
- b'37805-6A0-A850\x00\x00',
- b'37805-6A0-C540\x00\x00',
- b'37805-6A1-H650\x00\x00',
+ b'37805-6B2-Y810\x00\x00',
b'37805-6M4-B730\x00\x00',
],
(Ecu.shiftByWire, 0x18da0bf1, None): [
b'54008-TVC-A910\x00\x00',
],
(Ecu.transmission, 0x18da1ef1, None): [
+ b'28101-6A7-A220\x00\x00',
+ b'28101-6A7-A230\x00\x00',
+ b'28101-6A7-A320\x00\x00',
+ b'28101-6A7-A330\x00\x00',
+ b'28101-6A7-A410\x00\x00',
+ b'28101-6A7-A510\x00\x00',
+ b'28101-6A7-A610\x00\x00',
+ b'28101-6A9-H140\x00\x00',
+ b'28101-6A9-H420\x00\x00',
b'28102-6B8-A560\x00\x00',
b'28102-6B8-A570\x00\x00',
b'28102-6B8-A700\x00\x00',
@@ -134,14 +130,7 @@ FW_VERSIONS = {
b'28102-6B8-C560\x00\x00',
b'28102-6B8-C570\x00\x00',
b'28102-6B8-M520\x00\x00',
- b'28101-6A7-A220\x00\x00',
- b'28101-6A7-A230\x00\x00',
- b'28101-6A7-A320\x00\x00',
- b'28101-6A7-A330\x00\x00',
- b'28101-6A7-A410\x00\x00',
- b'28101-6A7-A510\x00\x00',
- b'28101-6A9-H140\x00\x00',
- b'28101-6A9-H420\x00\x00',
+ b'28102-6B8-R700\x00\x00',
],
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [
b'46114-TVA-A060\x00\x00',
@@ -153,38 +142,51 @@ FW_VERSIONS = {
b'46114-TVE-H560\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
+ b'57114-TVA-B040\x00\x00',
+ b'57114-TVA-B050\x00\x00',
+ b'57114-TVA-B060\x00\x00',
+ b'57114-TVA-B530\x00\x00',
b'57114-TVA-C040\x00\x00',
b'57114-TVA-C050\x00\x00',
b'57114-TVA-C060\x00\x00',
b'57114-TVA-C530\x00\x00',
- b'57114-TVA-B040\x00\x00',
- b'57114-TVA-B050\x00\x00',
- b'57114-TVA-B060\x00\x00',
b'57114-TVE-H250\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
- b'39990-TVA,A150\x00\x00',
+ b'39990-TBX-H120\x00\x00',
+ b'39990-TVA-A140\x00\x00',
b'39990-TVA-A150\x00\x00',
b'39990-TVA-A160\x00\x00',
b'39990-TVA-A340\x00\x00',
b'39990-TVA-X030\x00\x00',
- b'39990-TVA-A140\x00\x00',
+ b'39990-TVA-X040\x00\x00',
+ b'39990-TVA,A150\x00\x00',
b'39990-TVE-H130\x00\x00',
- b'39990-TBX-H120\x00\x00',
],
(Ecu.unknown, 0x18da3af1, None): [
b'39390-TVA-A020\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
+ b'77959-TBX-H230\x00\x00',
b'77959-TVA-A460\x00\x00',
+ b'77959-TVA-F330\x00\x00',
+ b'77959-TVA-H230\x00\x00',
b'77959-TVA-L420\x00\x00',
b'77959-TVA-X330\x00\x00',
- b'77959-TVA-H230\x00\x00',
- b'77959-TBX-H230\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
- b'78109-TVC-C010\x00\x00',
+ b'78109-TBX-H310\x00\x00',
+ b'78109-TVA-A010\x00\x00',
+ b'78109-TVA-A020\x00\x00',
+ b'78109-TVA-A030\x00\x00',
+ b'78109-TVA-A110\x00\x00',
+ b'78109-TVA-A120\x00\x00',
b'78109-TVA-A210\x00\x00',
+ b'78109-TVA-A220\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-TVC-A010\x00\x00',
b'78109-TVC-A020\x00\x00',
b'78109-TVC-A030\x00\x00',
@@ -192,20 +194,12 @@ FW_VERSIONS = {
b'78109-TVC-A130\x00\x00',
b'78109-TVC-A210\x00\x00',
b'78109-TVC-A220\x00\x00',
+ b'78109-TVC-C010\x00\x00',
b'78109-TVC-C110\x00\x00',
b'78109-TVC-L010\x00\x00',
b'78109-TVC-L210\x00\x00',
b'78109-TVC-M510\x00\x00',
- b'78109-TBX-H310\x00\x00',
- b'78109-TVA-A010\x00\x00',
- b'78109-TVA-A020\x00\x00',
- b'78109-TVA-A110\x00\x00',
- b'78109-TVA-A120\x00\x00',
- b'78109-TVA-A220\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-TVC-YF10\x00\x00',
b'78109-TVE-H610\x00\x00',
b'78109-TWA-A210\x00\x00',
],
@@ -213,25 +207,29 @@ FW_VERSIONS = {
b'78209-TVA-A010\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
+ b'36802-TBX-H140\x00\x00',
+ b'36802-TVA-A150\x00\x00',
b'36802-TVA-A160\x00\x00',
b'36802-TVA-A170\x00\x00',
+ b'36802-TVA-A330\x00\x00',
b'36802-TVC-A330\x00\x00',
- b'36802-TWA-A070\x00\x00',
- b'36802-TVA-A150\x00\x00',
b'36802-TVE-H070\x00\x00',
- b'36802-TBX-H140\x00\x00',
+ b'36802-TWA-A070\x00\x00',
+ b'36802-TWA-A080\x00\x00',
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
- b'36161-TVA-A060\x00\x00',
- b'36161-TVC-A330\x00\x00',
- b'36161-TWA-A070\x00\x00',
- b'36161-TVE-H050\x00\x00',
b'36161-TBX-H130\x00\x00',
+ b'36161-TVA-A060\x00\x00',
+ b'36161-TVA-A330\x00\x00',
+ b'36161-TVC-A330\x00\x00',
+ b'36161-TVE-H050\x00\x00',
+ b'36161-TWA-A070\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TVA-A010\x00\x00',
b'38897-TVA-A020\x00\x00',
b'38897-TVA-A230\x00\x00',
+ b'38897-TVA-A240\x00\x00',
],
},
CAR.ACCORDH: {
@@ -242,6 +240,7 @@ FW_VERSIONS = {
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TWA-A040\x00\x00',
b'57114-TWA-A050\x00\x00',
+ b'57114-TWA-A530\x00\x00',
b'57114-TWA-B520\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
@@ -256,7 +255,9 @@ FW_VERSIONS = {
b'78109-TWA-A120\x00\x00',
b'78109-TWA-A210\x00\x00',
b'78109-TWA-A220\x00\x00',
+ b'78109-TWA-A230\x00\x00',
b'78109-TWA-L010\x00\x00',
+ b'78109-TWA-L210\x00\x00',
],
(Ecu.shiftByWire, 0x18da0bf1, None): [
b'54008-TWA-A910\x00\x00',
@@ -413,6 +414,7 @@ FW_VERSIONS = {
b'37805-5AN-LS20\x00\x00',
b'37805-5AW-G720\x00\x00',
b'37805-5AZ-E850\x00\x00',
+ b'37805-5AZ-G540\x00\x00',
b'37805-5AZ-G740\x00\x00',
b'37805-5AZ-G840\x00\x00',
b'37805-5BB-A530\x00\x00',
@@ -478,6 +480,7 @@ FW_VERSIONS = {
b'77959-TEA-G020\x00\x00',
b'77959-TGG-A020\x00\x00',
b'77959-TGG-A030\x00\x00',
+ b'77959-TGG-E010\x00\x00',
b'77959-TGG-G010\x00\x00',
b'77959-TGG-G110\x00\x00',
b'77959-TGG-J320\x00\x00',
@@ -503,12 +506,14 @@ FW_VERSIONS = {
b'78109-TGG-A810\x00\x00',
b'78109-TGG-A820\x00\x00',
b'78109-TGG-C220\x00\x00',
+ b'78109-TGG-E110\x00\x00',
b'78109-TGG-G030\x00\x00',
b'78109-TGG-G230\x00\x00',
b'78109-TGG-G410\x00\x00',
b'78109-TGK-Z410\x00\x00',
b'78109-TGL-G120\x00\x00',
b'78109-TGL-G130\x00\x00',
+ b'78109-TGL-G210\x00\x00',
b'78109-TGL-G230\x00\x00',
b'78109-TGL-GM10\x00\x00',
],
@@ -714,6 +719,7 @@ FW_VERSIONS = {
b'36161-TMC-Q030\x00\x00',
b'36161-TMC-Q040\x00\x00',
b'36161-TNY-A020\x00\x00',
+ b'36161-TNY-A030\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TLA-A240\x00\x00',
@@ -794,6 +800,7 @@ FW_VERSIONS = {
},
CAR.FIT: {
(Ecu.vsa, 0x18da28f1, None): [
+ b'57114-T5R-L020\x00\x00',
b'57114-T5R-L220\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
@@ -804,11 +811,13 @@ FW_VERSIONS = {
b'38897-T5A-J010\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
+ b'78109-T5A-A210\x00\x00',
b'78109-T5A-A410\x00\x00',
b'78109-T5A-A420\x00\x00',
b'78109-T5A-A910\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
+ b'36161-T5R-A040\x00\x00',
b'36161-T5R-A240\x00\x00',
b'36161-T5R-A520\x00\x00',
],
@@ -845,6 +854,7 @@ FW_VERSIONS = {
b'38897-THR-A020\x00\x00',
],
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
+ b'37805-5MR-4080\x00\x00',
b'37805-5MR-A240\x00\x00',
b'37805-5MR-A250\x00\x00',
b'37805-5MR-A310\x00\x00',
@@ -912,14 +922,18 @@ FW_VERSIONS = {
b'78109-THR-AC30\x00\x00',
b'78109-THR-AC40\x00\x00',
b'78109-THR-AC50\x00\x00',
+ b'78109-THR-AD30\x00\x00',
b'78109-THR-AE20\x00\x00',
+ b'78109-THR-AE30\x00\x00',
b'78109-THR-AE40\x00\x00',
b'78109-THR-AK10\x00\x00',
b'78109-THR-AL10\x00\x00',
b'78109-THR-AN10\x00\x00',
+ b'78109-THR-C220\x00\x00',
b'78109-THR-C330\x00\x00',
b'78109-THR-CE20\x00\x00',
b'78109-THR-DA20\x00\x00',
+ b'78109-THR-DA30\x00\x00',
b'78109-THR-DA40\x00\x00',
b'78109-THR-K120\x00\x00',
],
@@ -1046,6 +1060,35 @@ FW_VERSIONS = {
b'57114-TGT-A530\x00\x00',
],
},
+ CAR.PASSPORT: {
+ (Ecu.programmedFuelInjection, 0x18da10f1, None): [
+ b'37805-RLV-B220\x00\x00',
+ ],
+ (Ecu.eps, 0x18da30f1, None): [
+ b'39990-TGS-A230\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x18dab0f1, None): [
+ b'36161-TGS-A030\x00\x00',
+ ],
+ (Ecu.gateway, 0x18daeff1, None): [
+ b'38897-TG7-A040\x00\x00',
+ ],
+ (Ecu.srs, 0x18da53f1, None): [
+ b'77959-TGS-A010\x00\x00',
+ ],
+ (Ecu.shiftByWire, 0x18da0bf1, None): [
+ b'54008-TG7-A530\x00\x00',
+ ],
+ (Ecu.transmission, 0x18da1ef1, None): [
+ b'28101-5EZ-A600\x00\x00',
+ ],
+ (Ecu.combinationMeter, 0x18da60f1, None): [
+ b'78109-TGS-AT20\x00\x00',
+ ],
+ (Ecu.vsa, 0x18da28f1, None): [
+ b'57114-TGS-A530\x00\x00',
+ ],
+ },
CAR.ACURA_RDX: {
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TX5-A220\x00\x00',
@@ -1068,16 +1111,22 @@ FW_VERSIONS = {
},
CAR.ACURA_RDX_3G: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
+ b'37805-5YF-A130\x00\x00',
b'37805-5YF-A230\x00\x00',
b'37805-5YF-A320\x00\x00',
b'37805-5YF-A330\x00\x00',
b'37805-5YF-A420\x00\x00',
b'37805-5YF-A430\x00\x00',
+ b'37805-5YF-A750\x00\x00',
+ b'37805-5YF-A850\x00\x00',
b'37805-5YF-A870\x00\x00',
b'37805-5YF-C210\x00\x00',
+ b'37805-5YF-C220\x00\x00',
b'37805-5YF-C410\000\000',
+ b'37805-5YF-C420\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
+ b'57114-TJB-A030\x00\x00',
b'57114-TJB-A040\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
@@ -1091,26 +1140,35 @@ FW_VERSIONS = {
b'54008-TJB-A520\x00\x00',
],
(Ecu.transmission, 0x18da1ef1, None): [
+ b'28102-5YK-A610\x00\x00',
+ b'28102-5YK-A620\x00\x00',
b'28102-5YK-A630\x00\x00',
b'28102-5YK-A700\x00\x00',
b'28102-5YK-A711\x00\x00',
+ b'28102-5YL-A620\x00\x00',
b'28102-5YL-A700\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
+ b'78109-TJB-A140\x00\x00',
b'78109-TJB-A240\x00\x00',
+ b'78109-TJB-A420\x00\x00',
b'78109-TJB-AB10\x00\x00',
b'78109-TJB-AD10\x00\x00',
b'78109-TJB-AF10\x00\x00',
+ b'78109-TJB-AS10\000\000',
+ b'78109-TJB-AU10\x00\x00',
b'78109-TJB-AW10\x00\x00',
+ b'78109-TJC-A420\x00\x00',
b'78109-TJC-AA10\x00\x00',
b'78109-TJC-AD10\x00\x00',
- b'78109-TJB-AS10\000\000',
+ b'78109-TJC-AF10\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TJB-A040\x00\x00',
b'77959-TJB-A210\x00\x00',
],
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [
+ b'46114-TJB-A040\x00\x00',
b'46114-TJB-A050\x00\x00',
b'46114-TJB-A060\x00\x00',
],
@@ -1284,6 +1342,7 @@ DBC = {
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
+ CAR.PASSPORT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None),
CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None),
@@ -1306,5 +1365,7 @@ SPEED_FACTOR = {
}
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_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 19ff38129..a8b70fcf9 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -104,7 +104,8 @@ class CarController():
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
- CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022]:
+ CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
+ CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
# 5 Hz ACC options
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 7cc62b3cb..4db775bea 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -67,6 +67,7 @@ class CarState(CarStateBase):
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
+ ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
@@ -108,7 +109,6 @@ class CarState(CarStateBase):
self.clu11 = copy.copy(cp.vl["CLU11"])
self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
- self.brake_hold = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index dd43ec1a8..c8130fd72 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -17,8 +17,9 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
values["CF_Lkas_MsgCount"] = frame % 0x10
if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
- CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
- CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022]:
+ CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
+ CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.SANTA_FE_2022,
+ CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]:
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index b738beb9f..57b313e53 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -3,7 +3,7 @@ from cereal import car
from panda import Panda
from common.params import Params
from selfdrive.config import Conversions as CV
-from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
+from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
@@ -26,7 +26,7 @@ class CarInterface(CarInterfaceBase):
ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1]
# WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB)
- ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar") and candidate in [CAR.SONATA, CAR.SONATA_HYBRID, CAR.PALISADE, CAR.SANTA_FE]
+ ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar") and (candidate not in LEGACY_SAFETY_MODE_CAR)
ret.pcmCruise = not ret.openpilotLongitudinalControl
@@ -45,7 +45,7 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalActuatorDelayUpperBound = 1.0 # s
- if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022]:
+ if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.766
@@ -232,6 +232,13 @@ class CarInterface(CarInterfaceBase):
ret.mass = 1640.0 + STD_CARGO_KG
ret.wheelbase = 2.84
ret.steerRatio = 13.56
+ elif candidate == CAR.GENESIS_G70_2020:
+ ret.lateralTuning.pid.kf = 0.
+ ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.112], [0.004]]
+ ret.mass = 3673.0 * CV.LB_TO_KG + STD_CARGO_KG
+ ret.wheelbase = 2.83
+ ret.steerRatio = 12.9
elif candidate == CAR.GENESIS_G80:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
@@ -247,9 +254,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
# these cars require a special panda safety mode due to missing counters and checksums in the messages
- if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO,
- CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER,
- CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022]:
+ if candidate in LEGACY_SAFETY_MODE_CAR:
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundaiLegacy)]
# set appropriate safety param for gas signal
@@ -293,8 +298,6 @@ class CarInterface(CarInterfaceBase):
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
- if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
- events.add(EventName.brakeHold)
if self.CS.park_brake:
events.add(EventName.parkBrake)
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 2f2995e6b..ddcd00ba0 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -10,9 +10,10 @@ class CarControllerParams:
ACCEL_MAX = 2.0 # m/s
def __init__(self, CP):
- if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70,
+ if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G70_2020,
CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
- CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.KONA_EV, CAR.KONA, CAR.IONIQ_HEV_2022]:
+ CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
+ CAR.KIA_K5_2021, CAR.KONA_EV, CAR.KONA, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]:
self.STEER_MAX = 384
else:
self.STEER_MAX = 255
@@ -39,6 +40,7 @@ class CAR:
KONA_HEV = "HYUNDAI KONA HYBRID 2020"
SANTA_FE = "HYUNDAI SANTA FE 2019"
SANTA_FE_2022 = "HYUNDAI SANTA FE 2022"
+ SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022"
SONATA = "HYUNDAI SONATA 2020"
SONATA_LF = "HYUNDAI SONATA 2019"
PALISADE = "HYUNDAI PALISADE 2020"
@@ -60,6 +62,7 @@ class CAR:
# Genesis
GENESIS_G70 = "GENESIS G70 2018"
+ GENESIS_G70_2020 = "GENESIS G70 2020"
GENESIS_G80 = "GENESIS G80 2017"
GENESIS_G90 = "GENESIS G90 2017"
@@ -248,14 +251,15 @@ FW_VERSIONS = {
},
CAR.SONATA: {
(Ecu.fwdRadar, 0x7d0, None): [
+ b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ',
+ b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ',
b'\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ',
b'\xf1\x00DN8_ SCC F-CUP 1.00 1.02 99110-L1000 ',
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ',
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ',
b'\xf1\x00DN89110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ',
- b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ',
- b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\xf1\x8799110L0000\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ',
b'\xf1\x8799110L0000\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ',
],
(Ecu.esp, 0x7d1, None): [
@@ -273,16 +277,18 @@ FW_VERSIONS = {
b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
],
(Ecu.engine, 0x7e0, None): [
- b'\xf1\x82DNCVN5GMCCXXXF0A',
b'\xf1\x81HM6M1_0a0_F00',
b'\xf1\x82DNBVN5GMCCXXXDCA',
+ b'\xf1\x82DNBVN5GMCCXXXG2F',
b'\xf1\x82DNBWN5TMDCXXXG2E',
+ b'\xf1\x82DNCVN5GMCCXXXF0A',
b'\xf1\x82DNCVN5GMCCXXXG2B',
b'\xf1\x87391162M003',
b'\xf1\x87391162M013',
+ b'\xf1\x87391162M023',
b'HM6M1_0a0_F00',
- b'HM6M2_0a0_BD0',
b'HM6M1_0a0_G20',
+ b'HM6M2_0a0_BD0',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101',
@@ -303,6 +309,9 @@ FW_VERSIONS = {
b'\xf1\x00DN8 MFC AT RUS LHD 1.00 1.03 99211-L1000 190705',
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.00 99211-L0000 190716',
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016',
+ b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.03 99211-L0000 210603',
+ b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.05 99211-L1000 201109',
+ b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.06 99211-L1000 210325',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
@@ -315,10 +324,14 @@ FW_VERSIONS = {
b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:',
b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5',
b'\xf1\x87954A02N060\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5',
+ 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\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',
b'\xf1\x87SALDBA3753044GJ3UUeVff\x86hwwwwvwwgvfgfvo\xf9\xfffU_\xffC\xae\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
+ b'\xf1\x87SALDBA3862294GJ3vfvgvefVxw\x87\x87w\x88\x87xwwwwc_\xf9\xff\x87w\x9f\xff\xd5\xdc\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA3873834GJ3fefVwuwWx\x88\x97\x88w\x88\x97xww\x87wU_\xfb\xff\x86f\x8f\xffN\x04\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
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',
@@ -326,8 +339,27 @@ FW_VERSIONS = {
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\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\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\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\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',
b'\xf1\x87SAMDBA8054504GJ3gw\x87xffvgffffwwwweUVUf?\xfc\xffvU_\xff\xddl\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00',
+ b'\xf1\x87SAMFB41553621GC7ww\x87xUU\x85Xvwwg\x88\x88\x88\x88wwgw\x86\xaf\xfb\xffuDo\xff\xaa\x8f\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc',
+ b'\xf1\x87SAMFB42555421GC7\x88\x88\x88\x88wvwgx\x88\x87\x88wwgw\x87wxw3\x8f\xfc\xff\x98f\x8f\xffga\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc',
+ b'\xf1\x87SAMFBA7978674GJ2gw\x87xgw\x97ywwwwvUGeUUeU\x87O\xfb\xff\x98w\x8f\xfffF\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc',
+ b'\xf1\x87SAMFBA9283024GJ2wwwwEUuWwwgwwwwwwwww\x87/\xfb\xff\x98w\x8f\xff<\xd3\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc',
+ b'\xf1\x87SAMFBA9708354GJ2wwwwVf\x86h\x88wx\x87xww\x87\x88\x88\x88\x88w/\xfa\xff\x97w\x8f\xff\x86\xa0\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc',
],
},
CAR.SONATA_LF: {
@@ -347,11 +379,12 @@ FW_VERSIONS = {
b'\xf1\x00LFF LKAS AT USA LHD 1.00 1.01 95740-C1000 E51',
],
(Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5',
+ b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\x00\x00\x00\x00',
+ b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5',
+ b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24SL2n\x8d\xbe\xd8',
b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2\x00\x00\x00\x00',
b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm',
- b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5',
- b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\x00\x00\x00\x00',
- b'\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5',
],
},
CAR.SANTA_FE: {
@@ -403,10 +436,12 @@ FW_VERSIONS = {
b'\xf1\x87SBJWAA7780564GG0wvwgUUeVwwwwx\x88\x87\x88wwwwd_\xfc\xff\x86f\x7f\xff\xd7*\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0',
b'\xf1\x87SBJWAA8278284GG0ffvgUU\x85Xx\x88\x87\x88x\x88w\x88ww\x87w\x96o\xfd\xff\xa7U_\xff\xf2\xa0\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0',
b'\xf1\x87SBLWAA4363244GG0wvwgwv\x87hgw\x86ww\x88\x87xww\x87wdo\xfb\xff\x86f\x7f\xff3$\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS6\x00\x00\x00\x00',
+ b'\xf1\x87SBLWAA4363244GG0wvwgwv\x87hgw\x86ww\x88\x87xww\x87wdo\xfb\xff\x86f\x7f\xff3$\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS6x0\x17\xfe',
+ b'\xf1\x87SBLWAA4899564GG0VfvgUU\x85Xx\x88\x87\x88vfgf\x87wxwvO\xfb\xff\x97f\xb1\xffSB\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7\x00\x00\x00\x00',
b'\xf1\x87SBLWAA6622844GG0wwwwff\x86hwwwwx\x88\x87\x88\x88\x88\x88\x88\x98?\xfd\xff\xa9\x88\x7f\xffn\xe5\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7u\x1e{\x1c',
b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2\x00\x00\x00\x00',
+ b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2K\xdaV0',
b'\xf1\x87SDKXAA2443414GG1vfvgwv\x87h\x88\x88\x88\x88ww\x87wwwww\x99_\xfc\xffvD?\xffl\xd2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4G24NS6\x00\x00\x00\x00',
- b'\xf1\x87SBLWAA4899564GG0VfvgUU\x85Xx\x88\x87\x88vfgf\x87wxwvO\xfb\xff\x97f\xb1\xffSB\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7\x00\x00\x00\x00',
],
},
CAR.SANTA_FE_2022: {
@@ -437,6 +472,23 @@ FW_VERSIONS = {
b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7',
],
},
+ CAR.SANTA_FE_HEV_2022: {
+ (Ecu.fwdRadar, 0x7d0, None): [
+ b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ',
+ ],
+ (Ecu.eps, 0x7d4, None): [
+ b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102',
+ ],
+ (Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\x00TMH MFC AT USA LHD 1.00 1.03 99211-S1500 210224',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TTM2H16SA2\x80\xd7l\xb2',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x87391312MTC1',
+ ],
+ },
CAR.KIA_STINGER: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 ',
@@ -453,6 +505,7 @@ FW_VERSIONS = {
b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5220 4C2VL104',
b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5420 4C4VL104',
b'\xf1\x00CK MDPS R 1.00 1.06 57700-J5420 4C4VL106',
+ b'\xf1\x00CK MDPS R 1.00 1.07 57700-J5220 4C2VL107',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 95740-J5000 170822',
@@ -472,6 +525,8 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\000LX2_ SCC F-CUP 1.00 1.05 99110-S8100 ',
b'\xf1\x00LX2 SCC FHCUP 1.00 1.04 99110-S8100 ',
+ b'\xf1\x00LX2_ SCC FHCU- 1.00 1.05 99110-S8100 ',
+ 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 ',
],
@@ -489,10 +544,12 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00',
b'\xf1\x81640K0051\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\xf1\x81640S1051\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00LX2 MDPS C 1,00 1,03 56310-S8020 4LXDC103', # modified firmware
b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103',
+ b'\xf1\x00LX2 MDPS C 1.00 1.04 56310-S8020 4LXDC104',
b'\xf1\x00ON MDPS C 1.00 1.00 56340-S9000 8B13',
b'\xf1\x00ON MDPS C 1.00 1.01 56340-S9000 9201',
],
@@ -507,25 +564,42 @@ FW_VERSIONS = {
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28',
b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6',
+ b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1',
b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',
b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',
b'\xf1\x87LBLUFN731381KF36\xb9\x99\x89\x98\x98\x88\x88\x88\x89\x99\xa8\x99\x88\x99\xa8\x89\x88\x88\x98\x88V\177\xf6\xff\x99w\x8f\xff\xad\xd8\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\000bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',
+ b'\xf1\x87LDKVAA0028604HH1\xa8\x88x\x87vgvw\x88\x99\xa8\x89gw\x86ww\x88\x97x\x97o\xf9\xff\x97w\x7f\xffo\x02\xf1\x81U872\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28',
+ b'\xf1\x87LDKVAA3068374HH1wwww\x87xw\x87y\x99\xa7\x99w\x88\x87xw\x88\x97x\x85\xaf\xfa\xffvU/\xffU\xdc\xf1\x81U872\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28',
b'\xf1\x87LDKVBN382172KF26\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\xa7\x89\x87\x88\x98x\x98\x99\xa9\x89\xa5_\xf6\xffDDO\xff\xcd\x16\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
b'\xf1\x87LDKVBN540766KF37\x87wgv\x87w\x87xx\x99\x97\x89v\x88\x97h\x88\x88\x88\x88x\x7f\xf6\xffvUo\xff\xd3\x01\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
+ b'\xf1\x87LDLVAA4225634HH1\x98\x88\x88\x88eUeVx\x88\x87\x88g\x88\x86xx\x88\x87\x88\x86o\xf9\xff\x87w\x7f\xff\xf2\xf7\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6',
+ b'\xf1\x87LDLVAA4777834HH1\x98\x88x\x87\x87wwwx\x88\x87\x88x\x99\x97\x89x\x88\x97\x88\x86o\xfa\xff\x86fO\xff\x1d9\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6',
+ b'\xf1\x87LDLVAA5194534HH1ffvguUUUx\x88\xa7\x88h\x99\x96\x89x\x88\x97\x88ro\xf9\xff\x98wo\xff\xaaM\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6',
+ b'\xf1\x87LDLVAA5949924HH1\xa9\x99y\x97\x87wwwx\x99\x97\x89x\x99\xa7\x89x\x99\xa7\x89\x87_\xfa\xffeD?\xff\xf1\xfd\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6',
b'\xf1\x87LDLVBN560098KF26\x86fff\x87vgfg\x88\x96xfw\x86gfw\x86g\x95\xf6\xffeU_\xff\x92c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
+ b'\xf1\x87LDLVBN602045KF26\xb9\x99\x89\x98\x97vwgy\xaa\xb7\x9af\x88\x96hw\x99\xa7y\xa9\x7f\xf5\xff\x99w\x7f\xff,\xd3\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
+ b'\xf1\x87LDLVBN628911KF26\xa9\x99\x89\x98\x98\x88\x88\x88y\x99\xa7\x99fw\x86gw\x88\x87x\x83\x7f\xf6\xff\x98wo\xff2\xda\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN645817KF37\x87www\x98\x87xwx\x99\x97\x89\x99\x99\x99\x99g\x88\x96x\xb6_\xf7\xff\x98fo\xff\xe2\x86\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN662115KF37\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\x97\x89x\x99\xa7\x89\x88\x99\xa8\x89\x88\x7f\xf7\xfffD_\xff\xdc\x84\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN667933KF37\xb9\x99\x89\x98\xb9\x99\x99\x99x\x88\x87\x88w\x88\x87x\x88\x88\x98\x88\xcbo\xf7\xffe3/\xffQ!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN673087KF37\x97www\x86fvgx\x99\x97\x89\x99\xaa\xa9\x9ag\x88\x86x\xe9_\xf8\xff\x98w\x7f\xff"\xad\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
+ b'\xf1\x87LDLVBN673841KF37\x98\x88x\x87\x86g\x86xy\x99\xa7\x99\x88\x99\xa8\x89w\x88\x97xdo\xf5\xff\x98\x88\x8f\xffT\xec\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN681363KF37\x98\x88\x88\x88\x97x\x87\x88y\xaa\xa7\x9a\x88\x88\x98\x88\x88\x88\x88\x88vo\xf6\xffvD\x7f\xff%v\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
+ b'\xf1\x87LDLVBN713782KF37\x99\x99y\x97\x98\x88\x88\x88x\x88\x97\x88\x88\x99\x98\x89\x88\x99\xa8\x89\x87o\xf7\xffeU?\xff7,\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN713890KF26\xb9\x99\x89\x98\xa9\x99\x99\x99x\x99\x97\x89\x88\x99\xa8\x89\x88\x99\xb8\x89Do\xf7\xff\xa9\x88o\xffs\r\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN733215KF37\x99\x98y\x87\x97wwwi\x99\xa6\x99x\x99\xa7\x89V\x88\x95h\x86o\xf7\xffeDO\xff\x12\xe7\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN750044KF37\xca\xa9\x8a\x98\xa7wwwy\xaa\xb7\x9ag\x88\x96x\x88\x99\xa8\x89\xb9\x7f\xf6\xff\xa8w\x7f\xff\xbe\xde\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN752612KF37\xba\xaa\x8a\xa8\x87w\x87xy\xaa\xa7\x9a\x88\x99\x98\x89x\x88\x97\x88\x96o\xf6\xffvU_\xffh\x1b\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x87LDLVBN755553KF37\x87xw\x87\x97w\x87xy\x99\xa7\x99\x99\x99\xa9\x99Vw\x95gwo\xf6\xffwUO\xff\xb5T\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
+ 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\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\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\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",
],
},
@@ -557,6 +631,25 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', ],
},
+ CAR.GENESIS_G70_2020: {
+ (Ecu.eps, 0x7d4, None): [
+ b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9220 4I2VL107',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00'
+ b'\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL',
+ ],
+ (Ecu.fwdRadar, 0x7d0, None): [
+ b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ',
+ b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 \xf1\xa01.02',
+ ],
+ (Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ },
CAR.KONA: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP 1.00 1.00 95655-J9200 ', ],
(Ecu.esp, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ],
@@ -570,11 +663,15 @@ FW_VERSIONS = {
(Ecu.esp, 0x7D4, None): [b'\xf1\000CD MDPS C 1.00 1.06 56310-XX000 4CDEC106', ],
(Ecu.fwdCamera, 0x7C4, None): [b'\xf1\000CD LKAS AT EUR LHD 1.00 1.01 99211-J7000 B40', ],
(Ecu.engine, 0x7E0, None): [b'\001TCD-JECU4F202H0K', ],
- (Ecu.transmission, 0x7E1, None): [b'\xf1\x816U2V7051\000\000\xf1\0006U2V0_C2\000\0006U2V7051\000\000DCD0T14US1\000\000\000\000', ],
+ (Ecu.transmission, 0x7E1, None): [
+ b'\xf1\x816U2V7051\000\000\xf1\0006U2V0_C2\000\0006U2V7051\000\000DCD0T14US1\000\000\000\000',
+ b'\xf1\x816U2V7051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V7051\x00\x00DCD0T14US1U\x867Z',
+ ],
(Ecu.esp, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ],
},
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 56310M6300\x00 4BDDC108',
],
(Ecu.fwdCamera, 0x7C4, None): [
@@ -585,6 +682,7 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x7e0, None): [
b'\x01TBDM1NU06F200H01',
+ b'391182B945\x00',
],
(Ecu.esp, 0x7d1, None): [
b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
@@ -597,24 +695,29 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x7D0, None): [
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 ',
],
(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',
],
(Ecu.fwdCamera, 0x7C4, None): [
- b'\xf1\000DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915',
+ b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915',
],
(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',
],
(Ecu.engine, 0x7E0, None): [
- b'\xf1\x87391212MKT0\xf1\xa00240',
b'\xf1\x87391212MKT0',
+ b'\xf1\x87391212MKV0',
],
(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',
],
},
CAR.KONA_EV: {
@@ -749,17 +852,17 @@ FW_VERSIONS = {
],
(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',
+ b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106',
b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106',
- b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106\xf1\xa01.06',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819',
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AA000 200819',
],
(Ecu.esp, 0x7d1, None): [
- b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
- b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800\xf1\xa01.04',
+ b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800',
+ b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -776,22 +879,23 @@ FW_VERSIONS = {
},
CAR.ELANTRA_HEV_2021: {
(Ecu.fwdCamera, 0x7c4, None) : [
- b'\xf1\000CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819'
+ b'\xf1\000CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819',
],
(Ecu.fwdRadar, 0x7d0, None) : [
- b'\xf1\000CNhe SCC FHCUP 1.00 1.01 99110-BY000 '
+ b'\xf1\000CNhe SCC FHCUP 1.00 1.01 99110-BY000 ',
+ b'\xf1\x8799110BY000\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ',
],
(Ecu.eps, 0x7d4, None) :[
- b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102'
+ b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102',
],
(Ecu.transmission, 0x7e1, None) :[
b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa',
b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000',
b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa',
- b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000'
+ b'\xf1\x816U3K3051\x00\x00\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None) : [
- b'\xf1\x816H6G5051\000\000\000\000\000\000\000\000'
+ b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00',
]
},
CAR.KONA_HEV: {
@@ -837,6 +941,7 @@ FW_VERSIONS = {
b'\xf1\000PSBG2323 E09\000\000\000\000\000\000\000TDN2H20SA5\x97R\x88\x9e',
b'\xf1\000PSBG2333 E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab',
b'\xf1\x87PCU\000\000\000\000\000\000\000\000\000\xf1\x81E16\000\000\000\000\000\000\000\xf1\000PSBG2333 E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab',
+ b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x87391162J012',
@@ -846,7 +951,7 @@ FW_VERSIONS = {
}
CHECKSUM = {
- "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021],
+ "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022],
"6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
}
@@ -854,15 +959,18 @@ FEATURES = {
# which message has the gear
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
"use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER]),
- "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022]),
+ "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]),
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
- "use_fca": set([CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022]),
+ "use_fca": set([CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]),
}
-HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022]) # these cars use a different gas signal
+HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022]) # these cars use a different gas signal
EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV])
+# these cars require a special panda safety mode due to missing counters and checksums in the messages
+LEGACY_SAFETY_MODE_CAR = set([CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022])
+
# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py
DBC = {
@@ -871,6 +979,7 @@ DBC = {
CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None),
+ CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None),
CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
@@ -894,6 +1003,7 @@ DBC = {
CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None),
+ CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index ada906367..336c1ee6c 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -30,15 +30,16 @@ class CarInterfaceBase():
self.VM = VehicleModel(CP)
self.frame = 0
- self.steer_warning = 0
self.steering_unpressed = 0
self.low_speed_alert = False
+ self.silent_steer_warning = True
if CarState is not None:
self.CS = CarState(CP)
self.cp = self.CS.get_can_parser(CP)
self.cp_cam = self.CS.get_cam_can_parser(CP)
self.cp_body = self.CS.get_body_can_parser(CP)
+ self.cp_loopback = self.CS.get_loopback_can_parser(CP)
self.CC = None
if CarController is not None:
@@ -134,21 +135,23 @@ class CarInterfaceBase():
events.add(EventName.speedTooHigh)
if cs_out.cruiseState.nonAdaptive:
events.add(EventName.wrongCruiseMode)
+ if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
+ events.add(EventName.brakeHold)
- self.steer_warning = self.steer_warning + 1 if cs_out.steerWarning else 0
- self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
# Handle permanent and temporary steering faults
+ self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
+ if cs_out.steerWarning:
+ # if the user overrode recently, show a less harsh alert
+ if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
+ self.silent_steer_warning = True
+ events.add(EventName.steerTempUnavailableSilent)
+ else:
+ events.add(EventName.steerTempUnavailable)
+ else:
+ self.silent_steer_warning = False
if cs_out.steerError:
events.add(EventName.steerUnavailable)
- elif cs_out.steerWarning:
- # only escalate to the harsher alert after the condition has
- # persisted for 0.5s and we're certain that the user isn't overriding
- if not cs_out.standstill and self.steering_unpressed > int(0.5 / DT_CTRL) and \
- self.steer_warning > int(0.5 / DT_CTRL):
- events.add(EventName.steerTempUnavailable)
- else:
- events.add(EventName.steerTempUnavailableSilent)
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
# Optionally allow to press gas at zero speed to resume.
@@ -254,3 +257,7 @@ class CarStateBase:
@staticmethod
def get_body_can_parser(CP):
return None
+
+ @staticmethod
+ def get_loopback_can_parser(CP):
+ return None
diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py
index bda5378dd..06c5eb094 100644
--- a/selfdrive/car/mazda/carcontroller.py
+++ b/selfdrive/car/mazda/carcontroller.py
@@ -1,8 +1,11 @@
+from cereal import car
+from opendbc.can.packer import CANPacker
from selfdrive.car.mazda import mazdacan
from selfdrive.car.mazda.values import CarControllerParams, Buttons
-from opendbc.can.packer import CANPacker
from selfdrive.car import apply_std_steer_torque_limits
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+
class CarController():
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
@@ -10,16 +13,15 @@ class CarController():
self.steer_rate_limited = False
self.brake_counter = 0
- def update(self, enabled, CS, frame, actuators):
- """ Controls thread """
-
+ def update(self, c, CS, frame):
can_sends = []
- ### STEER ###
+ apply_steer = 0
+ self.steer_rate_limited = False
- if enabled:
+ if c.enabled:
# calculate steer and also set limits due to driver torque
- new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
+ new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorque, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
@@ -28,24 +30,32 @@ class CarController():
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
# TODO: improve the resume trigger logic by looking at actual radar data
- can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.RESUME))
+ can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
+
+ if c.cruiseControl.cancel or (CS.out.cruiseState.enabled and not c.enabled):
+ # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
+ # a race condition with the stock system, where the second cancel from openpilot
+ # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
+ # read 3 messages and most likely sync state before we attempt cancel.
+ self.brake_counter = self.brake_counter + 1
+ if frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
+ # Cancel Stock ACC if it's enabled while OP is disengaged
+ # Send at a rate of 10hz until we sync with stock ACC state
+ can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
else:
- apply_steer = 0
- self.steer_rate_limited = False
- if CS.out.cruiseState.enabled:
- # if brake is pressed, let us wait >20ms before trying to disable crz to avoid
- # a race condition with the stock system, where the second cancel from openpilot
- # will disable the crz 'main on'
- self.brake_counter = self.brake_counter + 1
- if frame % 20 == 0 and not (CS.out.brakePressed and self.brake_counter < 3):
- # Cancel Stock ACC if it's enabled while OP is disengaged
- # Send at a rate of 5hz until we sync with stock ACC state
- can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.CANCEL))
- else:
- self.brake_counter = 0
+ self.brake_counter = 0
self.apply_steer_last = apply_steer
+ # send HUD alerts
+ if frame % 50 == 0:
+ ldw = c.hudControl.visualAlert == VisualAlert.ldw
+ steer_required = c.hudControl.visualAlert == VisualAlert.steerRequired
+ # TODO: find a way to silence audible warnings so we can add more hud alerts
+ steer_required = steer_required and CS.lkas_allowed_speed
+ can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
+
+ # send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
return can_sends
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index a96c68b0e..4140a4e07 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -12,11 +12,10 @@ class CarState(CarStateBase):
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["GEAR"]["GEAR"]
- self.cruise_speed = 0
+ self.crz_btns_counter = 0
self.acc_active_last = False
- self.low_speed_lockout = True
self.low_speed_alert = False
- self.lkas_allowed = False
+ self.lkas_allowed_speed = False
def update(self, cp, cp_cam):
@@ -35,8 +34,11 @@ class CarState(CarStateBase):
can_gear = int(cp.vl["GEAR"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
- ret.leftBlinker = cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1
- ret.rightBlinker = cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1
+ ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"])
+ ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1
+ ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1
+ ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1,
+ cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1)
ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"]
ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"]
@@ -45,6 +47,7 @@ class CarState(CarStateBase):
ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"]
ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"]
+ # TODO: this should be from 0 - 1.
ret.brakePressed = cp.vl["PEDALS"]["BRAKE_ON"] == 1
ret.brake = cp.vl["BRAKE"]["BRAKE_PRESSURE"]
@@ -52,39 +55,43 @@ class CarState(CarStateBase):
ret.doorOpen = any([cp.vl["DOORS"]["FL"], cp.vl["DOORS"]["FR"],
cp.vl["DOORS"]["BL"], cp.vl["DOORS"]["BR"]])
+ # TODO: this should be from 0 - 1.
ret.gas = cp.vl["ENGINE_DATA"]["PEDAL_GAS"]
ret.gasPressed = ret.gas > 0
- ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1
- ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1
+ # Either due to low speed or hands off
+ lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1
# LKAS is enabled at 52kph going up and disabled at 45kph going down
- if speed_kph > LKAS_LIMITS.ENABLE_SPEED:
- self.lkas_allowed = True
+ # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes
+ if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked:
+ self.lkas_allowed_speed = True
elif speed_kph < LKAS_LIMITS.DISABLE_SPEED:
- self.lkas_allowed = False
+ self.lkas_allowed_speed = False
+ # TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on
+ # it should be used for carState.cruiseState.nonAdaptive instead
ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
if ret.cruiseState.enabled:
- if not self.lkas_allowed:
- if not self.acc_active_last:
- self.low_speed_lockout = True
- else:
- self.low_speed_alert = True
+ if not self.lkas_allowed_speed and self.acc_active_last:
+ self.low_speed_alert = True
else:
- self.low_speed_lockout = False
self.low_speed_alert = False
# Check if LKAS is disabled due to lack of driver torque when all other states indicate
- # it should be enabled (steer lockout)
- ret.steerWarning = self.lkas_allowed and (cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1)
+ # it should be enabled (steer lockout). Don't warn until we actually get lkas active
+ # and lose it again, i.e, after initial lkas activation
+
+ ret.steerWarning = self.lkas_allowed_speed and lkas_blocked
self.acc_active_last = ret.cruiseState.enabled
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
+ self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
+ self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
return ret
@@ -96,6 +103,7 @@ class CarState(CarStateBase):
# sig_name, sig_address, default
("LEFT_BLINK", "BLINK_INFO", 0),
("RIGHT_BLINK", "BLINK_INFO", 0),
+ ("HIGH_BEAMS", "BLINK_INFO", 0),
("STEER_ANGLE", "STEER", 0),
("STEER_ANGLE_RATE", "STEER_RATE", 0),
("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0),
@@ -134,9 +142,6 @@ class CarState(CarStateBase):
("BR", "DOORS", 0),
("PEDAL_GAS", "ENGINE_DATA", 0),
("SPEED", "ENGINE_DATA", 0),
- ("RES", "CRZ_BTNS", 0),
- ("SET_P", "CRZ_BTNS", 0),
- ("SET_M", "CRZ_BTNS", 0),
("CTR", "CRZ_BTNS", 0),
("LEFT_BS1", "BSM", 0),
("RIGHT_BS1", "BSM", 0),
@@ -165,21 +170,31 @@ class CarState(CarStateBase):
if CP.carFingerprint in GEN1:
signals += [
# sig_name, sig_address, default
- ("LKAS_REQUEST", "CAM_LKAS", 0),
- ("CTR", "CAM_LKAS", 0),
- ("ERR_BIT_1", "CAM_LKAS", 0),
+ ("LKAS_REQUEST", "CAM_LKAS", 0),
+ ("CTR", "CAM_LKAS", 0),
+ ("ERR_BIT_1", "CAM_LKAS", 0),
("LINE_NOT_VISIBLE", "CAM_LKAS", 0),
- ("LDW", "CAM_LKAS", 0),
- ("BIT_1", "CAM_LKAS", 1),
- ("ERR_BIT_2", "CAM_LKAS", 0),
- ("STEERING_ANGLE", "CAM_LKAS", 0),
- ("ANGLE_ENABLED", "CAM_LKAS", 0),
- ("CHKSUM", "CAM_LKAS", 0),
+ ("BIT_1", "CAM_LKAS", 1),
+ ("ERR_BIT_2", "CAM_LKAS", 0),
+ ("STEERING_ANGLE", "CAM_LKAS", 0),
+ ("ANGLE_ENABLED", "CAM_LKAS", 0),
+ ("CHKSUM", "CAM_LKAS", 0),
+
+ ("LINE_VISIBLE", "CAM_LANEINFO", 0),
+ ("LINE_NOT_VISIBLE", "CAM_LANEINFO", 1),
+ ("LANE_LINES", "CAM_LANEINFO", 0),
+ ("BIT1", "CAM_LANEINFO", 0),
+ ("BIT2", "CAM_LANEINFO", 0),
+ ("BIT3", "CAM_LANEINFO", 0),
+ ("NO_ERR_BIT", "CAM_LANEINFO", 1),
+ ("S1", "CAM_LANEINFO", 0),
+ ("S1_HBEAM", "CAM_LANEINFO", 0),
]
checks += [
# sig_address, frequency
- ("CAM_LKAS", 16),
+ ("CAM_LANEINFO", 2),
+ ("CAM_LKAS", 16),
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index 5472a0297..8364bf000 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -22,7 +22,7 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
ret.radarOffCan = True
- ret.dashcamOnly = True
+ ret.dashcamOnly = candidate not in [CAR.CX9_2021]
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1.0
@@ -86,9 +86,6 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret)
- if self.CS.low_speed_lockout:
- events.add(EventName.belowEngageSpeed)
-
if self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
@@ -98,6 +95,6 @@ class CarInterface(CarInterfaceBase):
return self.CS.out
def apply(self, c):
- can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators)
+ can_sends = self.CC.update(c, self.CS, self.frame)
self.frame += 1
return can_sends
diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py
index 456f7f672..e2ee93e02 100644
--- a/selfdrive/car/mazda/mazdacan.py
+++ b/selfdrive/car/mazda/mazdacan.py
@@ -1,5 +1,8 @@
+import copy
+
from selfdrive.car.mazda.values import GEN1, Buttons
+
def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
tmp = apply_steer + 2048
@@ -7,10 +10,11 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
lo = tmp & 0xFF
hi = tmp >> 8
+ # copy values from camera
b1 = int(lkas["BIT_1"])
- ldw = int(lkas["LDW"])
er1 = int(lkas["ERR_BIT_1"])
lnv = 0
+ ldw = 0
er2 = int(lkas["ERR_BIT_2"])
# Some older models do have these, newer models don't.
@@ -28,7 +32,7 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
# bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ]
csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
- #bytes [ 5 ] [ 6 ] [ 7 ]
+ # bytes [ 5 ] [ 6 ] [ 7 ]
csum = csum - ahi - amd - alo - b2
if ahi == 1:
@@ -44,63 +48,72 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
if car_fingerprint in GEN1:
values = {
- "LKAS_REQUEST" : apply_steer,
- "CTR" : ctr,
- "ERR_BIT_1" : er1,
+ "LKAS_REQUEST": apply_steer,
+ "CTR": ctr,
+ "ERR_BIT_1": er1,
"LINE_NOT_VISIBLE" : lnv,
- "LDW" : ldw,
- "BIT_1" : b1,
- "ERR_BIT_2" : er2,
- "STEERING_ANGLE" : steering_angle,
- "ANGLE_ENABLED" : b2,
- "CHKSUM" : csum
+ "LDW": ldw,
+ "BIT_1": b1,
+ "ERR_BIT_2": er2,
+ "STEERING_ANGLE": steering_angle,
+ "ANGLE_ENABLED": b2,
+ "CHKSUM": csum
}
return packer.make_can_msg("CAM_LKAS", 0, values)
-def create_button_cmd(packer, car_fingerprint, button):
+def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
+ values = copy.copy(cam_msg)
+ values.update({
+ # TODO: what's the difference between all these? do we need to send all?
+ "HANDS_WARN_3_BITS": 0b111 if steer_required else 0,
+ "HANDS_ON_STEER_WARN": steer_required,
+ "HANDS_ON_STEER_WARN_2": steer_required,
- if button == Buttons.CANCEL:
- can = 1
- res = 0
- elif button == Buttons.RESUME:
- can = 0
- res = 1
- else:
- can = 0
- res = 0
+ # TODO: right lane works, left doesn't
+ # TODO: need to do something about L/R
+ "LDW_WARN_LL": 0,
+ "LDW_WARN_RL": 0,
+ })
+ return packer.make_can_msg("CAM_LANEINFO", 0, values)
+
+
+def create_button_cmd(packer, car_fingerprint, counter, button):
+
+ can = int(button == Buttons.CANCEL)
+ res = int(button == Buttons.RESUME)
if car_fingerprint in GEN1:
values = {
- "CAN_OFF" : can,
- "CAN_OFF_INV" : (can + 1) % 2,
+ "CAN_OFF": can,
+ "CAN_OFF_INV": (can + 1) % 2,
- "SET_P" : 0,
- "SET_P_INV" : 1,
+ "SET_P": 0,
+ "SET_P_INV": 1,
- "RES" : res,
- "RES_INV" : (res + 1) % 2,
+ "RES": res,
+ "RES_INV": (res + 1) % 2,
- "SET_M" : 0,
- "SET_M_INV" : 1,
+ "SET_M": 0,
+ "SET_M_INV": 1,
- "DISTANCE_LESS" : 0,
- "DISTANCE_LESS_INV" : 1,
+ "DISTANCE_LESS": 0,
+ "DISTANCE_LESS_INV": 1,
- "DISTANCE_MORE" : 0,
- "DISTANCE_MORE_INV" : 1,
+ "DISTANCE_MORE": 0,
+ "DISTANCE_MORE_INV": 1,
- "MODE_X" : 0,
- "MODE_X_INV" : 1,
+ "MODE_X": 0,
+ "MODE_X_INV": 1,
- "MODE_Y" : 0,
- "MODE_Y_INV" : 1,
+ "MODE_Y": 0,
+ "MODE_Y_INV": 1,
- "BIT1" : 1,
- "BIT2" : 1,
- "BIT3" : 1,
- "CTR" : 0
+ "BIT1": 1,
+ "BIT2": 1,
+ "BIT3": 1,
+ "CTR": (counter + 1) % 16,
}
return packer.make_can_msg("CRZ_BTNS", 0, values)
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index 4e4ecd341..ada3ea1dd 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -46,13 +46,14 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x7e0, None): [
b'PA53-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFA-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2G-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PX2H-188K2-D\0x0\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PX2H-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PX2K-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX38-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX42-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX68-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -67,14 +68,14 @@ FW_VERSIONS = {
],
(Ecu.esp, 0x760, None): [
b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'KBJ5-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KL2K-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KN0W-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'KBJ5-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
- b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -82,14 +83,15 @@ FW_VERSIONS = {
(Ecu.transmission, 0x7e1, None): [
b'PA66-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX39-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PX39-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB1-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'SH9T-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
@@ -101,55 +103,59 @@ FW_VERSIONS = {
b'KJ01-3210X-L-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
- b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX23-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX24-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXN8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PXN8-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYD8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
- b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x760, None): [
+ b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TM53-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TN40-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
- b'TK80-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- ],
+ b'TK80-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
(Ecu.transmission, 0x7e1, None): [
+ b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD5-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD5-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYD6-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAZDA3: {
(Ecu.eps, 0x730, None): [
+ b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KR11-3210X-K-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'P5JD-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKC-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKE-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
+ b'B63C-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GHP9-67Y10---41\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'B63C-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x760, None): [
b'B45A-437AS-0-08\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -173,8 +179,8 @@ FW_VERSIONS = {
b'GFBC-3210X-A-00\000\000\000\000\000\000\000\000\000',
],
(Ecu.engine, 0x7e0, None): [
- b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX4F-188K2-D\000\000\000\000\000\000\000\000\000\000\000\000',
+ b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -189,8 +195,8 @@ FW_VERSIONS = {
b'GSH7-67XK2-P\000\000\000\000\000\000\000\000\000\000\000\000',
],
(Ecu.transmission, 0x7e1, None): [
- b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYH3-21PS1-D\000\000\000\000\000\000\000\000\000\000\000\000',
+ b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py
index 4e5e02cdf..a4019b538 100644
--- a/selfdrive/car/nissan/carstate.py
+++ b/selfdrive/car/nissan/carstate.py
@@ -33,7 +33,7 @@ class CarState(CarStateBase):
if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]:
ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"])
elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]:
- ret.brakePressed = bool(cp.vl["BRAKE_PEDAL"]["BRAKE_PEDAL"] > 3)
+ 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
@@ -184,8 +184,7 @@ class CarState(CarStateBase):
elif CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]:
signals += [
- ("BRAKE_PEDAL", "BRAKE_PEDAL", 0),
-
+ ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 1),
("GAS_PEDAL", "CRUISE_THROTTLE", 0),
("CRUISE_AVAILABLE", "CRUISE_THROTTLE", 0),
("SPEED_MPH", "HUD_SETTINGS", 0),
diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py
index 3c91d3d22..22bd4a129 100644
--- a/selfdrive/car/nissan/values.py
+++ b/selfdrive/car/nissan/values.py
@@ -1,6 +1,8 @@
# flake8: noqa
from selfdrive.car import dbc_dict
+from cereal import car
+Ecu = car.CarParams.Ecu
class CarControllerParams:
@@ -55,6 +57,63 @@ FINGERPRINTS = {
]
}
+FW_VERSIONS = {
+ CAR.ALTIMA: {
+ (Ecu.fwdCamera, 0x707, None): [
+ b'284N86CA1D',
+ ],
+ (Ecu.eps, 0x742, None): [
+ b'6CA2B\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'237109HE2B',
+ ],
+ (Ecu.gateway, 0x18dad0f1, None): [
+ b'284U29HE0A',
+ ],
+ },
+ CAR.LEAF_IC: {
+ (Ecu.fwdCamera, 0x707, None): [
+ b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80',
+ b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80',
+ ],
+ (Ecu.esp, 0x740, None): [
+ b'476605SH1D',
+ b'476605SK2A',
+ ],
+ (Ecu.eps, 0x742, None): [
+ b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80',
+ b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80',
+ ],
+ (Ecu.gateway, 0x18dad0f1, None): [
+ b'284U25SH3A',
+ b'284U25SK2D',
+ ],
+ },
+ CAR.XTRAIL: {
+ (Ecu.fwdCamera, 0x707, None): [
+ b'284N86FR2A',
+ ],
+ (Ecu.esp, 0x740, None): [
+ b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80',
+ b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80',
+ ],
+ (Ecu.eps, 0x742, None): [
+ b'6FP2A\x99A\x05\x02N123F\x18\x02\x00\x00\x00\x00\x00\x00\x00\x80',
+ ],
+ (Ecu.combinationMeter, 0x743, None): [
+ b'6FR2A\x18B\x05\x17\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'6FU9B\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
+ b'6FR9A\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80',
+ ],
+ (Ecu.gateway, 0x18dad0f1, None): [
+ b'284U26FR0E',
+ ],
+ },
+}
+
DBC = {
CAR.XTRAIL: dbc_dict('nissan_x_trail_2017', None),
CAR.LEAF: dbc_dict('nissan_leaf_2018', None),
diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py
index df53b9dd9..7e6a2f2e9 100644
--- a/selfdrive/car/tesla/carcontroller.py
+++ b/selfdrive/car/tesla/carcontroller.py
@@ -1,14 +1,16 @@
from common.numpy_fast import clip, interp
-from selfdrive.car.tesla.teslacan import TeslaCAN
from opendbc.can.packer import CANPacker
-from selfdrive.car.tesla.values import CANBUS, CarControllerParams
+from selfdrive.car.tesla.teslacan import TeslaCAN
+from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
class CarController():
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.last_angle = 0
+ self.long_control_counter = 0
self.packer = CANPacker(dbc_name)
- self.tesla_can = TeslaCAN(dbc_name, self.packer)
+ self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
+ self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
def update(self, enabled, CS, frame, actuators, cruise_cancel):
can_sends = []
@@ -34,6 +36,16 @@ class CarController():
self.last_angle = apply_angle
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, frame))
+ # Longitudinal control (40Hz)
+ if self.CP.openpilotLongitudinalControl and ((frame % 5) in [0, 2]):
+ target_accel = actuators.accel
+ target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
+ max_accel = 0 if target_accel < 0 else target_accel
+ min_accel = 0 if target_accel > 0 else target_accel
+
+ can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, self.long_control_counter))
+ self.long_control_counter += 1
+
# Cancel on user steering override, since there is no steering torque blending
if hands_on_fault:
cruise_cancel = True
@@ -46,7 +58,7 @@ class CarController():
# Spam every possible counter value, otherwise it might not be accepted
for counter in range(16):
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.chassis, counter))
- can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.autopilot, counter))
+ can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.autopilot_chassis, counter))
# TODO: HUD control
diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py
index 204152611..0a45b6f2b 100644
--- a/selfdrive/car/tesla/carstate.py
+++ b/selfdrive/car/tesla/carstate.py
@@ -16,6 +16,7 @@ class CarState(CarStateBase):
self.msg_stw_actn_req = None
self.hands_on_level = 0
self.steer_warning = None
+ self.acc_state = 0
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
@@ -43,7 +44,7 @@ class CarState(CarStateBase):
ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"]
ret.steeringPressed = (self.hands_on_level > 0)
ret.steerError = steer_status == "EAC_FAULT"
- ret.steerWarning = self.steer_warning in ["EAC_ERROR_MAX_SPEED", "EAC_ERROR_MIN_SPEED", "EAC_ERROR_TMP_FAULT", "SNA"] # TODO: not sure if this list is complete
+ ret.steerWarning = self.steer_warning != "EAC_ERROR_IDLE"
# Cruise state
cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)
@@ -57,7 +58,7 @@ class CarState(CarStateBase):
elif speed_units == "MPH":
ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS
ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled)
- ret.cruiseState.standstill = (cruise_state == "STANDSTILL")
+ ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special
# Gear
ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
@@ -88,6 +89,7 @@ class CarState(CarStateBase):
# Messages needed by carcontroller
self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"])
+ self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"]
return ret
@@ -174,8 +176,10 @@ class CarState(CarStateBase):
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
+ ("DAS_accState", "DAS_control", 0),
]
checks = [
# sig_address, frequency
+ ("DAS_control", 40),
]
- return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot)
+ return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot_chassis)
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
index e26421545..45dc0a723 100755
--- a/selfdrive/car/tesla/interface.py
+++ b/selfdrive/car/tesla/interface.py
@@ -1,6 +1,7 @@
#!/usr/bin/env python3
from cereal import car
-from selfdrive.car.tesla.values import CAR
+from panda import Panda
+from selfdrive.car.tesla.values import CANBUS, CAR
from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
@@ -10,7 +11,6 @@ class CarInterface(CarInterfaceBase):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "tesla"
- ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla)]
# There is no safe way to do steer blending with user torque,
# so the steering behaves like autopilot. This is not
@@ -18,7 +18,28 @@ class CarInterface(CarInterfaceBase):
ret.dashcamOnly = True
ret.steerControlType = car.CarParams.SteerControlType.angle
- ret.openpilotLongitudinalControl = False
+
+ # Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command
+ ret.longitudinalTuning.kpBP = [0]
+ ret.longitudinalTuning.kpV = [0]
+ ret.longitudinalTuning.kiBP = [0]
+ ret.longitudinalTuning.kiV = [0]
+ ret.stopAccel = 0.0
+ ret.startAccel = 0.0
+ ret.longitudinalActuatorDelayUpperBound = 0.5 # s
+ ret.radarTimeStep = (1.0 / 8) # 8Hz
+
+ # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus
+ # If so, we assume that it is connected to the longitudinal harness.
+ if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()):
+ ret.openpilotLongitudinalControl = True
+ ret.safetyConfigs = [
+ get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL),
+ get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN),
+ ]
+ else:
+ ret.openpilotLongitudinalControl = False
+ ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)]
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 0.5
diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py
index 0dee8b182..130180286 100644
--- a/selfdrive/car/tesla/teslacan.py
+++ b/selfdrive/car/tesla/teslacan.py
@@ -1,13 +1,13 @@
import copy
import crcmod
-from opendbc.can.can_define import CANDefine
-from selfdrive.car.tesla.values import CANBUS
+from selfdrive.config import Conversions as CV
+from selfdrive.car.tesla.values import CANBUS, CarControllerParams
class TeslaCAN:
- def __init__(self, dbc_name, packer):
- self.can_define = CANDefine(dbc_name)
+ def __init__(self, packer, pt_packer):
self.packer = packer
+ self.pt_packer = pt_packer
self.crc = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
@staticmethod
@@ -39,3 +39,23 @@ class TeslaCAN:
data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2]
values["CRC_STW_ACTN_RQ"] = self.crc(data[:7])
return self.packer.make_can_msg("STW_ACTN_RQ", bus, values)
+
+ def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt):
+ messages = []
+ values = {
+ "DAS_setSpeed": speed * CV.MS_TO_KPH,
+ "DAS_accState": acc_state,
+ "DAS_aebEvent": 0,
+ "DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN,
+ "DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX,
+ "DAS_accelMin": min_accel,
+ "DAS_accelMax": max_accel,
+ "DAS_controlCounter": (cnt % 8),
+ "DAS_controlChecksum": 0,
+ }
+
+ for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]:
+ data = packer.make_can_msg("DAS_control", bus, values)[2]
+ values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
+ messages.append(packer.make_can_msg("DAS_control", bus, values))
+ return messages
diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py
index 2a7cf9e50..90bc45c7a 100644
--- a/selfdrive/car/tesla/values.py
+++ b/selfdrive/car/tesla/values.py
@@ -25,14 +25,20 @@ FINGERPRINTS = {
}
DBC = {
- CAR.AP2_MODELS: dbc_dict(None, 'tesla_radar', chassis_dbc='tesla_can'),
- CAR.AP1_MODELS: dbc_dict(None, 'tesla_radar', chassis_dbc='tesla_can'),
+ CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
+ CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
}
class CANBUS:
+ # Lateral harness
chassis = 0
- autopilot = 2
radar = 1
+ autopilot_chassis = 2
+
+ # Longitudinal harness
+ powertrain = 4
+ private = 5
+ autopilot_powertrain = 6
GEAR_MAP = {
"DI_GEAR_INVALID": car.CarState.GearShifter.unknown,
@@ -58,4 +64,6 @@ BUTTONS = [
class CarControllerParams:
RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
-
+ JERK_LIMIT_MAX = 8
+ JERK_LIMIT_MIN = -8
+ ACCEL_TO_SPEED_MULTIPLIER = 3
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 2d17ee20f..daf91a49d 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -37,7 +37,7 @@ class CarController():
self.packer = CANPacker(dbc_name)
- def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
+ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
@@ -53,7 +53,7 @@ class CarController():
elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
self.use_interceptor = False
- if self.use_interceptor and enabled:
+ 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])
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 83a152ac1..cacd5b7be 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -33,6 +33,7 @@ class CarState(CarStateBase):
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]["SEATBELT_DRIVER_UNLATCHED"] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
+ ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
ret.gasPressed = ret.gas > 15
@@ -138,6 +139,7 @@ class CarState(CarStateBase):
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
("TC_DISABLED", "ESP_CONTROL", 1),
+ ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL", 1),
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 7453bda0d..a1221ae8a 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -1,7 +1,8 @@
#!/usr/bin/env python3
from cereal import car
from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
+from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune
+from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, CarControllerParams
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
@@ -23,267 +24,208 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4
- ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
+ ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
- if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
- ret.lateralTuning.init('pid')
- ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+ # Most cars use this default safety param
+ ret.safetyConfigs[0].safetyParam = 73
if candidate == CAR.PRIUS:
- stop_and_go = True
ret.safetyConfigs[0].safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
+ stop_and_go = True
ret.wheelbase = 2.70
ret.steerRatio = 15.74 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.init('indi')
- ret.lateralTuning.indi.innerLoopGainBP = [0.]
- ret.lateralTuning.indi.innerLoopGainV = [4.0]
- ret.lateralTuning.indi.outerLoopGainBP = [0.]
- ret.lateralTuning.indi.outerLoopGainV = [3.0]
- ret.lateralTuning.indi.timeConstantBP = [0.]
- ret.lateralTuning.indi.timeConstantV = [1.0]
- ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
- ret.lateralTuning.indi.actuatorEffectivenessV = [1.0]
+ set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
ret.steerActuatorDelay = 0.3
elif candidate in [CAR.RAV4, CAR.RAV4H]:
stop_and_go = True if (candidate in CAR.RAV4H) else False
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.65
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
tire_stiffness_factor = 0.5533
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
- ret.lateralTuning.init('lqr')
-
- ret.lateralTuning.lqr.scale = 1500.0
- ret.lateralTuning.lqr.ki = 0.05
-
- ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
- ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
- ret.lateralTuning.lqr.c = [1., 0.]
- ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
- ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
- ret.lateralTuning.lqr.dcGain = 0.002237852961363602
+ set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
elif candidate == CAR.COROLLA:
- stop_and_go = False
ret.safetyConfigs[0].safetyParam = 88
+ stop_and_go = False
ret.wheelbase = 2.70
ret.steerRatio = 18.27
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
- ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_A)
elif candidate == CAR.LEXUS_RX:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.79
ret.steerRatio = 14.8
tire_stiffness_factor = 0.5533
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
- ret.lateralTuning.pid.kf = 0.00006
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_B)
+
elif candidate == CAR.LEXUS_RXH:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.79
ret.steerRatio = 16. # 14.8 is spec end-to-end
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
elif candidate == CAR.LEXUS_RX_TSS2:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.79
ret.steerRatio = 14.8
tire_stiffness_factor = 0.5533 # not optimized yet
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate == CAR.LEXUS_RXH_TSS2:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.79
ret.steerRatio = 16.0 # 14.8 is spec end-to-end
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4481.0 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.15]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_E)
elif candidate in [CAR.CHR, CAR.CHRH]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.63906
ret.steerRatio = 13.6
tire_stiffness_factor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]]
- ret.lateralTuning.pid.kf = 0.00006
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_F)
elif candidate in [CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.82448
ret.steerRatio = 13.7
tire_stiffness_factor = 0.7933
ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00006
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.84988 # 112.2 in = 2.84988 m
ret.steerRatio = 16.0
tire_stiffness_factor = 0.8
ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning
- ret.lateralTuning.pid.kf = 0.00012 # community tuning
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.78
ret.steerRatio = 16.0
tire_stiffness_factor = 0.8
ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning
- ret.lateralTuning.pid.kf = 0.00012 # community tuning
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
elif candidate in [CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019]:
stop_and_go = False
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.82
ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
tire_stiffness_factor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]]
- ret.lateralTuning.pid.kf = 0.00006
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_H)
elif candidate in [CAR.RAV4_TSS2, CAR.RAV4H_TSS2]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
tire_stiffness_factor = 0.7933
- ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
# 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
# See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
for fw in car_fw:
if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']):
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]]
- ret.lateralTuning.pid.kf = 0.00004
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_I)
break
elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback
ret.steerRatio = 13.9
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.8702
ret.steerRatio = 16.0 # not optimized
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate == CAR.LEXUS_ESH:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.8190
ret.steerRatio = 16.06
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3682. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
elif candidate == CAR.SIENNA:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 3.03
ret.steerRatio = 15.5
tire_stiffness_factor = 0.444
ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
elif candidate == CAR.LEXUS_IS:
- stop_and_go = False
ret.safetyConfigs[0].safetyParam = 77
+ stop_and_go = False
ret.wheelbase = 2.79908
ret.steerRatio = 13.3
tire_stiffness_factor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
- ret.lateralTuning.pid.kf = 0.00006
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
elif candidate == CAR.LEXUS_CTH:
- stop_and_go = True
ret.safetyConfigs[0].safetyParam = 100
+ stop_and_go = True
ret.wheelbase = 2.60
ret.steerRatio = 18.6
tire_stiffness_factor = 0.517
ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
- ret.lateralTuning.pid.kf = 0.00007
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_M)
elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2]:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.66
ret.steerRatio = 14.7
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00006
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
elif candidate == CAR.PRIUS_TSS2:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.70002 # from toyota online sepc.
ret.steerRatio = 13.4 # True steerRation from older prius
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.15]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_N)
elif candidate == CAR.MIRAI:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 2.91
ret.steerRatio = 14.8
tire_stiffness_factor = 0.8
ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00006
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
elif candidate == CAR.ALPHARD_TSS2:
stop_and_go = True
- ret.safetyConfigs[0].safetyParam = 73
ret.wheelbase = 3.00
ret.steerRatio = 14.2
tire_stiffness_factor = 0.444
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]]
ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kf = 0.00007818594
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44
@@ -302,7 +244,7 @@ class CarInterface(CarInterfaceBase):
smartDsu = 0x2FF in fingerprint[0]
# In TSS2 cars the camera does long control
found_ecus = [fw.ecu for fw in car_fw]
- ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR)
+ ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) and (not smartDsu)
ret.enableGasInterceptor = 0x201 in fingerprint[0]
# if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in TSS2_CAR
@@ -316,28 +258,13 @@ class CarInterface(CarInterfaceBase):
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
if ret.enableGasInterceptor:
- ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
- ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
- ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
- ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36]
+ 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]:
- # Improved longitudinal tune
- ret.longitudinalTuning.deadzoneBP = [0., 8.05]
- ret.longitudinalTuning.deadzoneV = [.0, .14]
- ret.longitudinalTuning.kpBP = [0., 5., 20.]
- ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
- ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.]
- ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
+ set_long_tune(ret.longitudinalTuning, LongTunes.TSS2)
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
ret.startingAccelRate = 6.0 # release brakes fast
else:
- # Default longitudinal tune
- ret.longitudinalTuning.deadzoneBP = [0., 9.]
- ret.longitudinalTuning.deadzoneV = [0., .15]
- ret.longitudinalTuning.kpBP = [0., 5., 35.]
- ret.longitudinalTuning.kiBP = [0., 35.]
- ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
- ret.longitudinalTuning.kiV = [0.54, 0.36]
+ set_long_tune(ret.longitudinalTuning, LongTunes.TSS)
return ret
@@ -375,7 +302,7 @@ class CarInterface(CarInterfaceBase):
# to be called @ 100hz
def apply(self, c):
- can_sends = self.CC.update(c.enabled, self.CS, self.frame,
+ can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel,
c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 43b06d9dc..5a78e3c7b 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -35,10 +35,10 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
"ACC_TYPE": acc_type,
"DISTANCE": 0,
"MINI_CAR": lead,
- "SET_ME_X3": 3,
"PERMIT_BRAKING": 1,
"RELEASE_STANDSTILL": not standstill_req,
"CANCEL_REQ": pcm_cancel,
+ "ALLOW_LONG_PRESS": 1,
}
return packer.make_can_msg("ACC_CONTROL", 0, values)
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
new file mode 100644
index 000000000..3f210d48a
--- /dev/null
+++ b/selfdrive/car/toyota/tunes.py
@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+from enum import Enum
+from selfdrive.car.toyota.values import MIN_ACC_SPEED, PEDAL_HYST_GAP
+
+
+class LongTunes(Enum):
+ PEDAL = 0
+ TSS2 = 1
+ TSS = 2
+
+class LatTunes(Enum):
+ INDI_PRIUS = 0
+ LQR_RAV4 = 1
+ PID_A = 2
+ PID_B = 3
+ PID_C = 4
+ PID_D = 5
+ PID_E = 6
+ PID_F = 7
+ PID_G = 8
+ PID_I = 9
+ PID_H = 10
+ PID_J = 11
+ PID_K = 12
+ PID_L = 13
+ PID_M = 14
+ PID_N = 15
+
+
+###### 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:
+ tune.deadzoneBP = [0., 8.05]
+ tune.deadzoneV = [.0, .14]
+ tune.kpBP = [0., 5., 20.]
+ tune.kpV = [1.3, 1.0, 0.7]
+ tune.kiBP = [0., 5., 12., 20., 27.]
+ tune.kiV = [.35, .23, .20, .17, .1]
+ # Default longitudinal tune
+ elif name == LongTunes.TSS:
+ tune.deadzoneBP = [0., 9.]
+ tune.deadzoneV = [0., .15]
+ tune.kpBP = [0., 5., 35.]
+ tune.kiBP = [0., 35.]
+ tune.kpV = [3.6, 2.4, 1.5]
+ tune.kiV = [0.54, 0.36]
+ else:
+ raise NotImplementedError('This longitudinal tune does not exist')
+
+
+###### LAT ######
+def set_lat_tune(tune, name):
+ if name == LatTunes.INDI_PRIUS:
+ tune.init('indi')
+ tune.indi.innerLoopGainBP = [0.]
+ tune.indi.innerLoopGainV = [4.0]
+ tune.indi.outerLoopGainBP = [0.]
+ tune.indi.outerLoopGainV = [3.0]
+ tune.indi.timeConstantBP = [0.]
+ tune.indi.timeConstantV = [1.0]
+ tune.indi.actuatorEffectivenessBP = [0.]
+ tune.indi.actuatorEffectivenessV = [1.0]
+
+ elif name == LatTunes.LQR_RAV4:
+ tune.init('lqr')
+ tune.lqr.scale = 1500.0
+ tune.lqr.ki = 0.05
+ tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
+ tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+ tune.lqr.c = [1., 0.]
+ tune.lqr.k = [-110.73572306, 451.22718255]
+ tune.lqr.l = [0.3233671, 0.3185757]
+ tune.lqr.dcGain = 0.002237852961363602
+
+ elif 'PID' in str(name):
+ tune.init('pid')
+ tune.pid.kiBP = [0.0]
+ tune.pid.kpBP = [0.0]
+ if name == LatTunes.PID_A:
+ tune.pid.kpV = [0.2]
+ tune.pid.kiV = [0.05]
+ tune.pid.kf = 0.00003
+ elif name == LatTunes.PID_B:
+ tune.pid.kpV = [0.6]
+ tune.pid.kiV = [0.05]
+ tune.pid.kf = 0.00006
+ elif name == LatTunes.PID_C:
+ tune.pid.kpV = [0.6]
+ tune.pid.kiV = [0.1]
+ tune.pid.kf = 0.00006
+ elif name == LatTunes.PID_D:
+ tune.pid.kpV = [0.6]
+ tune.pid.kiV = [0.1]
+ tune.pid.kf = 0.00007818594
+ elif name == LatTunes.PID_E:
+ tune.pid.kpV = [0.6]
+ tune.pid.kiV = [0.15]
+ tune.pid.kf = 0.00007818594
+ elif name == LatTunes.PID_F:
+ tune.pid.kpV = [0.723]
+ tune.pid.kiV = [0.0428]
+ tune.pid.kf = 0.00006
+ elif name == LatTunes.PID_G:
+ tune.pid.kpV = [0.18]
+ tune.pid.kiV = [0.015]
+ tune.pid.kf = 0.00012
+ elif name == LatTunes.PID_H:
+ tune.pid.kpV = [0.17]
+ tune.pid.kiV = [0.03]
+ tune.pid.kf = 0.00006
+ elif name == LatTunes.PID_I:
+ tune.pid.kpV = [0.15]
+ tune.pid.kiV = [0.05]
+ tune.pid.kf = 0.00004
+ elif name == LatTunes.PID_J:
+ tune.pid.kpV = [0.19]
+ tune.pid.kiV = [0.02]
+ tune.pid.kf = 0.00007818594
+ elif name == LatTunes.PID_L:
+ tune.pid.kpV = [0.3]
+ tune.pid.kiV = [0.05]
+ tune.pid.kf = 0.00006
+ elif name == LatTunes.PID_M:
+ tune.pid.kpV = [0.3]
+ tune.pid.kiV = [0.05]
+ tune.pid.kf = 0.00007
+ elif name == LatTunes.PID_N:
+ tune.pid.kpV = [0.35]
+ tune.pid.kiV = [0.15]
+ tune.pid.kf = 0.00007818594
+ else:
+ raise NotImplementedError('This PID tune does not exist')
+ else:
+ raise NotImplementedError('This lateral tune does not exist')
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index b9312eef4..73198d925 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -86,33 +86,6 @@ STATIC_DSU_MSGS = [
]
-FINGERPRINTS = {
- CAR.PRIUS: [{
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- #2019 LE
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # 2020 Prius Prime LE
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- #2020 Prius Prime Limited
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8
- },
- #2020 Central Europe Prime
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 8, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8
- },
- #2017 German Prius
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1792: 8, 1767: 4, 1863: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996: 8, 1998: 8, 2002: 8, 2010: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2030: 8
- }],
-}
-
-
FW_VERSIONS = {
CAR.AVALON: {
(Ecu.esp, 0x7b0, None): [
@@ -155,8 +128,9 @@ FW_VERSIONS = {
b'8965B41090\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
- b'\x01896630735100\x00\x00\x00\x00',
+ b'\x01896630725200\x00\x00\x00\x00',
b'\x01896630725300\x00\x00\x00\x00',
+ b'\x01896630735100\x00\x00\x00\x00',
b'\x01896630738000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
@@ -451,6 +425,7 @@ FW_VERSIONS = {
b'8821F0W01100 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
+ b'8646FF401700 ',
b'8646FF401800 ',
b'8646FF404000 ',
b'8646FF406000 ',
@@ -643,6 +618,7 @@ FW_VERSIONS = {
b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
+ b'\x038966312T3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B12361\x00\x00\x00\x00\x00\x00',
@@ -689,6 +665,7 @@ FW_VERSIONS = {
b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+ b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
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',
@@ -705,6 +682,7 @@ FW_VERSIONS = {
b'\x01896630E45000\x00\x00\x00\x00',
b'\x01896630E45100\x00\x00\x00\x00',
b'\x01896630E45200\x00\x00\x00\x00',
+ b'\x01896630E46000\x00\x00\x00\x00',
b'\x01896630E46200\x00\x00\x00\x00',
b'\x01896630E74000\x00\x00\x00\x00',
b'\x01896630E75000\x00\x00\x00\x00',
@@ -782,6 +760,7 @@ FW_VERSIONS = {
b'\x01896630EB2100\x00\x00\x00\x00',
b'\x01896630EB2200\x00\x00\x00\x00',
b'\x01896630EC4000\x00\x00\x00\x00',
+ b'\x01896630EE1000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
@@ -804,7 +783,8 @@ FW_VERSIONS = {
b'\x01F152648C6300\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
- b'\x01896630EA1000\000\000\000\000',
+ b'\x01896630EA1000\x00\x00\x00\x00',
+ b'\x01896630EE4000\x00\x00\x00\x00',
b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@@ -936,6 +916,7 @@ FW_VERSIONS = {
],
(Ecu.dsu, 0x791, None): [
b'881514702300\x00\x00\x00\x00',
+ b'881514702400\x00\x00\x00\x00',
b'881514703100\x00\x00\x00\x00',
b'881514704100\x00\x00\x00\x00',
b'881514706000\x00\x00\x00\x00',
@@ -989,6 +970,7 @@ FW_VERSIONS = {
b'8821F4702300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
+ b'8646F4201100\x00\x00\x00\x00',
b'8646F4201200\x00\x00\x00\x00',
b'8646F4202001\x00\x00\x00\x00',
b'8646F4202100\x00\x00\x00\x00',
@@ -1045,6 +1027,7 @@ FW_VERSIONS = {
b'\x018966342U4000\x00\x00\x00\x00',
b'\x018966342U4100\x00\x00\x00\x00',
b'\x018966342U5100\x00\x00\x00\x00',
+ b'\x018966342V0000\x00\x00\x00\x00',
b'\x018966342V3000\x00\x00\x00\x00',
b'\x018966342V3100\x00\x00\x00\x00',
b'\x018966342V3200\x00\x00\x00\x00',
@@ -1163,35 +1146,6 @@ FW_VERSIONS = {
b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
],
},
- CAR.LEXUS_ES_TSS2: {
- (Ecu.engine, 0x700, None): [
- b'\x01896630EC9100\x00\x00\x00\x00',
- b'\x018966333T5000\x00\x00\x00\x00',
- b'\x018966333T5100\x00\x00\x00\x00',
- b'\x018966333X6000\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'\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'8965B48271\x00\x00\x00\x00\x00\x00',
- ],
- (Ecu.fwdRadar, 0x750, 0xf): [
- b'\x018821F3301100\x00\x00\x00\x00',
- b'\x018821F3301200\x00\x00\x00\x00',
- b'\x018821F3301400\x00\x00\x00\x00',
- ],
- (Ecu.fwdCamera, 0x750, 0x6d): [
- 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'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
- ],
- },
CAR.SIENNA: {
(Ecu.engine, 0x700, None): [
b'\x01896630832100\x00\x00\x00\x00',
@@ -1202,6 +1156,7 @@ FW_VERSIONS = {
b'\x01896630843000\x00\x00\x00\x00',
b'\x01896630851000\x00\x00\x00\x00',
b'\x01896630851100\x00\x00\x00\x00',
+ b'\x01896630851200\x00\x00\x00\x00',
b'\x01896630852100\x00\x00\x00\x00',
b'\x01896630859000\x00\x00\x00\x00',
b'\x01896630860000\x00\x00\x00\x00',
@@ -1243,6 +1198,35 @@ FW_VERSIONS = {
b'8646F7601100\x00\x00\x00\x00',
],
},
+ CAR.LEXUS_ES_TSS2: {
+ (Ecu.engine, 0x700, None): [
+ b'\x01896630EC9100\x00\x00\x00\x00',
+ b'\x018966333T5000\x00\x00\x00\x00',
+ b'\x018966333T5100\x00\x00\x00\x00',
+ b'\x018966333X6000\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'\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'8965B48271\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x750, 0xf): [
+ b'\x018821F3301100\x00\x00\x00\x00',
+ b'\x018821F3301200\x00\x00\x00\x00',
+ b'\x018821F3301400\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x750, 0x6d): [
+ 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'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+ ],
+ },
CAR.LEXUS_ESH_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
@@ -1273,6 +1257,7 @@ FW_VERSIONS = {
b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+ b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
],
},
CAR.LEXUS_ESH: {
@@ -1299,6 +1284,7 @@ FW_VERSIONS = {
},
CAR.LEXUS_NX: {
(Ecu.engine, 0x700, None): [
+ b'\x01896637850000\x00\x00\x00\x00',
b'\x01896637851000\x00\x00\x00\x00',
b'\x01896637852000\x00\x00\x00\x00',
b'\x01896637854000\x00\x00\x00\x00',
@@ -1343,10 +1329,11 @@ FW_VERSIONS = {
},
CAR.LEXUS_NXH: {
(Ecu.engine, 0x7e0, None): [
- b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
- b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\x0237842000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0237880000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152678160\x00\x00\x00\x00\x00\x00',
@@ -1380,9 +1367,11 @@ FW_VERSIONS = {
b'\x01896630E41000\x00\x00\x00\x00',
b'\x01896630E41100\x00\x00\x00\x00',
b'\x01896630E41200\x00\x00\x00\x00',
+ b'\x01896630E41500\x00\x00\x00\x00',
b'\x01896630EA3100\x00\x00\x00\x00',
b'\x01896630EA4100\x00\x00\x00\x00',
b'\x01896630EA4300\x00\x00\x00\x00',
+ b'\x01896630EA4400\x00\x00\x00\x00',
b'\x01896630EA6300\x00\x00\x00\x00',
b'\x018966348R1300\x00\x00\x00\x00',
b'\x018966348R8500\x00\x00\x00\x00',
@@ -1406,6 +1395,7 @@ FW_VERSIONS = {
b'8965B0E011\x00\x00\x00\x00\x00\x00',
b'8965B0E012\x00\x00\x00\x00\x00\x00',
b'8965B48102\x00\x00\x00\x00\x00\x00',
+ b'8965B48111\x00\x00\x00\x00\x00\x00',
b'8965B48112\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
@@ -1474,6 +1464,7 @@ FW_VERSIONS = {
b'\x01896630EB0000\x00\x00\x00\x00',
b'\x01896630EA9000\x00\x00\x00\x00',
b'\x01896630ED0000\x00\x00\x00\x00',
+ b'\x018966348W5100\x00\x00\x00\x00',
b'\x018966348W9000\x00\x00\x00\x00',
b'\x01896634D12100\x00\x00\x00\x00',
],
@@ -1484,6 +1475,7 @@ FW_VERSIONS = {
b'\x01F152648781\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
+ b'8965B48261\x00\x00\x00\x00\x00\x00',
b'8965B48271\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index 51d5af8bc..b8c9c5d67 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -78,6 +78,10 @@ class CarInterface(CarInterfaceBase):
ret.mass = 1551 + STD_CARGO_KG
ret.wheelbase = 2.79
+ elif candidate == CAR.POLO_MK6:
+ ret.mass = 1230 + STD_CARGO_KG
+ ret.wheelbase = 2.55
+
elif candidate == CAR.TAOS_MK1:
ret.mass = 1498 + STD_CARGO_KG
ret.wheelbase = 2.69
@@ -99,6 +103,10 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 3.00 # SWB, LWB is 3.40, TBD how to detect difference
ret.minSteerSpeed = 14.0
+ elif candidate == CAR.TROC_MK1:
+ ret.mass = 1413 + STD_CARGO_KG
+ ret.wheelbase = 2.63
+
elif candidate == CAR.AUDI_A3_MK3:
ret.mass = 1335 + STD_CARGO_KG
ret.wheelbase = 2.61
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 162462f1c..7b3160e62 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -68,13 +68,15 @@ class CAR:
ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN" # Chassis AN, Mk1 VW Arteon and variants
ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport
GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants
- JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 Jetta
- PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 Passat and variants
+ JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta
+ PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants
+ POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN" # Chassis AW, Mk6 VW Polo
TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN" # Chassis B2, Mk1 VW Taos and Tharu
TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN" # Chassis C1, Mk1 VW T-Cross SWB and LWB variants
TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants
TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants
TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1" # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California
+ 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)
SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca
@@ -118,6 +120,7 @@ FW_VERSIONS = {
b'\xf1\x8703H906026AA\xf1\x899970',
b'\xf1\x8703H906026F \xf1\x896696',
b'\xf1\x8703H906026F \xf1\x899970',
+ b'\xf1\x8703H906026J \xf1\x896026',
b'\xf1\x8703H906026S \xf1\x896693',
b'\xf1\x8703H906026S \xf1\x899970',
],
@@ -158,6 +161,7 @@ FW_VERSIONS = {
b'\xf1\x8704L906056CR\xf1\x895813',
b'\xf1\x8704L906056HE\xf1\x893758',
b'\xf1\x870EA906016A \xf1\x898343',
+ b'\xf1\x870EA906016E \xf1\x894219',
b'\xf1\x870EA906016F \xf1\x895002',
b'\xf1\x870EA906016S \xf1\x897207',
b'\xf1\x875G0906259 \xf1\x890007',
@@ -225,6 +229,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02324230011211200061104171724102491132111',
b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02324230011211200621143171724112491132111',
b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011211200061104171717101791132111',
+ b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1324230011211200631143171724122491132111',
b'\xf1\x875Q0959655T \xf1\x890825\xf1\x82\023271200111312--071104171837103791132111',
b'\xf1\x875Q0959655T \xf1\x890830\xf1\x82\x13271100111312--071104171826102691131211',
b'\xf1\x875QD959655 \xf1\x890388\xf1\x82\x111413001113120006110417121D101D9112',
@@ -246,11 +251,13 @@ FW_VERSIONS = {
b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A07B05A1',
b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00521A00602A0',
b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00522A00402A0',
+ b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\x0521A00502A0',
b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511A00403A0',
b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516A00604A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516A00604A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516A07A02A1',
b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521A00507A1',
+ b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521A07B04A1',
b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521A20B03A1',
b'\xf1\x875QD909144B \xf1\x891072\xf1\x82\x0521A00507A1',
b'\xf1\x875QM909144A \xf1\x891072\xf1\x82\x0521A20B03A1',
@@ -298,8 +305,10 @@ FW_VERSIONS = {
b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02314171231313500314643011650169333463100',
b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02311170031313300314240011150119333433100',
b'\xf1\x875Q0959655BR\xf1\x890403\xf1\x82\02319170031313300314240011550159333463100',
+ b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1314171231313500314643021650169333613100',
],
(Ecu.eps, 0x712, None): [
+ b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A10A11A1',
b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\00521A10A01A1',
b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521B00404A1',
b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A00642A1',
@@ -342,6 +351,23 @@ FW_VERSIONS = {
b'\xf1\x875Q0907572R \xf1\x890771',
],
},
+ CAR.POLO_MK6: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704C906025H \xf1\x895177',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870CW300050D \xf1\x891908',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1248130411110416--04040404784811152H14',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x872Q1909144M \xf1\x896041',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572R \xf1\x890372',
+ ],
+ },
CAR.TAOS_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8705E906013E \xf1\x891624',
@@ -442,6 +468,23 @@ FW_VERSIONS = {
b'\xf1\x872Q0907572R \xf1\x890372',
],
},
+ CAR.TROC_MK1: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8705E906018AT\xf1\x899640',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870CW300051M \xf1\x891925',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x13111100123333003142404M1152119333613100',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521060405A1',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572T \xf1\x890383',
+ ],
+ },
CAR.AUDI_A3_MK3: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704E906023AN\xf1\x893695',
diff --git a/selfdrive/common/clutil.cc b/selfdrive/common/clutil.cc
index 952fc6863..313978525 100644
--- a/selfdrive/common/clutil.cc
+++ b/selfdrive/common/clutil.cc
@@ -1,12 +1,8 @@
#include "selfdrive/common/clutil.h"
-#include
-
#include
-#include
#include
#include
-#include
#include "selfdrive/common/util.h"
@@ -78,8 +74,10 @@ cl_device_id cl_get_device_id(cl_device_type device_type) {
}
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) {
- std::string src = util::read_file(path);
- assert(src.length() > 0);
+ return cl_program_from_source(ctx, device_id, util::read_file(path), args);
+}
+
+cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args) {
cl_program prg = CL_CHECK_ERR(clCreateProgramWithSource(ctx, 1, (const char*[]){src.c_str()}, NULL, &err));
if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) {
cl_print_build_errors(prg, device_id);
@@ -88,6 +86,15 @@ cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const ch
return prg;
}
+cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args) {
+ cl_program prg = CL_CHECK_ERR(clCreateProgramWithBinary(ctx, 1, &device_id, &length, (const uint8_t*[]){binary}, NULL, &err));
+ if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) {
+ cl_print_build_errors(prg, device_id);
+ assert(0);
+ }
+ return prg;
+}
+
// Given a cl code and return a string representation
#define CL_ERR_TO_STR(err) case err: return #err
const char* cl_get_error_string(int err) {
diff --git a/selfdrive/common/clutil.h b/selfdrive/common/clutil.h
index ffddafadb..be1a07c33 100644
--- a/selfdrive/common/clutil.h
+++ b/selfdrive/common/clutil.h
@@ -1,14 +1,13 @@
#pragma once
-#include
-#include
-
#ifdef __APPLE__
#include
#else
#include
#endif
+#include
+
#define CL_CHECK(_expr) \
do { \
assert(CL_SUCCESS == (_expr)); \
@@ -23,5 +22,7 @@
})
cl_device_id cl_get_device_id(cl_device_type device_type);
+cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
+cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);
const char* cl_get_error_string(int err);
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 22926c593..01bf9a8f4 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -1,19 +1,9 @@
#include "selfdrive/common/params.h"
-#ifndef _GNU_SOURCE
-#define _GNU_SOURCE
-#endif // _GNU_SOURCE
-
#include
#include
-#include
-#include
#include
-#include
-#include
-#include
-#include
#include
#include "selfdrive/common/swaglog.h"
@@ -27,50 +17,27 @@ void params_sig_handler(int signal) {
params_do_exit = 1;
}
-int fsync_dir(const char* path) {
- int fd = HANDLE_EINTR(open(path, O_RDONLY, 0755));
- if (fd < 0) {
- return -1;
- }
-
- int result = fsync(fd);
- int result_close = close(fd);
- if (result_close < 0) {
- result = result_close;
+int fsync_dir(const std::string &path) {
+ int result = -1;
+ int fd = HANDLE_EINTR(open(path.c_str(), O_RDONLY, 0755));
+ if (fd >= 0) {
+ result = fsync(fd);
+ close(fd);
}
return result;
}
-int mkdir_p(std::string path) {
- char * _path = (char *)path.c_str();
-
- for (char *p = _path + 1; *p; p++) {
- if (*p == '/') {
- *p = '\0'; // Temporarily truncate
- if (mkdir(_path, 0775) != 0) {
- if (errno != EEXIST) return -1;
- }
- *p = '/';
- }
- }
- if (mkdir(_path, 0775) != 0) {
- if (errno != EEXIST) return -1;
- }
- return 0;
-}
-
bool create_params_path(const std::string ¶m_path, const std::string &key_path) {
// Make sure params path exists
- if (!util::file_exists(param_path) && mkdir_p(param_path) != 0) {
+ if (!util::file_exists(param_path) && !util::create_directories(param_path, 0775)) {
return false;
}
// See if the symlink exists, otherwise create it
if (!util::file_exists(key_path)) {
// 1) Create temp folder
- // 2) Set permissions
- // 3) Symlink it to temp link
- // 4) Move symlink to /d
+ // 2) Symlink it to temp link
+ // 3) Move symlink to /d
std::string tmp_path = param_path + "/.tmp_XXXXXX";
// this should be OK since mkdtemp just replaces characters in place
@@ -93,32 +60,26 @@ bool create_params_path(const std::string ¶m_path, const std::string &key_pa
return true;
}
-void ensure_params_path(const std::string ¶ms_path) {
+std::string ensure_params_path(const std::string &path = {}) {
+ std::string params_path = path.empty() ? Path::params() : path;
if (!create_params_path(params_path, params_path + "/d")) {
throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno));
}
+ return params_path;
}
class FileLock {
- public:
- FileLock(const std::string& file_name, int op) : fn_(file_name), op_(op) {}
-
- void lock() {
- fd_ = HANDLE_EINTR(open(fn_.c_str(), O_CREAT, 0775));
- if (fd_ < 0) {
- LOGE("Failed to open lock file %s, errno=%d", fn_.c_str(), errno);
- return;
- }
- if (HANDLE_EINTR(flock(fd_, op_)) < 0) {
- LOGE("Failed to lock file %s, errno=%d", fn_.c_str(), errno);
+public:
+ FileLock(const std::string &fn) {
+ fd_ = HANDLE_EINTR(open(fn.c_str(), O_CREAT, 0775));
+ if (fd_ < 0 || HANDLE_EINTR(flock(fd_, LOCK_EX)) < 0) {
+ LOGE("Failed to lock file %s, errno=%d", fn.c_str(), errno);
}
}
-
- void unlock() { close(fd_); }
+ ~FileLock() { close(fd_); }
private:
- int fd_ = -1, op_;
- std::string fn_;
+ int fd_ = -1;
};
std::unordered_map keys = {
@@ -139,6 +100,8 @@ std::unordered_map keys = {
{"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB
{"DisableUpdates", PERSISTENT},
{"DongleId", PERSISTENT},
+ {"DoReboot", CLEAR_ON_MANAGER_START},
+ {"DoShutdown", CLEAR_ON_MANAGER_START},
{"DoUninstall", CLEAR_ON_MANAGER_START},
{"EnableWideCamera", CLEAR_ON_MANAGER_START},
{"EndToEndToggle", PERSISTENT},
@@ -153,6 +116,7 @@ std::unordered_map keys = {
{"GsmRoaming", PERSISTENT},
{"HardwareSerial", PERSISTENT},
{"HasAcceptedTerms", PERSISTENT},
+ {"HasPrime", PERSISTENT},
{"IMEI", PERSISTENT},
{"InstallDate", PERSISTENT},
{"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},
@@ -169,13 +133,9 @@ std::unordered_map keys = {
{"LastUpdateException", PERSISTENT},
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
- {"MapboxToken", PERSISTENT | DONT_LOG},
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"NavSettingTime24h", PERSISTENT},
{"OpenpilotEnabledToggle", PERSISTENT},
- {"PandaDongleId", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
- {"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
- {"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"Passive", PERSISTENT},
{"PrimeRedirected", PERSISTENT},
@@ -183,6 +143,7 @@ std::unordered_map keys = {
{"RecordFrontLock", PERSISTENT}, // for the internal fleet
{"ReleaseNotes", PERSISTENT},
{"ShouldDoUpdate", CLEAR_ON_MANAGER_START},
+ {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"SshEnabled", PERSISTENT},
{"SubscriberInfo", PERSISTENT},
{"TermsVersion", PERSISTENT},
@@ -197,15 +158,15 @@ std::unordered_map keys = {
{"ApiCache_DriveStats", PERSISTENT},
{"ApiCache_NavDestinations", PERSISTENT},
{"ApiCache_Owner", PERSISTENT},
+ {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
{"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
- {"Offroad_HardwareUnsupported", CLEAR_ON_MANAGER_START},
{"Offroad_InvalidTime", CLEAR_ON_MANAGER_START},
{"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START},
{"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START},
- {"Offroad_NvmeMissing", CLEAR_ON_MANAGER_START},
- {"Offroad_PandaFirmwareMismatch", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
+ {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
+ {"Offroad_StorageMissing", CLEAR_ON_MANAGER_START},
{"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
{"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
{"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
@@ -213,13 +174,9 @@ std::unordered_map keys = {
} // namespace
-Params::Params() : params_path(Path::params()) {
- static std::once_flag once_flag;
- std::call_once(once_flag, ensure_params_path, params_path);
-}
-
-Params::Params(const std::string &path) : params_path(path) {
- ensure_params_path(params_path);
+Params::Params(const std::string &path) {
+ static std::string default_param_path = ensure_params_path();
+ params_path = path.empty() ? default_param_path : ensure_params_path(path);
}
bool Params::checkKey(const std::string &key) {
@@ -253,16 +210,13 @@ int Params::put(const char* key, const char* value, size_t value_size) {
// fsync to force persist the changes.
if ((result = fsync(tmp_fd)) < 0) break;
- FileLock file_lock(params_path + "/.lock", LOCK_EX);
- std::lock_guard lk(file_lock);
+ FileLock file_lock(params_path + "/.lock");
// Move temp into place.
- std::string path = params_path + "/d/" + std::string(key);
- if ((result = rename(tmp_path.c_str(), path.c_str())) < 0) break;
+ if ((result = rename(tmp_path.c_str(), getParamPath(key).c_str())) < 0) break;
// fsync parent directory
- path = params_path + "/d";
- result = fsync_dir(path.c_str());
+ result = fsync_dir(getParamPath());
} while (false);
close(tmp_fd);
@@ -270,24 +224,18 @@ int Params::put(const char* key, const char* value, size_t value_size) {
return result;
}
-int Params::remove(const char *key) {
- FileLock file_lock(params_path + "/.lock", LOCK_EX);
- std::lock_guard lk(file_lock);
- // Delete value.
- std::string path = params_path + "/d/" + key;
- int result = unlink(path.c_str());
+int Params::remove(const std::string &key) {
+ FileLock file_lock(params_path + "/.lock");
+ int result = unlink(getParamPath(key).c_str());
if (result != 0) {
return result;
}
- // fsync parent directory
- path = params_path + "/d";
- return fsync_dir(path.c_str());
+ return fsync_dir(getParamPath());
}
-std::string Params::get(const char *key, bool block) {
- std::string path = params_path + "/d/" + key;
+std::string Params::get(const std::string &key, bool block) {
if (!block) {
- return util::read_file(path);
+ return util::read_file(getParamPath(key));
} else {
// blocking read until successful
params_do_exit = 0;
@@ -296,7 +244,7 @@ std::string Params::get(const char *key, bool block) {
std::string value;
while (!params_do_exit) {
- if (value = util::read_file(path); !value.empty()) {
+ if (value = util::read_file(getParamPath(key)); !value.empty()) {
break;
}
util::sleep_for(100); // 0.1 s
@@ -309,26 +257,19 @@ std::string Params::get(const char *key, bool block) {
}
std::map Params::readAll() {
- FileLock file_lock(params_path + "/.lock", LOCK_SH);
- std::lock_guard lk(file_lock);
-
- std::string key_path = params_path + "/d";
- return util::read_files_in_dir(key_path);
+ FileLock file_lock(params_path + "/.lock");
+ return util::read_files_in_dir(getParamPath());
}
void Params::clearAll(ParamKeyType key_type) {
- FileLock file_lock(params_path + "/.lock", LOCK_EX);
- std::lock_guard lk(file_lock);
+ FileLock file_lock(params_path + "/.lock");
std::string path;
for (auto &[key, type] : keys) {
if (type & key_type) {
- path = params_path + "/d/" + key;
- unlink(path.c_str());
+ unlink(getParamPath(key).c_str());
}
}
- // fsync parent directory
- path = params_path + "/d";
- fsync_dir(path.c_str());
+ fsync_dir(getParamPath());
}
diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h
index 77d00b76e..c4bdde001 100644
--- a/selfdrive/common/params.h
+++ b/selfdrive/common/params.h
@@ -1,9 +1,7 @@
#pragma once
#include