Skip to content

Commit

Permalink
Merge branch 'commaai:master' into master
Browse files Browse the repository at this point in the history
  • Loading branch information
@rav4kumar
rav4kumar authored Jul 12, 2023
2 parents c63274a + 8ba5683 commit 30c3d23
Show file tree
Hide file tree
Showing 2 changed files with 89 additions and 58 deletions.
91 changes: 54 additions & 37 deletions board/safety/safety_subaru.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down Expand Up @@ -88,36 +106,36 @@ 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);
update_sample(&torque_driver, torque_driver_new);
}

// 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;
}
Expand All @@ -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);

Expand All @@ -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
}
}

Expand Down
56 changes: 35 additions & 21 deletions board/safety/safety_subaru_preglobal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -28,35 +43,37 @@ 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);
update_sample(&torque_driver, torque_driver_new);
}

// 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;
}
Expand All @@ -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);

Expand All @@ -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
}
}

Expand Down

0 comments on commit 30c3d23

Please sign in to comment.