Skip to content

Commit

Permalink
Cleanup: avoid unnecessary bus checks in rx hooks
Browse files Browse the repository at this point in the history
  • Loading branch information
@rbiasini
rbiasini committed Mar 7, 2020
1 parent c7d0d5f commit 4ecc6b3
Show file tree
Hide file tree
Showing 6 changed files with 62 additions and 70 deletions.
15 changes: 7 additions & 8 deletions board/safety/safety_chrysler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,20 +76,19 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
chrysler_get_checksum, chrysler_compute_checksum,
chrysler_get_counter);

if (valid) {
int bus = GET_BUS(to_push);
if (valid && (GET_BUS(to_push) == 0)) {
int addr = GET_ADDR(to_push);

// Measured eps torque
if ((addr == 544) && (bus == 0)) {
if (addr == 544) {
int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;

// update array of samples
update_sample(&chrysler_torque_meas, torque_meas_new);
}

// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 500) && (bus == 0)) {
if (addr == 500) {
int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
if (cruise_engaged && !chrysler_cruise_engaged_last) {
controls_allowed = 1;
Expand All @@ -101,14 +100,14 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// update speed
if ((addr == 514) && (bus == 0)) {
if (addr == 514) {
int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
chrysler_speed = (speed_l + speed_r) / 2;
}

// exit controls on rising edge of gas press
if ((addr == 308) && (bus == 0)) {
if (addr == 308) {
bool gas = (GET_BYTE(to_push, 5) & 0x7F) != 0;
if (gas && !chrysler_gas_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
controls_allowed = 0;
Expand All @@ -117,7 +116,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// exit controls on rising edge of brake press
if ((addr == 320) && (bus == 0)) {
if (addr == 320) {
bool brake = (GET_BYTE(to_push, 0) & 0x7) == 5;
if (brake && (!chrysler_brake_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
controls_allowed = 0;
Expand All @@ -126,7 +125,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// check if stock camera ECU is on bus 0
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x292)) {
relay_malfunction = true;
}
}
Expand Down
5 changes: 2 additions & 3 deletions board/safety/safety_gm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
bool valid = addr_safety_check(to_push, gm_rx_checks, GM_RX_CHECK_LEN,
NULL, NULL, NULL);

if (valid) {
int bus = GET_BUS(to_push);
if (valid && (GET_BUS(to_push) == 0)) {
int addr = GET_ADDR(to_push);

if (addr == 388) {
Expand Down Expand Up @@ -115,7 +114,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// on powertrain bus.
// 384 = ASCMLKASteeringCmd
// 715 = ASCMGasRegenCmd
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) {
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 384) || (addr == 715))) {
relay_malfunction = true;
}
}
Expand Down
7 changes: 3 additions & 4 deletions board/safety/safety_hyundai.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN,
NULL, NULL, NULL);

if (valid) {
int bus = GET_BUS(to_push);
if (valid && GET_BUS(to_push) == 0) {
int addr = GET_ADDR(to_push);

if (addr == 897) {
Expand All @@ -51,7 +50,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// exit controls on rising edge of gas press
if ((addr == 608) && (bus == 0)) {
if (addr == 608) {
bool gas = (GET_BYTE(to_push, 7) >> 6) != 0;
if (gas && ! hyundai_gas_last) {
controls_allowed = 0;
Expand All @@ -60,7 +59,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// check if stock camera ECU is on bus 0
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) {
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) {
relay_malfunction = true;
}
}
Expand Down
73 changes: 35 additions & 38 deletions board/safety/safety_subaru.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,51 +64,48 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
NULL, NULL, NULL);
}

if (valid) {
int bus = GET_BUS(to_push);
if (valid && (GET_BUS(to_push) == 0)) {
int addr = GET_ADDR(to_push);
if (bus == 0) {
if (((addr == 0x119) && subaru_global) ||
((addr == 0x371) && !subaru_global)) {
int torque_driver_new;
if (subaru_global) {
torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
} else {
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(&subaru_torque_driver, torque_driver_new);
if (((addr == 0x119) && subaru_global) ||
((addr == 0x371) && !subaru_global)) {
int torque_driver_new;
if (subaru_global) {
torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
} else {
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(&subaru_torque_driver, torque_driver_new);
}

// enter controls on rising edge of ACC, exit controls on ACC off
if (((addr == 0x240) && subaru_global) ||
((addr == 0x144) && !subaru_global)) {
int bit_shift = subaru_global ? 9 : 17;
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
}
if (!cruise_engaged) {
controls_allowed = 0;
}
subaru_cruise_engaged_last = cruise_engaged;
// enter controls on rising edge of ACC, exit controls on ACC off
if (((addr == 0x240) && subaru_global) ||
((addr == 0x144) && !subaru_global)) {
int bit_shift = subaru_global ? 9 : 17;
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
}

// exit controls on rising edge of gas press
if (((addr == 0x40) && subaru_global) ||
((addr == 0x140) && !subaru_global)) {
int byte = subaru_global ? 4 : 0;
bool gas = GET_BYTE(to_push, byte) != 0;
if (gas && !subaru_gas_last) {
controls_allowed = 0;
}
subaru_gas_last = gas;
if (!cruise_engaged) {
controls_allowed = 0;
}
subaru_cruise_engaged_last = cruise_engaged;
}

if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
(((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) {
relay_malfunction = true;
// exit controls on rising edge of gas press
if (((addr == 0x40) && subaru_global) ||
((addr == 0x140) && !subaru_global)) {
int byte = subaru_global ? 4 : 0;
bool gas = GET_BYTE(to_push, byte) != 0;
if (gas && !subaru_gas_last) {
controls_allowed = 0;
}
subaru_gas_last = gas;
}

if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
(((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) {
relay_malfunction = true;
}
}
return valid;
Expand Down
17 changes: 8 additions & 9 deletions board/safety/safety_toyota.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,12 +65,11 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

bool valid = addr_safety_check(to_push, toyota_rx_checks, TOYOTA_RX_CHECKS_LEN,
toyota_get_checksum, toyota_compute_checksum, NULL);
if (valid) {
int bus = GET_BUS(to_push);
if (valid && (GET_BUS(to_push) == 0)) {
int addr = GET_ADDR(to_push);

// get eps motor torque (0.66 factor in dbc)
if ((addr == 0x260) && (bus == 0)) {
if (addr == 0x260) {
int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6);
torque_meas_new = to_signed(torque_meas_new, 16);

Expand All @@ -86,7 +85,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x1D2) && (bus == 0)) {
if (addr == 0x1D2) {
// 5th bit is CRUISE_ACTIVE
int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
if (!cruise_engaged) {
Expand All @@ -99,7 +98,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// sample speed
if ((addr == 0xaa) && (bus == 0)) {
if (addr == 0xaa) {
int speed = 0;
// sum 4 wheel speeds
for (int i=0; i<8; i+=2) {
Expand All @@ -111,7 +110,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of brake pedal
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
if (((addr == 0x224) || (addr == 0x226)) && (bus == 0)) {
if ((addr == 0x224) || (addr == 0x226)) {
int byte = (addr == 0x224) ? 0 : 4;
bool brake = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
if (brake && (!toyota_brake_prev || toyota_moving)) {
Expand All @@ -121,7 +120,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// exit controls on rising edge of interceptor gas press
if ((addr == 0x201) && (bus == 0)) {
if (addr == 0x201) {
gas_interceptor_detected = 1;
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) &&
Expand All @@ -132,7 +131,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// exit controls on rising edge of gas press
if ((addr == 0x2C1) && (bus == 0)) {
if (addr == 0x2C1) {
bool gas = GET_BYTE(to_push, 6) != 0;
if (gas && !toyota_gas_prev && !gas_interceptor_detected) {
controls_allowed = 0;
Expand All @@ -141,7 +140,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4) && (bus == 0)) {
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) {
relay_malfunction = true;
}
}
Expand Down
15 changes: 7 additions & 8 deletions board/safety/safety_volkswagen.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,14 +101,13 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN,
volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_get_counter);

if (valid) {
int bus = GET_BUS(to_push);
if (valid && (GET_BUS(to_push) == 0)) {
int addr = GET_ADDR(to_push);

// Update in-motion state by sampling front wheel speeds
// Signal: ESP_19.ESP_VL_Radgeschw_02 (front left) in scaled km/h
// Signal: ESP_19.ESP_VR_Radgeschw_02 (front right) in scaled km/h
if ((bus == 0) && (addr == MSG_ESP_19)) {
if (addr == MSG_ESP_19) {
int wheel_speed_fl = GET_BYTE(to_push, 4) | (GET_BYTE(to_push, 5) << 8);
int wheel_speed_fr = GET_BYTE(to_push, 6) | (GET_BYTE(to_push, 7) << 8);
// Check for average front speed in excess of 0.3m/s, 1.08km/h
Expand All @@ -119,7 +118,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// Update driver input torque samples
// Signal: EPS_01.Driver_Strain (absolute torque)
// Signal: EPS_01.Driver_Strain_VZ (direction)
if ((bus == 0) && (addr == MSG_EPS_01)) {
if (addr == MSG_EPS_01) {
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
if (sign == 1) {
Expand All @@ -130,14 +129,14 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// Update ACC status from drivetrain coordinator for controls-allowed state
// Signal: TSK_06.TSK_Status
if ((bus == 0) && (addr == MSG_TSK_06)) {
if (addr == MSG_TSK_06) {
int acc_status = (GET_BYTE(to_push, 3) & 0x7);
controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
}

// Exit controls on rising edge of gas press
// Signal: Motor_20.MO_Fahrpedalrohwert_01
if ((bus == 0) && (addr == MSG_MOTOR_20)) {
if (addr == MSG_MOTOR_20) {
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
if ((gas > 0) && (volkswagen_gas_prev == 0)) {
controls_allowed = 0;
Expand All @@ -147,7 +146,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// Exit controls on rising edge of brake press
// Signal: ESP_05.ESP_Fahrer_bremst
if ((bus == 0) && (addr == MSG_ESP_05)) {
if (addr == MSG_ESP_05) {
bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
if (brake_pressed && (!(volkswagen_brake_pressed_prev) || volkswagen_moving)) {
controls_allowed = 0;
Expand All @@ -156,7 +155,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}

// If there are HCA messages on bus 0 not sent by OP, there's a relay problem
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MSG_HCA_01)) {
relay_malfunction = true;
}
}
Expand Down

0 comments on commit 4ecc6b3

Please sign in to comment.