commaai
/
panda
mirror of https://github.com/commaai/panda.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Subaru: improve readability in panda code (#1380) 

* improve readability

* cleanup for PR

* switch to #define

* i guess i forgot how #define works

* fix #define

* improve for preglobal too

* rename because of conflicts

* rename with subaru in name

* missed some

* missed that one too

* made it pretty

* whitespace

* infotainment match opendbc

* whitespace

* whitespace

* parameterize the bus too

* fix that

* fix misra violation

* multiline

---------

Co-authored-by: Shane Smiskol <shane@smiskol.com>
This commit is contained in:
Justin Newberry 2023-07-07 04:48:30 -04:00 committed by GitHub
parent c1e79333fb
commit 8ba5683881
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 89 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
board/safety/safety_subaru.h
View File

@ -20,40 +20,58 @@ const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = {
.type = TorqueDriverLimited,
};
#define MSG_SUBARU_Brake_Status 0x13c
#define MSG_SUBARU_CruiseControl 0x240
#define MSG_SUBARU_Throttle 0x40
#define MSG_SUBARU_Steering_Torque 0x119
#define MSG_SUBARU_Wheel_Speeds 0x13a
#define MSG_SUBARU_ES_LKAS 0x122
#define MSG_SUBARU_ES_Brake 0x220
#define MSG_SUBARU_ES_Distance 0x221
#define MSG_SUBARU_ES_Status 0x222
#define MSG_SUBARU_ES_DashStatus 0x321
#define MSG_SUBARU_ES_LKAS_State 0x322
#define MSG_SUBARU_ES_Infotainment 0x323
#define SUBARU_MAIN_BUS 0
#define SUBARU_ALT_BUS 1
#define SUBARU_CAM_BUS 2
const CanMsg SUBARU_TX_MSGS[] = {
{0x122, 0, 8},
{0x221, 0, 8},
{0x321, 0, 8},
{0x322, 0, 8},
{0x323, 0, 8},
{MSG_SUBARU_ES_LKAS, SUBARU_MAIN_BUS, 8},
{MSG_SUBARU_ES_Distance, SUBARU_MAIN_BUS, 8},
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8},
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8},
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8},
};
#define SUBARU_TX_MSGS_LEN (sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))
const CanMsg SUBARU_GEN2_TX_MSGS[] = {
{0x122, 0, 8},
{0x221, 1, 8},
{0x321, 0, 8},
{0x322, 0, 8},
{0x323, 0, 8}
{MSG_SUBARU_ES_LKAS, SUBARU_MAIN_BUS, 8},
{MSG_SUBARU_ES_Distance, SUBARU_ALT_BUS, 8},
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8},
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8},
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8}
};
#define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0]))
AddrCheckStruct subaru_addr_checks[] = {
{.msg = {{ 0x40, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{0x119, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{0x13a, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{0x13c, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{0x240, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Wheel_Speeds, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Brake_Status, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_CruiseControl, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}},
};
#define SUBARU_ADDR_CHECK_LEN (sizeof(subaru_addr_checks) / sizeof(subaru_addr_checks[0]))
addr_checks subaru_rx_checks = {subaru_addr_checks, SUBARU_ADDR_CHECK_LEN};
AddrCheckStruct subaru_gen2_addr_checks[] = {
{.msg = {{ 0x40, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{0x119, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{0x13a, 1, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{0x13c, 1, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{0x240, 1, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Wheel_Speeds, SUBARU_ALT_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_Brake_Status, SUBARU_ALT_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_CruiseControl, SUBARU_ALT_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}},
};
#define SUBARU_GEN2_ADDR_CHECK_LEN (sizeof(subaru_gen2_addr_checks) / sizeof(subaru_gen2_addr_checks[0]))
addr_checks subaru_gen2_rx_checks = {subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_CHECK_LEN};
@ -88,10 +106,10 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
if (valid) {
const int bus = GET_BUS(to_push);
const int alt_bus = subaru_gen2 ? 1 : 0;
const int alt_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
int addr = GET_ADDR(to_push);
if ((addr == 0x119) && (bus == 0)) {
if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) {
int torque_driver_new;
torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 16) & 0x7FFU);
torque_driver_new = -1 * to_signed(torque_driver_new, 11);
@ -99,25 +117,25 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x240) && (bus == alt_bus)) {
if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_bus)) {
bool cruise_engaged = GET_BIT(to_push, 41U) != 0U;
pcm_cruise_check(cruise_engaged);
}
// update vehicle moving with any non-zero wheel speed
if ((addr == 0x13a) && (bus == alt_bus)) {
if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_bus)) {
vehicle_moving = ((GET_BYTES(to_push, 0, 4) >> 12) != 0U) || (GET_BYTES(to_push, 4, 4) != 0U);
}
if ((addr == 0x13c) && (bus == alt_bus)) {
if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_bus)) {
brake_pressed = ((GET_BYTE(to_push, 7) >> 6) & 1U);
}
if ((addr == 0x40) && (bus == 0)) {
if ((addr == MSG_SUBARU_Throttle) && (bus == SUBARU_MAIN_BUS)) {
gas_pressed = GET_BYTE(to_push, 4) != 0U;
}
generic_rx_checks((addr == 0x122) && (bus == 0));
generic_rx_checks((addr == MSG_SUBARU_ES_LKAS) && (bus == SUBARU_MAIN_BUS));
}
return valid;
}
@ -134,7 +152,7 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
}
// steer cmd checks
if (addr == 0x122) {
if (addr == MSG_SUBARU_ES_LKAS) {
int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x1FFFU);
desired_torque = -1 * to_signed(desired_torque, 13);
@ -150,19 +168,18 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
static int subaru_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == 0) {
bus_fwd = 2; // forward to camera
if (bus_num == SUBARU_MAIN_BUS) {
bus_fwd = SUBARU_CAM_BUS; // forward to camera
}
if (bus_num == 2) {
if (bus_num == SUBARU_CAM_BUS) {
// Global platform
// 0x122 ES_LKAS
// 0x321 ES_DashStatus
// 0x322 ES_LKAS_State
// 0x323 ES_Infotainment
bool block_lkas = (addr == 0x122) || (addr == 0x321) || (addr == 0x322) || (addr == 0x323);
bool block_lkas = ((addr == MSG_SUBARU_ES_LKAS) ||
(addr == MSG_SUBARU_ES_DashStatus) ||
(addr == MSG_SUBARU_ES_LKAS_State) ||
(addr == MSG_SUBARU_ES_Infotainment));
if (!block_lkas) {
bus_fwd = 0; // Main CAN
bus_fwd = SUBARU_MAIN_BUS; // Main CAN
}
}
@ -188,5 +205,3 @@ const safety_hooks subaru_hooks = {
.tx_lin = nooutput_tx_lin_hook,
.fwd = subaru_fwd_hook,
};

56
board/safety/safety_subaru_preglobal.h
View File

@ -9,17 +9,32 @@ const SteeringLimits SUBARU_PG_STEERING_LIMITS = {
.type = TorqueDriverLimited,
};
// Preglobal platform
// 0x161 is ES_CruiseThrottle
// 0x164 is ES_LKAS
#define MSG_SUBARU_PG_CruiseControl 0x144
#define MSG_SUBARU_PG_Throttle 0x140
#define MSG_SUBARU_PG_Wheel_Speeds 0xD4
#define MSG_SUBARU_PG_Brake_Pedal 0xD1
#define MSG_SUBARU_PG_ES_LKAS 0x164
#define MSG_SUBARU_PG_ES_Distance 0x161
#define MSG_SUBARU_PG_Steering_Torque 0x371
#define SUBARU_PG_MAIN_BUS 0
#define SUBARU_PG_CAM_BUS 2
const CanMsg SUBARU_PG_TX_MSGS[] = {
{0x161, 0, 8},
{0x164, 0, 8}
{MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8},
{MSG_SUBARU_PG_ES_LKAS, SUBARU_PG_MAIN_BUS, 8}
};
#define SUBARU_PG_TX_MSGS_LEN (sizeof(SUBARU_PG_TX_MSGS) / sizeof(SUBARU_PG_TX_MSGS[0]))
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
AddrCheckStruct subaru_preglobal_addr_checks[] = {
{.msg = {{0x140, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{0x371, 0, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{0x144, 0, 8, .expected_timestep = 50000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_Throttle, SUBARU_PG_MAIN_BUS, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_Steering_Torque, SUBARU_PG_MAIN_BUS, 8, .expected_timestep = 20000U}, { 0 }, { 0 }}},
{.msg = {{MSG_SUBARU_PG_CruiseControl, SUBARU_PG_MAIN_BUS, 8, .expected_timestep = 50000U}, { 0 }, { 0 }}},
};
#define SUBARU_PG_ADDR_CHECK_LEN (sizeof(subaru_preglobal_addr_checks) / sizeof(subaru_preglobal_addr_checks[0]))
addr_checks subaru_preglobal_rx_checks = {subaru_preglobal_addr_checks, SUBARU_PG_ADDR_CHECK_LEN};
@ -28,9 +43,11 @@ static int subaru_preglobal_rx_hook(CANPacket_t *to_push) {
bool valid = addr_safety_check(to_push, &subaru_preglobal_rx_checks, NULL, NULL, NULL, NULL);
if (valid && (GET_BUS(to_push) == 0U)) {
const int bus = GET_BUS(to_push);
if (valid && (bus == SUBARU_PG_MAIN_BUS)) {
int addr = GET_ADDR(to_push);
if (addr == 0x371) {
if (addr == MSG_SUBARU_PG_Steering_Torque) {
int torque_driver_new;
torque_driver_new = (GET_BYTE(to_push, 3) >> 5) + (GET_BYTE(to_push, 4) << 3);
torque_driver_new = to_signed(torque_driver_new, 11);
@ -38,25 +55,25 @@ static int subaru_preglobal_rx_hook(CANPacket_t *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x144) {
if (addr == MSG_SUBARU_PG_CruiseControl) {
bool cruise_engaged = GET_BIT(to_push, 49U) != 0U;
pcm_cruise_check(cruise_engaged);
}
// update vehicle moving with any non-zero wheel speed
if (addr == 0xD4) {
if (addr == MSG_SUBARU_PG_Wheel_Speeds) {
vehicle_moving = ((GET_BYTES(to_push, 0, 4) >> 12) != 0U) || (GET_BYTES(to_push, 4, 4) != 0U);
}
if (addr == 0xD1) {
if (addr == MSG_SUBARU_PG_Brake_Pedal) {
brake_pressed = ((GET_BYTES(to_push, 0, 4) >> 16) & 0xFFU) > 0U;
}
if (addr == 0x140) {
if (addr == MSG_SUBARU_PG_Throttle) {
gas_pressed = GET_BYTE(to_push, 0) != 0U;
}
generic_rx_checks((addr == 0x164));
generic_rx_checks((addr == MSG_SUBARU_PG_ES_LKAS));
}
return valid;
}
@ -71,7 +88,7 @@ static int subaru_preglobal_tx_hook(CANPacket_t *to_send) {
}
// steer cmd checks
if (addr == 0x164) {
if (addr == MSG_SUBARU_PG_ES_LKAS) {
int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 8) & 0x1FFFU);
desired_torque = -1 * to_signed(desired_torque, 13);
@ -86,17 +103,14 @@ static int subaru_preglobal_tx_hook(CANPacket_t *to_send) {
static int subaru_preglobal_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == 0) {
bus_fwd = 2; // Camera CAN
if (bus_num == SUBARU_PG_MAIN_BUS) {
bus_fwd = SUBARU_PG_CAM_BUS; // Camera CAN
}
if (bus_num == 2) {
// Preglobal platform
// 0x161 is ES_CruiseThrottle
// 0x164 is ES_LKAS
int block_msg = ((addr == 0x161) || (addr == 0x164));
if (bus_num == SUBARU_PG_CAM_BUS) {
int block_msg = ((addr == MSG_SUBARU_PG_ES_Distance) || (addr == MSG_SUBARU_PG_ES_LKAS));
if (!block_msg) {
bus_fwd = 0; // Main CAN
bus_fwd = SUBARU_PG_MAIN_BUS; // Main CAN
}
}