From 5b1494514042721b63f71254700ddcf94dbbd9c7 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh <8762862+adeebshihadeh@users.noreply.github.com> Date: Tue, 16 Jun 2020 02:01:00 -0700 Subject: [PATCH] Abstract RX checks for gas, brakes, and relay malfunction (#556) * abstract gas, brake, and relay RX checks * toyota * mazda * vw and subaru * gm, honda, nissan * chrysler * rename * revert that --- board/safety.h | 21 ++++++ board/safety/safety_chrysler.h | 15 +--- board/safety/safety_ford.h | 4 +- board/safety/safety_gm.h | 20 +----- board/safety/safety_honda.h | 26 ++----- board/safety/safety_hyundai.h | 28 ++------ board/safety/safety_mazda.h | 115 +++++++++++++------------------ board/safety/safety_nissan.h | 24 +------ board/safety/safety_subaru.h | 44 +++--------- board/safety/safety_toyota.h | 30 +++----- board/safety/safety_volkswagen.h | 51 ++++---------- board/safety_declarations.h | 3 + tests/safety/test_chrysler.py | 10 +++ 13 files changed, 132 insertions(+), 259 deletions(-) diff --git a/board/safety.h b/board/safety.h index e426779..32d9d3f 100644 --- a/board/safety.h +++ b/board/safety.h @@ -199,6 +199,25 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, return is_msg_valid(rx_checks, index); } +void generic_rx_checks(bool stock_ecu_detected) { + // exit controls on rising edge of gas press + if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + + // exit controls on rising edge of brake press + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + + // check if stock ECU is on bus broken by car harness + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) { + relay_malfunction_set(); + } +} + void relay_malfunction_set(void) { relay_malfunction = true; fault_occurred(FAULT_RELAY_MALFUNCTION); @@ -246,7 +265,9 @@ int set_safety_hooks(uint16_t mode, int16_t param) { relay_malfunction = false; gas_interceptor_detected = false; gas_interceptor_prev = 0; + gas_pressed = false; gas_pressed_prev = false; + brake_pressed = false; brake_pressed_prev = false; cruise_engaged_prev = false; vehicle_speed = 0; diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index be3b9c5..044485c 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -66,8 +66,6 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { chrysler_get_checksum, chrysler_compute_checksum, chrysler_get_counter); - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -101,26 +99,19 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 308) { - bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0; - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && ((int)vehicle_speed > CHRYSLER_GAS_THRSLD)) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; + gas_pressed = ((GET_BYTE(to_push, 5) & 0x7F) != 0) && ((int)vehicle_speed > CHRYSLER_GAS_THRSLD); } // exit controls on rising edge of brake press if (addr == 320) { - bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5; + brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5; if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } brake_pressed_prev = brake_pressed; } - // check if stock camera ECU is on bus 0 - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x292)) { - relay_malfunction_set(); - } + generic_rx_checks((addr == 0x292)); } return valid; } diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 3acf8eb..2c0c677 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -37,7 +37,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of brake press or on brake press when // speed > 0 if (addr == 0x165) { - int brake_pressed = GET_BYTE(to_push, 0) & 0x20; + brake_pressed = GET_BYTE(to_push, 0) & 0x20; if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = 0; } @@ -46,7 +46,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 0x204) { - bool gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0; + gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0; if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 1d617a2..0e9624d 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -38,8 +38,6 @@ 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); - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -72,25 +70,15 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } } - // exit controls on rising edge of brake press or on brake press when // speed > 0 if (addr == 241) { // Brake pedal's potentiometer returns near-zero reading // even when pedal is not pressed - bool brake_pressed = GET_BYTE(to_push, 1) >= 10; - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + brake_pressed = GET_BYTE(to_push, 1) >= 10; } - // exit controls on rising edge of gas press if (addr == 417) { - bool gas_pressed = GET_BYTE(to_push, 6) != 0; - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; + gas_pressed = GET_BYTE(to_push, 6) != 0; } // exit controls on regen paddle @@ -105,9 +93,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) && ((addr == 384) || (addr == 715))) { - relay_malfunction_set(); - } + generic_rx_checks(((addr == 384) || (addr == 715))); } return valid; } diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 33d1ef8..3923446 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -89,8 +89,6 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { honda_get_checksum, honda_compute_checksum, honda_get_counter); } - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (valid) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); @@ -125,36 +123,22 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // in these cases, this is used instead. // most hondas: 0x17C bit 53 // accord, crv: 0x1BE bit 4 - // exit controls on rising edge of brake press or on brake press when speed > 0 bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C); if (is_user_brake_msg) { - bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20); - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20); } - // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6) // length check because bosch hardware also uses this id (0x201 w/ len = 8) if ((addr == 0x201) && (len == 6)) { gas_interceptor_detected = 1; int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push); - if (!unsafe_allow_gas && (gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && - (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) { - controls_allowed = 0; - } + gas_pressed = gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD; gas_interceptor_prev = gas_interceptor; } - // exit controls on rising edge of gas press if no interceptor if (!gas_interceptor_detected) { if (addr == 0x17C) { - bool gas_pressed = GET_BYTE(to_push, 0) != 0; - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; + gas_pressed = GET_BYTE(to_push, 0) != 0; } } @@ -178,13 +162,15 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // if steering controls messages are received on the destination bus, it's an indication // that the relay might be malfunctioning + bool stock_ecu_detected = false; int bus_rdr_car = (honda_hw == HONDA_BH_HW) ? 0 : 2; // radar bus, car side if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 0xE4) || (addr == 0x194))) { if (((honda_hw != HONDA_N_HW) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_N_HW) && (bus == 0))) { - relay_malfunction_set(); + stock_ecu_detected = true; } } + generic_rx_checks(stock_ecu_detected); } return valid; } diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index c66770d..93a4375 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -102,12 +102,9 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { hyundai_get_counter); } - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { + int addr = GET_ADDR(to_push); - int addr = GET_ADDR(to_push); - int bus = GET_BUS(to_push); - - if (valid && (bus == 0)) { if (addr == 593) { int torque_driver_new = ((GET_BYTES_04(to_push) & 0x7ff) * 0.79) - 808; // scale down new driver torque signal to match previous one // update array of samples @@ -127,22 +124,15 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { cruise_engaged_prev = cruise_engaged; } - // exit controls on rising edge of gas press if ((addr == 608) || (hyundai_legacy && (addr == 881))) { - bool gas_pressed; if (addr == 608) { gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0; } else { gas_pressed = (((GET_BYTE(to_push, 4) & 0x7F) << 1) | GET_BYTE(to_push, 3) >> 7) != 0; } - - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; } - // sample subaru wheel speed, averaging opposite corners + // sample wheel speed, averaging opposite corners if (addr == 902) { int hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL @@ -150,19 +140,11 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD; } - // exit controls on rising edge of brake press if (addr == 916) { - bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0; - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0; } - // check if stock camera ECU is on bus 0 - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) { - relay_malfunction_set(); - } + generic_rx_checks((addr == 832)); } return valid; } diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index 236ce68..12e9240 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -41,80 +41,61 @@ const int MAZDA_RX_CHECKS_LEN = sizeof(mazda_rx_checks) / sizeof(mazda_rx_checks // 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) { - bool valid; - - valid = addr_safety_check(to_push, mazda_rx_checks, MAZDA_RX_CHECKS_LEN, + bool valid = addr_safety_check(to_push, mazda_rx_checks, MAZDA_RX_CHECKS_LEN, NULL, NULL, NULL); - if (valid) { - int bus = GET_BUS(to_push); + if (valid && (GET_BUS(to_push) == MAZDA_MAIN)) { int addr = GET_ADDR(to_push); - if (bus == MAZDA_MAIN) { + 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); - 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 - 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) { - int torque_driver_new = GET_BYTE(to_push, 0) - 127; - // update array of samples - update_sample(&torque_driver, torque_driver_new); - } - - // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == MAZDA_CRZ_CTRL) { - bool cruise_engaged = GET_BYTE(to_push, 0) & 8; - 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; - } - } - } else { - controls_allowed = 0; - } - cruise_engaged_prev = cruise_engaged; - } - - // Exit controls on rising edge of gas press - if (addr == MAZDA_ENGINE_DATA) { - bool gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0)); - if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; - } - - // Exit controls on rising edge of brake press - if (addr == MAZDA_PEDALS) { - bool brake_pressed = (GET_BYTE(to_push, 0) & 0x10); - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; - } - - // if we see lkas msg on MAZDA_MAIN bus then relay is closed - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MAZDA_LKAS)) { - relay_malfunction_set(); + // 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) { + int torque_driver_new = GET_BYTE(to_push, 0) - 127; + // update array of samples + update_sample(&torque_driver, torque_driver_new); + } + + // enter controls on rising edge of ACC, exit controls on ACC off + if (addr == MAZDA_CRZ_CTRL) { + bool cruise_engaged = GET_BYTE(to_push, 0) & 8; + 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; + } + } + } else { + controls_allowed = 0; + } + cruise_engaged_prev = cruise_engaged; + } + + if (addr == MAZDA_ENGINE_DATA) { + gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0)); + } + + if (addr == MAZDA_PEDALS) { + brake_pressed = (GET_BYTE(to_push, 0) & 0x10); + } + + generic_rx_checks((addr == MAZDA_LKAS)); } return valid; } diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index 6579e1c..d77aa04 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -30,8 +30,6 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, nissan_rx_checks, NISSAN_RX_CHECK_LEN, NULL, NULL, NULL); - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (valid) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -55,45 +53,25 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { vehicle_moving = vehicle_speed > 0.; } - // exit controls on rising edge of gas press // X-Trail 0x15c, Leaf 0x239 if ((addr == 0x15c) || (addr == 0x239)) { - bool gas_pressed = true; if (addr == 0x15c){ gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)) > 1; } else { gas_pressed = GET_BYTE(to_push, 0) > 3; } - - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; - } - - // 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x169)) { - relay_malfunction_set(); } } - // exit controls on rising edge of brake press, or if speed > 0 and brake // X-trail 0x454, Leaf 0x1cc if ((addr == 0x454) || (addr == 0x1cc)) { - bool brake_pressed = true; if (addr == 0x454){ brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0; } else { brake_pressed = GET_BYTE(to_push, 0) > 3; } - - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; } - // Handle cruise enabled if ((bus == 2) && (addr == 0x30f)) { bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1; @@ -106,6 +84,8 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } cruise_engaged_prev = cruise_engaged; } + + generic_rx_checks((addr == 0x169)); } return valid; } diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index bb88572..b2e8c07 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -58,8 +58,6 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN, subaru_get_checksum, subaru_compute_checksum, subaru_get_counter); - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); if (addr == 0x119) { @@ -81,7 +79,7 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { cruise_engaged_prev = cruise_engaged; } - // sample subaru wheel speed, averaging opposite corners + // sample wheel speed, averaging opposite corners if (addr == 0x13a) { int subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF; // RL @@ -89,27 +87,15 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD; } - // exit controls on rising edge of brake press if (addr == 0x139) { - bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0; - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0; } - // exit controls on rising edge of gas press if (addr == 0x40) { - bool gas_pressed = GET_BYTE(to_push, 4) != 0; - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; + gas_pressed = GET_BYTE(to_push, 4) != 0; } - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x122)) { - relay_malfunction_set(); - } + generic_rx_checks((addr == 0x122)); } return valid; } @@ -119,8 +105,6 @@ static int subaru_legacy_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, subaru_l_rx_checks, SUBARU_L_RX_CHECK_LEN, NULL, NULL, NULL); - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); if (addr == 0x371) { @@ -142,7 +126,7 @@ static int subaru_legacy_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { cruise_engaged_prev = cruise_engaged; } - // sample subaru wheel speed, averaging opposite corners + // sample wheel speed, averaging opposite corners if (addr == 0xD4) { int subaru_speed = (GET_BYTES_04(to_push) >> 16) & 0xFFFF; // FR subaru_speed += GET_BYTES_48(to_push) & 0xFFFF; // RL @@ -150,27 +134,15 @@ static int subaru_legacy_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD; } - // exit controls on rising edge of brake press if (addr == 0xD1) { - bool brake_pressed = ((GET_BYTES_04(to_push) >> 16) & 0xFF) > 0; - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + brake_pressed = ((GET_BYTES_04(to_push) >> 16) & 0xFF) > 0; } - // exit controls on rising edge of gas press if (addr == 0x140) { - bool gas_pressed = GET_BYTE(to_push, 0) != 0; - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; + gas_pressed = GET_BYTE(to_push, 0) != 0; } - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x164)) { - relay_malfunction_set(); - } + generic_rx_checks((addr == 0x164)); } return valid; } diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 1f749a9..2ffe58b 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -65,7 +65,6 @@ 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); - bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -99,12 +98,10 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } cruise_engaged_prev = cruise_engaged; - // handle gas_pressed - bool gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0; - if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) { - controls_allowed = 0; + // sample gas pedal + if (!gas_interceptor_detected) { + gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0; } - gas_pressed_prev = gas_pressed; } // sample speed @@ -118,32 +115,23 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { vehicle_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD; } - // 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)) { int byte = (addr == 0x224) ? 0 : 4; - bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0; - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0; } - // exit controls on rising edge of interceptor gas press + // sample gas interceptor if (addr == 0x201) { gas_interceptor_detected = 1; int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push); - if (!unsafe_allow_gas && (gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && - (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) { - controls_allowed = 0; - } + gas_pressed = gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD; + + // TODO: remove this, only left in for gas_interceptor_prev test gas_interceptor_prev = gas_interceptor; } - // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) { - relay_malfunction_set(); - } + generic_rx_checks((addr == 0x2E4)); } return valid; } diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index 419fb1a..2a2c447 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -174,30 +174,17 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { 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 (addr == MSG_MOTOR_20) { - bool gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0; - if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; + gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0; } - // Exit controls on rising edge of brake press // Signal: ESP_05.ESP_Fahrer_bremst if (addr == MSG_ESP_05) { - bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2; - if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2; } - // If there are HCA messages on bus 0 not sent by OP, there's a relay problem - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MSG_HCA_01)) { - relay_malfunction_set(); - } + generic_rx_checks((addr == MSG_HCA_01)); } return valid; } @@ -207,14 +194,13 @@ static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_RX_CHECKS_LEN, volkswagen_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_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: Bremse_3.Radgeschw__VL_4_1 (front left) // Signal: Bremse_3.Radgeschw__VR_4_1 (front right) - if ((bus == 0) && (addr == MSG_BREMSE_3)) { + if (addr == MSG_BREMSE_3) { int wheel_speed_fl = (GET_BYTE(to_push, 0) | (GET_BYTE(to_push, 1) << 8)) >> 1; int wheel_speed_fr = (GET_BYTE(to_push, 2) | (GET_BYTE(to_push, 3) << 8)) >> 1; // Check for average front speed in excess of 0.3m/s, 1.08km/h @@ -225,7 +211,7 @@ static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // Update driver input torque samples // Signal: Lenkhilfe_3.LH3_LM (absolute torque) // Signal: Lenkhilfe_3.LH3_LMSign (direction) - if ((bus == 0) && (addr == MSG_LENKHILFE_3)) { + if (addr == MSG_LENKHILFE_3) { int torque_driver_new = GET_BYTE(to_push, 2) | ((GET_BYTE(to_push, 3) & 0x3) << 8); int sign = (GET_BYTE(to_push, 3) & 0x4) >> 2; if (sign == 1) { @@ -236,35 +222,22 @@ static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // Update ACC status from ECU for controls-allowed state // Signal: Motor_2.GRA_Status - if ((bus == 0) && (addr == MSG_MOTOR_2)) { + if (addr == MSG_MOTOR_2) { int acc_status = (GET_BYTE(to_push, 2) & 0xC0) >> 6; controls_allowed = ((acc_status == 1) || (acc_status == 2)) ? 1 : 0; } - // Exit controls on rising edge of gas press // Signal: Motor_3.Fahrpedal_Rohsignal - if ((bus == 0) && (addr == MSG_MOTOR_3)) { - int gas_pressed = (GET_BYTE(to_push, 2)); - if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; + if (addr == MSG_MOTOR_3) { + gas_pressed = (GET_BYTE(to_push, 2)); } - // Exit controls on rising edge of brake press // Signal: Motor_2.Bremslichtschalter - if ((bus == 0) && (addr == MSG_MOTOR_2)) { - bool brake_pressed = (GET_BYTE(to_push, 2) & 0x1); - if (brake_pressed && (!(brake_pressed_prev) || vehicle_moving)) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + if (addr == MSG_MOTOR_2) { + brake_pressed = (GET_BYTE(to_push, 2) & 0x1); } - // 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_1)) { - relay_malfunction_set(); - } + generic_rx_checks((addr == MSG_HCA_1)); } return valid; } diff --git a/board/safety_declarations.h b/board/safety_declarations.h index eeaf50b..3f8221c 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -69,6 +69,7 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, 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)); +void generic_rx_checks(bool stock_ecu_detected); void relay_malfunction_set(void); void relay_malfunction_reset(void); @@ -95,7 +96,9 @@ bool controls_allowed = false; bool relay_malfunction = false; bool gas_interceptor_detected = false; int gas_interceptor_prev = 0; +bool gas_pressed = false; bool gas_pressed_prev = false; +bool brake_pressed = false; bool brake_pressed_prev = false; bool cruise_engaged_prev = false; float vehicle_speed = 0; diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index c485cb8..c2d8a0a 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -5,6 +5,7 @@ from panda.tests.safety import libpandasafety_py import panda.tests.safety.common as common from panda.tests.safety.common import CANPackerPanda +GAS_THRESHOLD = 2.14 class TestChryslerSafety(common.PandaSafetyTest, common.TorqueSteeringSafetyTest): TX_MSGS = [[571, 0], [658, 0], [678, 0]] @@ -66,6 +67,15 @@ class TestChryslerSafety(common.PandaSafetyTest, common.TorqueSteeringSafetyTest values = {"LKAS_STEERING_TORQUE": torque} return self.packer.make_can_msg_panda("LKAS_COMMAND", 0, values) + def test_prev_gas(self): + self.assertFalse(self.safety.get_gas_pressed_prev()) + + # chrysler has an additional check on wheel speed + self._rx(self._speed_msg(GAS_THRESHOLD + 1)) + for pressed in [self.GAS_PRESSED_THRESHOLD + 1, 0]: + self._rx(self._gas_msg(pressed)) + self.assertEqual(bool(pressed), self.safety.get_gas_pressed_prev()) + def test_disengage_on_gas(self): self.safety.set_controls_allowed(1) self._rx(self._speed_msg(2.1))