diff --git a/board/config.h b/board/config.h index 15096b69fa809b..335d2e1ec5e54d 100644 --- a/board/config.h +++ b/board/config.h @@ -36,6 +36,10 @@ __typeof__ (b) _b = (b); \ (_a > _b) ? _a : _b; }) +#define ABS(a) \ + ({ __typeof__ (a) _a = (a); \ + (_a > 0) ? _a : (-_a); }) + #define MAX_RESP_LEN 0x40U // Around (1Mbps / 8 bits/byte / 12 bytes per message) diff --git a/board/crc.h b/board/crc.h new file mode 100644 index 00000000000000..ab969e5176c8b9 --- /dev/null +++ b/board/crc.h @@ -0,0 +1,16 @@ +uint8_t crc_checksum(uint8_t *dat, int len, const uint8_t poly) { + uint8_t crc = 0xFF; + int i, j; + for (i = len - 1; i >= 0; i--) { + crc ^= dat[i]; + for (j = 0; j < 8; j++) { + if ((crc & 0x80U) != 0U) { + crc = (uint8_t)((crc << 1) ^ poly); + } + else { + crc <<= 1; + } + } + } + return crc; +} diff --git a/board/drivers/llcan.h b/board/drivers/llcan.h index 4cd9b4b5abd416..5e9f276e583c6b 100644 --- a/board/drivers/llcan.h +++ b/board/drivers/llcan.h @@ -52,7 +52,7 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s void llcan_init(CAN_TypeDef *CAN_obj) { // Enter init mode register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); - + // Wait for INAK bit to be set while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {} diff --git a/board/pedal/main.c b/board/pedal/main.c index 667d27bf0f8d29..3192f4b2bd2f77 100644 --- a/board/pedal/main.c +++ b/board/pedal/main.c @@ -19,6 +19,7 @@ #include "drivers/timer.h" #include "gpio.h" +#include "crc.h" #define CAN CAN1 @@ -105,26 +106,6 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) #endif -// ***************************** pedal can checksum ***************************** - -uint8_t pedal_checksum(uint8_t *dat, int len) { - uint8_t crc = 0xFF; - uint8_t poly = 0xD5; // standard crc8 - int i, j; - for (i = len - 1; i >= 0; i--) { - crc ^= dat[i]; - for (j = 0; j < 8; j++) { - if ((crc & 0x80U) != 0U) { - crc = (uint8_t)((crc << 1) ^ poly); - } - else { - crc <<= 1; - } - } - } - return crc; -} - // ***************************** can port ***************************** // addresses to be used on CAN @@ -155,6 +136,8 @@ uint32_t current_index = 0; #define FAULT_INVALID 6U uint8_t state = FAULT_STARTUP; +const uint8_t crc_poly = 0xD5; // standard crc8 + void CAN1_RX0_IRQ_Handler(void) { while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) { #ifdef DEBUG @@ -184,7 +167,7 @@ void CAN1_RX0_IRQ_Handler(void) { uint16_t value_1 = (dat[2] << 8) | dat[3]; bool enable = ((dat[4] >> 7) & 1U) != 0U; uint8_t index = dat[4] & COUNTER_CYCLE; - if (pedal_checksum(dat, CAN_GAS_SIZE - 1) == dat[5]) { + if (crc_checksum(dat, CAN_GAS_SIZE - 1, crc_poly) == dat[5]) { if (((current_index + 1U) & COUNTER_CYCLE) == index) { #ifdef DEBUG puts("setting gas "); @@ -247,7 +230,7 @@ void TIM3_IRQ_Handler(void) { dat[2] = (pdl1 >> 8) & 0xFFU; dat[3] = (pdl1 >> 0) & 0xFFU; dat[4] = ((state & 0xFU) << 4) | pkt_idx; - dat[5] = pedal_checksum(dat, CAN_GAS_SIZE - 1); + dat[5] = crc_checksum(dat, CAN_GAS_SIZE - 1, crc_poly); CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24); CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5] << 8); CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5 diff --git a/board/safety.h b/board/safety.h index 7fd057b5249837..407f33f6695ee8 100644 --- a/board/safety.h +++ b/board/safety.h @@ -4,7 +4,6 @@ #include "safety/safety_defaults.h" #include "safety/safety_honda.h" #include "safety/safety_toyota.h" -#include "safety/safety_toyota_ipas.h" #include "safety/safety_tesla.h" #include "safety/safety_gm_ascm.h" #include "safety/safety_gm.h" @@ -14,6 +13,7 @@ #include "safety/safety_chrysler.h" #include "safety/safety_subaru.h" #include "safety/safety_mazda.h" +#include "safety/safety_nissan.h" #include "safety/safety_volkswagen.h" #include "safety/safety_elm327.h" @@ -31,12 +31,13 @@ #define SAFETY_TESLA 10U #define SAFETY_SUBARU 11U #define SAFETY_MAZDA 13U -#define SAFETY_VOLKSWAGEN 15U -#define SAFETY_TOYOTA_IPAS 16U +#define SAFETY_NISSAN 14U +#define SAFETY_VOLKSWAGEN_MQB 15U #define SAFETY_ALLOUTPUT 17U #define SAFETY_GM_ASCM 18U #define SAFETY_NOOUTPUT 19U #define SAFETY_HONDA_BOSCH_HARNESS 20U +#define SAFETY_SUBARU_LEGACY 22U uint16_t current_safety_mode = SAFETY_SILENT; const safety_hooks *current_hooks = &nooutput_hooks; @@ -57,6 +58,21 @@ int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return current_hooks->fwd(bus_num, to_fwd); } +// Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8 +// algorithm. Called at init time for safety modes using CRC-8. +void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) { + for (int i = 0; i < 256; i++) { + uint8_t crc = i; + for (int j = 0; j < 8; j++) { + if ((crc & 0x80U) != 0U) + crc = (uint8_t)((crc << 1) ^ poly); + else + crc <<= 1; + } + crc_lut[i] = crc; + } +} + bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len) { bool allowed = false; for (int i = 0; i < len; i++) { @@ -184,13 +200,14 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_HYUNDAI, &hyundai_hooks}, {SAFETY_CHRYSLER, &chrysler_hooks}, {SAFETY_SUBARU, &subaru_hooks}, + {SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks}, {SAFETY_MAZDA, &mazda_hooks}, - {SAFETY_VOLKSWAGEN, &volkswagen_hooks}, + {SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks}, {SAFETY_NOOUTPUT, &nooutput_hooks}, #ifdef ALLOW_DEBUG {SAFETY_CADILLAC, &cadillac_hooks}, - {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, {SAFETY_TESLA, &tesla_hooks}, + {SAFETY_NISSAN, &nissan_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, {SAFETY_GM_ASCM, &gm_ascm_hooks}, {SAFETY_FORD, &ford_hooks}, diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 293839b7f62d98..0bcffd6363a217 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -4,28 +4,77 @@ const uint32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks const int CHRYSLER_MAX_RATE_UP = 3; const int CHRYSLER_MAX_RATE_DOWN = 3; const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor +const int CHRYSLER_GAS_THRSLD = 30; // 7% more than 2m/s +const int CHRYSLER_STANDSTILL_THRSLD = 10; // about 1m/s const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}}; // TODO: do checksum and counter checks AddrCheckStruct chrysler_rx_checks[] = { - {.addr = {544}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {500}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {544}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, + {.addr = {514}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, + {.addr = {500}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {308}, .bus = 0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {320}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, }; const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]); int chrysler_rt_torque_last = 0; int chrysler_desired_torque_last = 0; int chrysler_cruise_engaged_last = 0; +int chrysler_speed = 0; uint32_t chrysler_ts_last = 0; struct sample_t chrysler_torque_meas; // last few torques measured +static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *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) { + /* 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 = 0xFF; + int len = GET_LEN(to_push); + for (int j = 0; j < (len - 1); j++) { + uint8_t shift = 0x80; + uint8_t curr = (uint8_t)GET_BYTE(to_push, j); + for (int i=0; i<8; i++) { + uint8_t bit_sum = curr & shift; + uint8_t temp_chk = checksum & 0x80U; + if (bit_sum != 0U) { + bit_sum = 0x1C; + if (temp_chk != 0U) { + bit_sum = 1; + } + checksum = checksum << 1; + temp_chk = checksum | 1U; + bit_sum ^= temp_chk; + } else { + if (temp_chk != 0U) { + bit_sum = 0x1D; + } + checksum = checksum << 1; + bit_sum ^= checksum; + } + checksum = bit_sum; + shift = shift >> 1; + } + } + return ~checksum; +} + +static uint8_t chrysler_get_counter(CAN_FIFOMailBox_TypeDef *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) { bool valid = addr_safety_check(to_push, chrysler_rx_checks, CHRYSLER_RX_CHECK_LEN, - NULL, NULL, NULL); + 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 @@ -37,7 +86,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1F4) { + if (addr == 500) { int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7; if (cruise_engaged && !chrysler_cruise_engaged_last) { controls_allowed = 1; @@ -48,10 +97,33 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { chrysler_cruise_engaged_last = cruise_engaged; } - // TODO: add gas pressed check + // update speed + 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) { + bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0; + if (gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + } + + // exit controls on rising edge of brake press + if (addr == 320) { + bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5; + if (brake_pressed && (!brake_pressed_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } // 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; } } diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 97e0f57a2cd6ee..067ed1cadd3983 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -7,8 +7,6 @@ // brake rising edge // brake > 0mph -int ford_brake_prev = 0; -int ford_gas_prev = 0; bool ford_moving = false; static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { @@ -39,20 +37,20 @@ 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 = GET_BYTE(to_push, 0) & 0x20; - if (brake && (!(ford_brake_prev) || ford_moving)) { + int brake_pressed = GET_BYTE(to_push, 0) & 0x20; + if (brake_pressed && (!brake_pressed_prev || ford_moving)) { controls_allowed = 0; } - ford_brake_prev = brake; + brake_pressed_prev = brake_pressed; } // exit controls on rising edge of gas press if (addr == 0x204) { - int gas = (GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1); - if (gas && !(ford_gas_prev)) { + bool gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0; + if (gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } - ford_gas_prev = gas; + gas_pressed_prev = gas_pressed; } if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) { @@ -74,7 +72,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_moving); + int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && ford_moving); bool current_controls_allowed = controls_allowed && !(pedal_pressed); if (relay_malfunction) { diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 81512908488528..8672cc267a663a 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -33,8 +33,6 @@ AddrCheckStruct gm_rx_checks[] = { }; const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]); -int gm_brake_prev = 0; -int gm_gas_prev = 0; bool gm_moving = false; int gm_rt_torque_last = 0; int gm_desired_torque_last = 0; @@ -46,8 +44,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) { @@ -82,25 +79,22 @@ 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) { - int brake = GET_BYTE(to_push, 1); // Brake pedal's potentiometer returns near-zero reading // even when pedal is not pressed - if (brake < 10) { - brake = 0; - } - if (brake && (!gm_brake_prev || gm_moving)) { + bool brake_pressed = GET_BYTE(to_push, 1) >= 10; + if (brake_pressed && (!brake_pressed_prev || gm_moving)) { controls_allowed = 0; } - gm_brake_prev = brake; + brake_pressed_prev = brake_pressed; } // exit controls on rising edge of gas press if (addr == 417) { - int gas = GET_BYTE(to_push, 6); - if (gas && !gm_gas_prev) { + bool gas_pressed = GET_BYTE(to_push, 6) != 0; + if (gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } - gm_gas_prev = gas; + gas_pressed_prev = gas_pressed; } // exit controls on regen paddle @@ -115,7 +109,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; } } @@ -144,7 +138,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_moving); + int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && gm_moving); bool current_controls_allowed = controls_allowed && !pedal_pressed; // BRAKE: safety check diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index b49c9293631c43..ecac14890f68f9 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -28,8 +28,6 @@ AddrCheckStruct honda_bh_rx_checks[] = { const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]); int honda_brake = 0; -int honda_gas_prev = 0; -bool honda_brake_pressed_prev = false; bool honda_moving = false; bool honda_alt_brake_msg = false; bool honda_fwd_brake = false; @@ -48,7 +46,7 @@ static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { while (addr > 0U) { checksum += (addr & 0xFU); addr >>= 4; } - for (int j = 0; (j < len); j++) { + for (int j = 0; j < len; j++) { uint8_t byte = GET_BYTE(to_push, j); checksum += (byte & 0xFU) + (byte >> 4U); if (j == (len - 1)) { @@ -112,10 +110,10 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { 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 && (!(honda_brake_pressed_prev) || honda_moving)) { + if (brake_pressed && (!brake_pressed_prev || honda_moving)) { controls_allowed = 0; } - honda_brake_pressed_prev = brake_pressed; + brake_pressed_prev = brake_pressed; } // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6) @@ -133,11 +131,11 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if no interceptor if (!gas_interceptor_detected) { if (addr == 0x17C) { - int gas = GET_BYTE(to_push, 0); - if (gas && !honda_gas_prev) { + bool gas_pressed = GET_BYTE(to_push, 0) != 0; + if (gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } - honda_gas_prev = gas; + gas_pressed_prev = gas_pressed; } } if ((bus == 2) && (addr == 0x1FA)) { @@ -194,8 +192,8 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) || - (honda_brake_pressed_prev && honda_moving); + int pedal_pressed = gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) || + (brake_pressed_prev && honda_moving); bool current_controls_allowed = controls_allowed && !(pedal_pressed); // BRAKE: safety check diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 33a670f299b08e..1b5b656ff2fab8 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -1,15 +1,19 @@ const int HYUNDAI_MAX_STEER = 255; // like stock const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks -const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks +const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks const int HYUNDAI_MAX_RATE_UP = 3; const int HYUNDAI_MAX_RATE_DOWN = 7; const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50; const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2; +const int HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}}; // TODO: do checksum and counter checks AddrCheckStruct hyundai_rx_checks[] = { + {.addr = {608}, .bus = 0, .expected_timestep = 10000U}, {.addr = {897}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {902}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {916}, .bus = 0, .expected_timestep = 10000U}, {.addr = {1057}, .bus = 0, .expected_timestep = 20000U}, }; const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]); @@ -17,6 +21,7 @@ const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_c int hyundai_rt_torque_last = 0; int hyundai_desired_torque_last = 0; int hyundai_cruise_engaged_last = 0; +int hyundai_speed = 0; uint32_t hyundai_ts_last = 0; struct sample_t hyundai_torque_driver; // last few driver torques measured @@ -25,8 +30,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) { @@ -48,10 +52,33 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { hyundai_cruise_engaged_last = cruise_engaged; } - // TODO: check gas pressed + // exit controls on rising edge of gas press + if (addr == 608) { + bool gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0; + if (gas_pressed && !gas_pressed_prev) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + } + + // sample subaru wheel speed, averaging opposite corners + if (addr == 902) { + hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL + hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL + hyundai_speed /= 2; + } + + // 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 || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } // 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; } } diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h new file mode 100644 index 00000000000000..baa82b8d4aa319 --- /dev/null +++ b/board/safety/safety_nissan.h @@ -0,0 +1,213 @@ + +const uint32_t NISSAN_RT_INTERVAL = 250000; // 250ms between real time checks + +const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_UP = { + {2., 7., 17.}, + {5., .8, .15}}; + +const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = { + {2., 7., 17.}, + {5., 3.5, .5}}; + +const struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = { + {3.3, 12, 32}, + {540., 120., 23.}}; + +const int NISSAN_DEG_TO_CAN = 100; + +const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}}; + +AddrCheckStruct nissan_rx_checks[] = { + {.addr = {0x2}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {0x29a}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {0x1b6}, .bus = 1, .expected_timestep = 10000U}, +}; +const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]); + +float nissan_speed = 0; +//int nissan_controls_allowed_last = 0; +uint32_t nissan_ts_angle_last = 0; +int nissan_cruise_engaged_last = 0; +int nissan_desired_angle_last = 0; + +struct sample_t nissan_angle_meas; // last 3 steer angles + + +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); + + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + if (bus == 0) { + if (addr == 0x2) { + // Current steering angle + // Factor -0.1, little endian + int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFF); + // Need to multiply by 10 here as LKAS and Steering wheel are different base unit + angle_meas_new = to_signed(angle_meas_new, 16) * 10; + + // update array of samples + update_sample(&nissan_angle_meas, angle_meas_new); + } + + if (addr == 0x29a) { + // Get current speed + // Factor 0.00555 + nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6; + } + + // exit controls on rising edge of gas press + if (addr == 0x15c) { + bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)); + if (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 = true; + } + } + + if (bus == 1) { + if (addr == 0x1b6) { + int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1; + if (cruise_engaged && !nissan_cruise_engaged_last) { + controls_allowed = 1; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + nissan_cruise_engaged_last = cruise_engaged; + } + + // exit controls on rising edge of brake press, or if speed > 0 and brake + if (addr == 0x454) { + bool brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0; + if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } + } + } + return valid; +} + + +static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + int tx = 1; + int addr = GET_ADDR(to_send); + int bus = GET_BUS(to_send); + bool violation = 0; + + if (!msg_allowed(addr, bus, NISSAN_TX_MSGS, sizeof(NISSAN_TX_MSGS) / sizeof(NISSAN_TX_MSGS[0]))) { + 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)); + bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1; + + // offeset 1310 * NISSAN_DEG_TO_CAN + desired_angle = desired_angle - 131000; + + if (controls_allowed && lka_active) { + // add 1 to not false trigger the violation + float delta_angle_float; + delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.; + int delta_angle_up = (int)(delta_angle_float); + delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.; + int delta_angle_down = (int)(delta_angle_float); + int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down); + int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); + + // Limit maximum steering angle at current speed + int maximum_angle = ((int)interpolate(NISSAN_LOOKUP_MAX_ANGLE, nissan_speed)); + + if (highest_desired_angle > (maximum_angle * NISSAN_DEG_TO_CAN)) { + highest_desired_angle = (maximum_angle * NISSAN_DEG_TO_CAN); + } + if (lowest_desired_angle < (-maximum_angle * NISSAN_DEG_TO_CAN)) { + lowest_desired_angle = (-maximum_angle * NISSAN_DEG_TO_CAN); + } + + // check for violation; + violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); + + //nissan_controls_allowed_last = controls_allowed; + } + nissan_desired_angle_last = desired_angle; + + // desired steer angle should be the same as steer angle measured when controls are off + if ((!controls_allowed) && + ((desired_angle < (nissan_angle_meas.min - 1)) || + (desired_angle > (nissan_angle_meas.max + 1)))) { + violation = 1; + } + + // no lka_enabled bit if controls not allowed + if (!controls_allowed && lka_active) { + violation = 1; + } + } + + // acc button check, only allow cancel button to be sent + if (addr == 0x20b) { + // Violation of any button other than cancel is pressed + violation |= ((GET_BYTE(to_send, 1) & 0x3d) > 0); + } + + if (violation) { + controls_allowed = 0; + tx = 0; + } + + return tx; +} + + +static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + int bus_fwd = -1; + int addr = GET_ADDR(to_fwd); + + if (bus_num == 0) { + bus_fwd = 2; // ADAS + } + + if (bus_num == 2) { + // 0x169 is LKAS, 0x2b1 LKAS_HUD, 0x4cc LKAS_HUD_INFO_MSG + int block_msg = ((addr == 0x169) || (addr == 0x2b1) || (addr == 0x4cc)); + if (!block_msg) { + bus_fwd = 0; // V-CAN + } + } + + if (relay_malfunction) { + bus_fwd = -1; + } + + // fallback to do not forward + return bus_fwd; +} + +const safety_hooks nissan_hooks = { + .init = nooutput_init, + .rx = nissan_rx_hook, + .tx = nissan_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .fwd = nissan_fwd_hook, + .addr_check = nissan_rx_checks, + .addr_check_len = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]), +}; diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 885e2731d523c2..9b7665f2bcbe36 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -7,42 +7,84 @@ const int SUBARU_MAX_RATE_UP = 50; const int SUBARU_MAX_RATE_DOWN = 70; const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60; const int SUBARU_DRIVER_TORQUE_FACTOR = 10; +const int SUBARU_STANDSTILL_THRSLD = 20; // about 1kph -const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x164, 0}, {0x221, 0}, {0x322, 0}}; +const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x221, 0}, {0x322, 0}}; +const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}}; +const int SUBARU_TX_MSGS_LEN = sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]); +const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_MSGS[0]); -// TODO: do checksum and counter checks after adding the signals to the outback dbc file AddrCheckStruct subaru_rx_checks[] = { - {.addr = {0x119, 0x371}, .bus = 0, .expected_timestep = 20000U}, - {.addr = {0x240, 0x144}, .bus = 0, .expected_timestep = 50000U}, + {.addr = { 0x40}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, + {.addr = {0x119}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {0x139}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {0x13a}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {0x240}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, +}; +// TODO: do checksum and counter checks after adding the signals to the outback dbc file +AddrCheckStruct subaru_l_rx_checks[] = { + {.addr = {0x140}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {0x371}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {0x144}, .bus = 0, .expected_timestep = 50000U}, }; const int SUBARU_RX_CHECK_LEN = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]); +const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]); int subaru_cruise_engaged_last = 0; int subaru_rt_torque_last = 0; int subaru_desired_torque_last = 0; +int subaru_speed = 0; uint32_t subaru_ts_last = 0; +bool subaru_global = false; struct sample_t subaru_torque_driver; // last few driver torques measured +static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + return (uint8_t)GET_BYTE(to_push, 0); +} + +static uint8_t subaru_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + return (uint8_t)(GET_BYTE(to_push, 1) & 0xF); +} + +static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *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); + for (int i = 1; i < len; i++) { + checksum += (uint8_t)GET_BYTE(to_push, i); + } + return checksum; +} + static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - bool valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN, - NULL, NULL, NULL); + bool valid = false; + if (subaru_global) { + valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN, + subaru_get_checksum, subaru_compute_checksum, subaru_get_counter); + } else { + valid = addr_safety_check(to_push, subaru_l_rx_checks, SUBARU_L_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 == 0x119) || (addr == 0x371)) && (bus == 0)){ - int bit_shift = (addr == 0x119) ? 16 : 29; - int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF); + 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 array of samples update_sample(&subaru_torque_driver, torque_driver_new); } // enter controls on rising edge of ACC, exit controls on ACC off - if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) { - int bit_shift = (addr == 0x240) ? 9 : 17; + 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; @@ -53,9 +95,35 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { subaru_cruise_engaged_last = cruise_engaged; } - // TODO: enforce cancellation on gas pressed + // sample subaru wheel speed, averaging opposite corners + if ((addr == 0x13a) && subaru_global) { + subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR + subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF; // RL + subaru_speed /= 2; + } + + // exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models) + if ((addr == 0x139) && subaru_global) { + bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0; + if (brake_pressed && (!brake_pressed_prev || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } + + // 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_pressed = GET_BYTE(to_push, byte) != 0; + if (gas_pressed && !gas_pressed_prev) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + } - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) { + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && + (((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) { relay_malfunction = true; } } @@ -67,7 +135,8 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); - if (!msg_allowed(addr, bus, SUBARU_TX_MSGS, sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))) { + if ((!msg_allowed(addr, bus, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN) && subaru_global) || + (!msg_allowed(addr, bus, SUBARU_L_TX_MSGS, SUBARU_L_TX_MSGS_LEN) && !subaru_global)) { tx = 0; } @@ -76,8 +145,9 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } // steer cmd checks - if ((addr == 0x122) || (addr == 0x164)) { - int bit_shift = (addr == 0x122) ? 16 : 8; + if (((addr == 0x122) && subaru_global) || + ((addr == 0x164) && !subaru_global)) { + int bit_shift = subaru_global ? 16 : 8; int desired_torque = ((GET_BYTES_04(to_send) >> bit_shift) & 0x1FFF); bool violation = 0; uint32_t ts = TIM2->CNT; @@ -141,7 +211,9 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { // 545 is ES_Distance // 802 is ES_LKAS int addr = GET_ADDR(to_fwd); - int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802); + int block_msg = ((addr == 0x122) && subaru_global) || + ((addr == 0x164) && !subaru_global) || + (addr == 0x221) || (addr == 0x322); if (!block_msg) { bus_fwd = 0; // Main CAN } @@ -151,8 +223,22 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return bus_fwd; } +static void subaru_init(int16_t param) { + UNUSED(param); + controls_allowed = false; + relay_malfunction = false; + subaru_global = true; +} + +static void subaru_legacy_init(int16_t param) { + UNUSED(param); + controls_allowed = false; + relay_malfunction = false; + subaru_global = false; +} + const safety_hooks subaru_hooks = { - .init = nooutput_init, + .init = subaru_init, .rx = subaru_rx_hook, .tx = subaru_tx_hook, .tx_lin = nooutput_tx_lin_hook, @@ -160,3 +246,13 @@ const safety_hooks subaru_hooks = { .addr_check = subaru_rx_checks, .addr_check_len = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]), }; + +const safety_hooks subaru_legacy_hooks = { + .init = subaru_legacy_init, + .rx = subaru_rx_hook, + .tx = subaru_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .fwd = subaru_fwd_hook, + .addr_check = subaru_l_rx_checks, + .addr_check_len = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]), +}; diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index fbb464dcde1cc0..78b80e3bc535b6 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -34,8 +34,6 @@ float tesla_ts_angle_last = 0; int tesla_controls_allowed_last = 0; -int tesla_brake_prev = 0; -int tesla_gas_prev = 0; int tesla_speed = 0; int eac_status = 0; diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 4bf0a5de43d067..4f18ee678f1f20 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -16,7 +16,8 @@ const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2 const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 -const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file +const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph +const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 475; // ratio between offset and gain from dbc file const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}, {0x344, 0}, {0x365, 0}, {0x366, 0}, {0x4CB, 0}, // DSU bus 0 {0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1}, // DSU bus 1 @@ -24,8 +25,10 @@ const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0} {0x200, 0}, {0x750, 0}}; // interceptor + Blindspot monitor AddrCheckStruct toyota_rx_checks[] = { - {.addr = {0x260}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 20000U}, - {.addr = {0x1D2}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 30000U}, + {.addr = { 0xaa}, .bus = 0, .check_checksum = false, .expected_timestep = 12000U}, + {.addr = {0x260}, .bus = 0, .check_checksum = true, .expected_timestep = 20000U}, + {.addr = {0x1D2}, .bus = 0, .check_checksum = true, .expected_timestep = 30000U}, + {.addr = {0x224, 0x226}, .bus = 0, .check_checksum = false, .expected_timestep = 25000U}, }; const int TOYOTA_RX_CHECKS_LEN = sizeof(toyota_rx_checks) / sizeof(toyota_rx_checks[0]); @@ -37,7 +40,7 @@ int toyota_desired_torque_last = 0; // last desired steer torque int toyota_rt_torque_last = 0; // last desired torque for real time check uint32_t toyota_ts_last = 0; int toyota_cruise_engaged_last = 0; // cruise state -int toyota_gas_prev = 0; +bool toyota_moving = false; struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps @@ -60,8 +63,7 @@ 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) @@ -93,12 +95,34 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { toyota_cruise_engaged_last = cruise_engaged; } + // sample speed + if (addr == 0xaa) { + int speed = 0; + // sum 4 wheel speeds + for (int i=0; i<8; i+=2) { + int next_byte = i + 1; // hack to deal with misra 10.8 + speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte) - 0x1a6f; + } + toyota_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 || toyota_moving)) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } + // exit controls on rising edge of interceptor gas press if (addr == 0x201) { gas_interceptor_detected = 1; int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && - (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) { + if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && + (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) { controls_allowed = 0; } gas_interceptor_prev = gas_interceptor; @@ -106,15 +130,15 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 0x2C1) { - int gas = GET_BYTE(to_push, 6) & 0xFF; - if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) { + bool gas_pressed = GET_BYTE(to_push, 6) != 0; + if (gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) { controls_allowed = 0; } - toyota_gas_prev = gas; + gas_pressed_prev = gas_pressed; } // 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; } } diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h deleted file mode 100644 index e9515658cdded0..00000000000000 --- a/board/safety/safety_toyota_ipas.h +++ /dev/null @@ -1,169 +0,0 @@ -// uses tons from safety_toyota -// TODO: refactor to repeat less code - -// IPAS override -const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value - -// 2m/s are added to be less restrictive -const struct lookup_t LOOKUP_ANGLE_RATE_UP = { - {2., 7., 17.}, - {5., .8, .15}}; - -const struct lookup_t LOOKUP_ANGLE_RATE_DOWN = { - {2., 7., 17.}, - {5., 3.5, .4}}; - -const float RT_ANGLE_FUDGE = 1.5; // for RT checks allow 50% more angle change -const float CAN_TO_DEG = 2. / 3.; // convert angles from CAN unit to degrees - -int ipas_state = 1; // 1 disabled, 3 executing angle control, 5 override -int angle_control = 0; // 1 if direct angle control packets are seen -float speed = 0.; - -struct sample_t angle_meas; // last 3 steer angles -struct sample_t torque_driver; // last 3 driver steering torque - -// state of angle limits -int16_t desired_angle_last = 0; // last desired steer angle -int16_t rt_angle_last = 0; // last desired torque for real time check -uint32_t ts_angle_last = 0; - -int controls_allowed_last = 0; - - -static int toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - // check standard toyota stuff as well - bool valid = toyota_rx_hook(to_push); - - int addr = GET_ADDR(to_push); - - if (addr == 0x260) { - // get driver steering torque - int16_t torque_driver_new = (GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 2); - - // update array of samples - update_sample(&torque_driver, torque_driver_new); - } - - // get steer angle - if (addr == 0x25) { - int angle_meas_new = ((GET_BYTE(to_push, 0) & 0xF) << 8) | GET_BYTE(to_push, 1); - uint32_t ts = TIM2->CNT; - - angle_meas_new = to_signed(angle_meas_new, 12); - - // update array of samples - update_sample(&angle_meas, angle_meas_new); - - // *** angle real time check - // add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz - int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG) + 1.))); - int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * ((interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG) + 1.))); - int highest_rt_angle = rt_angle_last + ((rt_angle_last > 0) ? rt_delta_angle_up : rt_delta_angle_down); - int lowest_rt_angle = rt_angle_last - ((rt_angle_last > 0) ? rt_delta_angle_down : rt_delta_angle_up); - - // every RT_INTERVAL or when controls are turned on, set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); - if ((ts_elapsed > TOYOTA_RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) { - rt_angle_last = angle_meas_new; - ts_angle_last = ts; - } - - // check for violation - if (angle_control && - ((angle_meas_new < lowest_rt_angle) || - (angle_meas_new > highest_rt_angle))) { - controls_allowed = 0; - } - - controls_allowed_last = controls_allowed; - } - - // get speed - if (addr == 0xb4) { - speed = ((float)((GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6))) * 0.01 / 3.6; - } - - // get ipas state - if (addr == 0x262) { - ipas_state = GET_BYTE(to_push, 0) & 0xf; - } - - // exit controls on high steering override - if (angle_control && ((torque_driver.min > TOYOTA_IPAS_OVERRIDE_THRESHOLD) || - (torque_driver.max < -TOYOTA_IPAS_OVERRIDE_THRESHOLD) || - (ipas_state==5))) { - controls_allowed = 0; - } - return valid; -} - -static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - - int tx = 1; - int bypass_standard_tx_hook = 0; - int bus = GET_BUS(to_send); - int addr = GET_ADDR(to_send); - - // Check if msg is sent on BUS 0 - if (bus == 0) { - - // STEER ANGLE - if ((addr == 0x266) || (addr == 0x167)) { - - angle_control = 1; // we are in angle control mode - int desired_angle = ((GET_BYTE(to_send, 0) & 0xF) << 8) | GET_BYTE(to_send, 1); - int ipas_state_cmd = GET_BYTE(to_send, 0) >> 4; - bool violation = 0; - - desired_angle = to_signed(desired_angle, 12); - - if (controls_allowed) { - // add 1 to not false trigger the violation - float delta_angle_float; - delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG) + 1.; - int delta_angle_up = (int) (delta_angle_float); - delta_angle_float = (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG) + 1.; - int delta_angle_down = (int) (delta_angle_float); - - int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down); - int lowest_desired_angle = desired_angle_last - ((desired_angle_last > 0) ? delta_angle_down : delta_angle_up); - if ((desired_angle > highest_desired_angle) || - (desired_angle < lowest_desired_angle)){ - violation = 1; - controls_allowed = 0; - } - } - - // desired steer angle should be the same as steer angle measured when controls are off - if ((!controls_allowed) && - ((desired_angle < (angle_meas.min - 1)) || - (desired_angle > (angle_meas.max + 1)) || - (ipas_state_cmd != 1))) { - violation = 1; - } - - desired_angle_last = desired_angle; - - if (violation) { - tx = 0; - } - bypass_standard_tx_hook = 1; - } - } - - // check standard toyota stuff as well if addr isn't IPAS related - if (!bypass_standard_tx_hook) { - tx &= toyota_tx_hook(to_send); - } - - return tx; -} - -const safety_hooks toyota_ipas_hooks = { - .init = toyota_init, - .rx = toyota_ipas_rx_hook, - .tx = toyota_ipas_tx_hook, - .tx_lin = nooutput_tx_lin_hook, - .fwd = toyota_fwd_hook, -}; diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index 102cb22b57b04f..132c10d110fd96 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -1,142 +1,225 @@ -// Safety-relevant CAN messages for the Volkswagen MQB platform. -#define MSG_EPS_01 0x09F -#define MSG_MOTOR_20 0x121 -#define MSG_ACC_06 0x122 -#define MSG_HCA_01 0x126 -#define MSG_GRA_ACC_01 0x12B -#define MSG_LDW_02 0x397 - -const int VOLKSWAGEN_MAX_STEER = 250; // 2.5 Nm (EPS side max of 3.0Nm with fault if violated) +// Safety-relevant steering constants for Volkswagen +const int VOLKSWAGEN_MAX_STEER = 300; // 3.0 Nm (EPS side max of 3.0Nm with fault if violated) const int VOLKSWAGEN_MAX_RT_DELTA = 75; // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75 const uint32_t VOLKSWAGEN_RT_INTERVAL = 250000; // 250ms between real time checks -const int VOLKSWAGEN_MAX_RATE_UP = 4; // 2.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s) -const int VOLKSWAGEN_MAX_RATE_DOWN = 10; // 5.0 Nm/s available rate of change from the steering rack (EPS side delta-limit of 5.0 Nm/s) +const int VOLKSWAGEN_MAX_RATE_UP = 4; // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) +const int VOLKSWAGEN_MAX_RATE_DOWN = 10; // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s) const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80; const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3; -// MSG_GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration -const AddrBus VOLKSWAGEN_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}}; - -// TODO: do checksum and counter checks -AddrCheckStruct volkswagen_rx_checks[] = { - {.addr = {MSG_EPS_01}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {MSG_ACC_06}, .bus = 0, .expected_timestep = 20000U}, - {.addr = {MSG_MOTOR_20}, .bus = 0, .expected_timestep = 20000U}, +// Safety-relevant CAN messages for the Volkswagen MQB platform +#define MSG_ESP_19 0x0B2 // RX from ABS, for wheel speeds +#define MSG_EPS_01 0x09F // RX from EPS, for driver steering torque +#define MSG_ESP_05 0x106 // RX from ABS, for brake switch state +#define MSG_TSK_06 0x120 // RX from ECU, for ACC status from drivetrain coordinator +#define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input +#define MSG_HCA_01 0x126 // TX by OP, Heading Control Assist steering torque +#define MSG_GRA_ACC_01 0x12B // TX by OP, ACC control buttons for cancel/resume +#define MSG_LDW_02 0x397 // TX by OP, Lane line recognition and text alerts + +// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration +const AddrBus VOLKSWAGEN_MQB_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}}; +const int VOLKSWAGEN_MQB_TX_MSGS_LEN = sizeof(VOLKSWAGEN_MQB_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_TX_MSGS[0]); + +AddrCheckStruct volkswagen_mqb_rx_checks[] = { + {.addr = {MSG_ESP_19}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, + {.addr = {MSG_EPS_01}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, + {.addr = {MSG_ESP_05}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {MSG_TSK_06}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {MSG_MOTOR_20}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, }; +const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]); -const int VOLKSWAGEN_RX_CHECK_LEN = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]); -struct sample_t volkswagen_torque_driver; // last few driver torques measured +struct sample_t volkswagen_torque_driver; // Last few driver torques measured int volkswagen_rt_torque_last = 0; int volkswagen_desired_torque_last = 0; uint32_t volkswagen_ts_last = 0; -int volkswagen_gas_prev = 0; - -static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - - bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN, - NULL, NULL, NULL); - - if (valid) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - - // Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ - // for the direction. - if ((bus == 0) && (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) { - torque_driver_new *= -1; - } - - update_sample(&volkswagen_torque_driver, torque_driver_new); - } - - // Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control - // allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in - // order to accommodate future camera-side integrations if needed. - if (addr == MSG_ACC_06) { - int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4; - controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0; - } - - // exit controls on rising edge of gas press. Bits [12-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; - } - volkswagen_gas_prev = gas; - } - - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) { - relay_malfunction = true; - } - } - return valid; +bool volkswagen_moving = false; +int volkswagen_torque_msg = 0; +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) { + return (uint8_t)GET_BYTE(to_push, 0); } -static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int addr = GET_ADDR(to_send); - int bus = GET_BUS(to_send); - int tx = 1; +static uint8_t volkswagen_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + return (uint8_t)GET_BYTE(to_push, 1) & 0xFU; +} - if (!msg_allowed(addr, bus, VOLKSWAGEN_TX_MSGS, sizeof(VOLKSWAGEN_TX_MSGS)/sizeof(VOLKSWAGEN_TX_MSGS[0]))) { - tx = 0; +static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) { + int addr = GET_ADDR(to_push); + int len = GET_LEN(to_push); + + // This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation + // of this algorithm for a version with explanatory comments. + + uint8_t crc = 0xFFU; + for (int i = 1; i < len; i++) { + crc ^= (uint8_t)GET_BYTE(to_push, i); + crc = volkswagen_crc8_lut_8h2f[crc]; } - if (relay_malfunction) { - tx = 0; + uint8_t counter = volkswagen_get_counter(to_push); + switch(addr) { + case MSG_EPS_01: + crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter]; + break; + case MSG_ESP_05: + crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter]; + break; + case MSG_TSK_06: + crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter]; + break; + case MSG_MOTOR_20: + crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter]; + break; + default: // Undefined CAN message, CRC check expected to fail + break; } + crc = volkswagen_crc8_lut_8h2f[crc]; - // Safety check for HCA_01 Heading Control Assist torque. - if (addr == MSG_HCA_01) { - bool violation = false; + return crc ^ 0xFFU; +} - int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3F) << 8); - int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7; - if (sign == 1) { - desired_torque *= -1; - } +static void volkswagen_mqb_init(int16_t param) { + UNUSED(param); - uint32_t ts = TIM2->CNT; + controls_allowed = false; + relay_malfunction = false; + volkswagen_torque_msg = MSG_HCA_01; + volkswagen_lane_msg = MSG_LDW_02; + gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f); +} - if (controls_allowed) { +static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - // *** global torque limit check *** - violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER); + 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); - // *** torque rate limit check *** - violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver, - VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN, - VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR); - volkswagen_desired_torque_last = desired_torque; + if (valid && (GET_BUS(to_push) == 0)) { + int addr = GET_ADDR(to_push); - // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA); + // 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 (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 + // DBC speed scale 0.0075: 0.3m/s = 144, sum both wheels to compare + volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 288; + } - // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last); - if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) { - volkswagen_rt_torque_last = desired_torque; - volkswagen_ts_last = ts; + // Update driver input torque samples + // Signal: EPS_01.Driver_Strain (absolute torque) + // Signal: EPS_01.Driver_Strain_VZ (direction) + 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) { + torque_driver_new *= -1; } + update_sample(&volkswagen_torque_driver, torque_driver_new); } - // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { - violation = true; + // Update ACC status from drivetrain coordinator for controls-allowed state + // Signal: TSK_06.TSK_Status + 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; } - // reset to 0 if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { - volkswagen_desired_torque_last = 0; - volkswagen_rt_torque_last = 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) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + } + + // 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 || volkswagen_moving)) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } + + // 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 = true; + } + } + return valid; +} + +static bool volkswagen_steering_check(int desired_torque) { + bool violation = false; + uint32_t ts = TIM2->CNT; + + if (controls_allowed) { + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER); + + // *** torque rate limit check *** + violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver, + VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN, + VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR); + volkswagen_desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA); + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last); + if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) { + volkswagen_rt_torque_last = desired_torque; volkswagen_ts_last = ts; } + } - if (violation) { + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = true; + } + + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !controls_allowed) { + volkswagen_desired_torque_last = 0; + volkswagen_rt_torque_last = 0; + volkswagen_ts_last = ts; + } + + return violation; +} + +static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + int addr = GET_ADDR(to_send); + int bus = GET_BUS(to_send); + int tx = 1; + + if (!msg_allowed(addr, bus, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN) || relay_malfunction) { + tx = 0; + } + + // Safety check for HCA_01 Heading Control Assist torque + // Signal: HCA_01.Assist_Torque (absolute torque) + // Signal: HCA_01.Assist_VZ (direction) + if (addr == MSG_HCA_01) { + int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x3F) << 8); + int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7; + if (sign == 1) { + desired_torque *= -1; + } + + if (volkswagen_steering_check(desired_torque)) { tx = 0; } } @@ -158,25 +241,23 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = GET_ADDR(to_fwd); int bus_fwd = -1; - // NOTE: Will need refactoring for other bus layouts, such as no-forwarding at camera or J533 running-gear CAN - if (!relay_malfunction) { switch (bus_num) { case 0: - // Forward all traffic from J533 gateway to Extended CAN devices. + // Forward all traffic from the Extended CAN onward bus_fwd = 2; break; case 2: - if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) { - // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera. + 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. + // 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. + // No other buses should be in use; fallback to do-not-forward bus_fwd = -1; break; } @@ -184,12 +265,13 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return bus_fwd; } -const safety_hooks volkswagen_hooks = { - .init = nooutput_init, - .rx = volkswagen_rx_hook, - .tx = volkswagen_tx_hook, +// Volkswagen MQB platform +const safety_hooks volkswagen_mqb_hooks = { + .init = volkswagen_mqb_init, + .rx = volkswagen_mqb_rx_hook, + .tx = volkswagen_mqb_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = volkswagen_fwd_hook, - .addr_check = volkswagen_rx_checks, - .addr_check_len = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]), + .addr_check = volkswagen_mqb_rx_checks, + .addr_check_len = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]), }; diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 1a6a8dd21a16f9..aa9f7fdfe43704 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -49,6 +49,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, const int MAX_ALLOWANCE, const int DRIVER_FACTOR); 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(int addr, int bus, const AddrBus addr_list[], int len); int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len); void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter); @@ -84,6 +85,8 @@ bool controls_allowed = false; bool relay_malfunction = false; bool gas_interceptor_detected = false; int gas_interceptor_prev = 0; +bool gas_pressed_prev = false; +bool brake_pressed_prev = false; // time since safety mode has been changed uint32_t safety_mode_cnt = 0U; diff --git a/docs/panda_wifi_setup.md b/docs/panda_wifi_setup.md new file mode 100644 index 00000000000000..a6dfaf1abae41a --- /dev/null +++ b/docs/panda_wifi_setup.md @@ -0,0 +1,16 @@ +# Connecting to White Panda via Wi-Fi + +1. First connect to your White Panda's Wi-Fi pairing network (this should be the Wi-Fi network WITH the "-pair" at the end) + +2. Now in your favorite web browser go to this address **192.168.0.10** (this should open a web interface to interact with the White Panda) + +3. Inside the web interface enable secured mode by clinking the **secure it** link/button (this should make the White Panda's Wi-Fi network visible) + + ### If you need your White Panda's Wi-Fi Password + + * Run the **get_panda_password.py** script in found in **examples/** (Must have panda paw for this step because you need to connect White Panda via USB to retrive the Wi-Fi password) + * Also ensure that you are connected to your White Panda's Wi-Fi pairing network + +4. Connect to your White Panda's default Wi-Fi network (this should be the Wi-Fi network WITHOUT the "-pair" at the end) + +5. Your White Panda is now connected to Wi-Fi you can test this by running this line of code `python -c 'from panda import Panda; panda = Panda("WIFI")'` in your terminal of choice. \ No newline at end of file diff --git a/python/__init__.py b/python/__init__.py index ea8dea13d93874..c0e27e8d6a775e 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -123,12 +123,13 @@ class Panda(object): SAFETY_TESLA = 10 SAFETY_SUBARU = 11 SAFETY_MAZDA = 13 - SAFETY_VOLKSWAGEN = 15 - SAFETY_TOYOTA_IPAS = 16 + SAFETY_NISSAN = 14 + SAFETY_VOLKSWAGEN_MQB = 15 SAFETY_ALLOUTPUT = 17 SAFETY_GM_ASCM = 18 SAFETY_NOOUTPUT = 19 SAFETY_HONDA_BOSCH_HARNESS = 20 + SAFETY_SUBARU_LEGACY = 22 SERIAL_DEBUG = 0 SERIAL_ESP = 1 diff --git a/requirements.txt b/requirements.txt index 5ceb3cf6e7d6cf..fb01edcedb9107 100644 --- a/requirements.txt +++ b/requirements.txt @@ -9,3 +9,4 @@ requests flake8==3.7.9 pylint==2.4.3 cffi==1.11.4 +crcmod diff --git a/tests/safety/common.py b/tests/safety/common.py index 63408ce430ed50..e0296d3ac11d54 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -13,26 +13,58 @@ def make_msg(bus, addr, length=8): return to_send -def test_relay_malfunction(test, addr): - # input is a test class and the address that, if seen on bus 0, triggers - # the relay_malfunction protection logic: both tx_hook and fwd_hook are - # expected to return failure - test.assertFalse(test.safety.get_relay_malfunction()) - test.safety.safety_rx_hook(make_msg(0, addr, 8)) - test.assertTrue(test.safety.get_relay_malfunction()) - for a in range(1, 0x800): - for b in range(0, 3): - test.assertFalse(test.safety.safety_tx_hook(make_msg(b, a, 8))) - test.assertEqual(-1, test.safety.safety_fwd_hook(b, make_msg(b, a, 8))) - -def test_manually_enable_controls_allowed(test): - test.safety.set_controls_allowed(1) - test.assertTrue(test.safety.get_controls_allowed()) - test.safety.set_controls_allowed(0) - test.assertFalse(test.safety.get_controls_allowed()) - -def test_spam_can_buses(test, TX_MSGS): - for addr in range(1, 0x800): - for bus in range(0, 4): - if all(addr != m[0] or bus != m[1] for m in TX_MSGS): - test.assertFalse(test.safety.safety_tx_hook(make_msg(bus, addr, 8))) +class StdTest: + @staticmethod + def test_relay_malfunction(test, addr, bus=0): + # input is a test class and the address that, if seen on specified bus, triggers + # the relay_malfunction protection logic: both tx_hook and fwd_hook are + # expected to return failure + test.assertFalse(test.safety.get_relay_malfunction()) + test.safety.safety_rx_hook(make_msg(bus, addr, 8)) + test.assertTrue(test.safety.get_relay_malfunction()) + for a in range(1, 0x800): + for b in range(0, 3): + test.assertFalse(test.safety.safety_tx_hook(make_msg(b, a, 8))) + test.assertEqual(-1, test.safety.safety_fwd_hook(b, make_msg(b, a, 8))) + + @staticmethod + def test_manually_enable_controls_allowed(test): + test.safety.set_controls_allowed(1) + test.assertTrue(test.safety.get_controls_allowed()) + test.safety.set_controls_allowed(0) + test.assertFalse(test.safety.get_controls_allowed()) + + @staticmethod + def test_spam_can_buses(test, TX_MSGS): + for addr in range(1, 0x800): + for bus in range(0, 4): + if all(addr != m[0] or bus != m[1] for m in TX_MSGS): + test.assertFalse(test.safety.safety_tx_hook(make_msg(bus, addr, 8))) + + @staticmethod + def test_allow_brake_at_zero_speed(test): + # Brake was already pressed + test.safety.safety_rx_hook(test._speed_msg(0)) + test.safety.safety_rx_hook(test._brake_msg(1)) + test.safety.set_controls_allowed(1) + test.safety.safety_rx_hook(test._brake_msg(1)) + test.assertTrue(test.safety.get_controls_allowed()) + test.safety.safety_rx_hook(test._brake_msg(0)) + test.assertTrue(test.safety.get_controls_allowed()) + # rising edge of brake should disengage + test.safety.safety_rx_hook(test._brake_msg(1)) + test.assertFalse(test.safety.get_controls_allowed()) + test.safety.safety_rx_hook(test._brake_msg(0)) # reset no brakes + + @staticmethod + def test_not_allow_brake_when_moving(test, standstill_threshold): + # Brake was already pressed + test.safety.safety_rx_hook(test._brake_msg(1)) + test.safety.set_controls_allowed(1) + test.safety.safety_rx_hook(test._speed_msg(standstill_threshold)) + test.safety.safety_rx_hook(test._brake_msg(1)) + test.assertTrue(test.safety.get_controls_allowed()) + test.safety.safety_rx_hook(test._speed_msg(standstill_threshold + 1)) + test.safety.safety_rx_hook(test._brake_msg(1)) + test.assertFalse(test.safety.get_controls_allowed()) + test.safety.safety_rx_hook(test._speed_msg(0)) diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 77131c1a3c8e53..3b552d0f80ed62 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -37,9 +37,10 @@ void set_gas_interceptor_detected(bool c); bool get_gas_interceptor_detetcted(void); int get_gas_interceptor_prev(void); +bool get_gas_pressed_prev(void); +bool get_brake_pressed_prev(void); int get_hw_type(void); void set_timer(uint32_t t); -void reset_angle_control(void); int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send); int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push); @@ -49,18 +50,14 @@ void init_tests_toyota(void); int get_toyota_torque_meas_min(void); int get_toyota_torque_meas_max(void); -int get_toyota_gas_prev(void); void set_toyota_torque_meas(int min, int max); void set_toyota_desired_torque_last(int t); void set_toyota_rt_torque_last(int t); void init_tests_honda(void); bool get_honda_moving(void); -bool get_honda_brake_pressed_prev(void); -int get_honda_gas_prev(void); void set_honda_fwd_brake(bool); void set_honda_alt_brake_msg(bool); -void set_honda_hw(int); int get_honda_hw(void); void init_tests_cadillac(void); @@ -88,13 +85,18 @@ void init_tests_subaru(void); void set_subaru_desired_torque_last(int t); void set_subaru_rt_torque_last(int t); -void set_subaru_torque_driver(int min, int max); +bool get_subaru_global(void); void init_tests_volkswagen(void); +int get_volkswagen_torque_driver_min(void); +int get_volkswagen_torque_driver_max(void); +bool get_volkswagen_moving(void); void set_volkswagen_desired_torque_last(int t); void set_volkswagen_rt_torque_last(int t); void set_volkswagen_torque_driver(int min, int max); -int get_volkswagen_gas_prev(void); + +void init_tests_nissan(void); +void set_nissan_desired_angle_last(int t); """) diff --git a/tests/safety/test.c b/tests/safety/test.c index 59611d3131c67e..9c7955b2869ce0 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -58,6 +58,10 @@ uint8_t hw_type = HW_TYPE_UNKNOWN; __typeof__ (b) _b = (b); \ _a > _b ? _a : _b; }) +#define ABS(a) \ + ({ __typeof__ (a) _a = (a); \ + (_a > 0) ? _a : (-_a); }) + // from llcan.h #define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF) #define GET_LEN(msg) ((msg)->RDTR & 0xf) @@ -85,10 +89,6 @@ void set_gas_interceptor_detected(bool c){ gas_interceptor_detected = c; } -void reset_angle_control(void){ - angle_control = 0; -} - bool get_controls_allowed(void){ return controls_allowed; } @@ -105,10 +105,22 @@ int get_gas_interceptor_prev(void){ return gas_interceptor_prev; } +bool get_gas_pressed_prev(void){ + return gas_pressed_prev; +} + +bool get_brake_pressed_prev(void){ + return brake_pressed_prev; +} + int get_hw_type(void){ return hw_type; } +bool get_subaru_global(void){ + return subaru_global; +} + void set_timer(uint32_t t){ timer.CNT = t; } @@ -138,16 +150,19 @@ void set_chrysler_torque_meas(int min, int max){ chrysler_torque_meas.max = max; } -void set_subaru_torque_driver(int min, int max){ - subaru_torque_driver.min = min; - subaru_torque_driver.max = max; -} - void set_volkswagen_torque_driver(int min, int max){ volkswagen_torque_driver.min = min; volkswagen_torque_driver.max = max; } +int get_volkswagen_torque_driver_min(void){ + return volkswagen_torque_driver.min; +} + +int get_volkswagen_torque_driver_max(void){ + return volkswagen_torque_driver.max; +} + int get_chrysler_torque_meas_min(void){ return chrysler_torque_meas.min; } @@ -156,10 +171,6 @@ int get_chrysler_torque_meas_max(void){ return chrysler_torque_meas.max; } -int get_toyota_gas_prev(void){ - return toyota_gas_prev; -} - int get_toyota_torque_meas_min(void){ return toyota_torque_meas.min; } @@ -224,30 +235,18 @@ void set_volkswagen_desired_torque_last(int t){ volkswagen_desired_torque_last = t; } -int get_volkswagen_gas_prev(void){ - return volkswagen_gas_prev; +int get_volkswagen_moving(void){ + return volkswagen_moving; } bool get_honda_moving(void){ return honda_moving; } -bool get_honda_brake_pressed_prev(void){ - return honda_brake_pressed_prev; -} - -int get_honda_gas_prev(void){ - return honda_gas_prev; -} - void set_honda_alt_brake_msg(bool c){ honda_alt_brake_msg = c; } -void set_honda_hw(int c){ - honda_hw = c; -} - int get_honda_hw(void) { return honda_hw; } @@ -256,10 +255,16 @@ void set_honda_fwd_brake(bool c){ honda_fwd_brake = c; } +void set_nissan_desired_angle_last(int t){ + nissan_desired_angle_last = t; +} + void init_tests(void){ // get HW_TYPE from env variable set in test.sh hw_type = atoi(getenv("HW_TYPE")); safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic + gas_pressed_prev = false; + brake_pressed_prev = false; } void init_tests_toyota(void){ @@ -304,6 +309,7 @@ void init_tests_hyundai(void){ void init_tests_chrysler(void){ init_tests(); + chrysler_speed = 0; chrysler_torque_meas.min = 0; chrysler_torque_meas.max = 0; chrysler_desired_torque_last = 0; @@ -324,6 +330,7 @@ void init_tests_subaru(void){ void init_tests_volkswagen(void){ init_tests(); + volkswagen_moving = false; volkswagen_torque_driver.min = 0; volkswagen_torque_driver.max = 0; volkswagen_desired_torque_last = 0; @@ -335,11 +342,17 @@ void init_tests_volkswagen(void){ void init_tests_honda(void){ init_tests(); honda_moving = false; - honda_brake_pressed_prev = false; - honda_gas_prev = 0; honda_fwd_brake = false; } +void init_tests_nissan(void){ + init_tests(); + nissan_angle_meas.min = 0; + nissan_angle_meas.max = 0; + nissan_desired_angle_last = 0; + set_timer(0); +} + void set_gmlan_digital_output(int to_set){ } diff --git a/tests/safety/test_cadillac.py b/tests/safety/test_cadillac.py index 7806fb7b285cc1..e4e75f15bdf4a9 100644 --- a/tests/safety/test_cadillac.py +++ b/tests/safety/test_cadillac.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import make_msg, test_manually_enable_controls_allowed, test_spam_can_buses +from panda.tests.safety.common import make_msg, StdTest MAX_RATE_UP = 2 @@ -56,13 +56,13 @@ def _torque_msg(self, torque): return to_send def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) + StdTest.test_spam_can_buses(self, TX_MSGS) def test_default_controls_not_allowed(self): self.assertFalse(self.safety.get_controls_allowed()) def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) + StdTest.test_manually_enable_controls_allowed(self) def test_enable_control_allowed_from_cruise(self): to_push = make_msg(0, 0x370) diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index 2c472706fe1c99..c66b90b6ba3eb0 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses +from panda.tests.safety.common import StdTest, make_msg MAX_RATE_UP = 3 MAX_RATE_DOWN = 3 @@ -16,7 +16,37 @@ TX_MSGS = [[571, 0], [658, 0], [678, 0]] +def chrysler_checksum(msg, len_msg): + checksum = 0xFF + for idx in range(0, len_msg-1): + curr = (msg.RDLR >> (8*idx)) if idx < 4 else (msg.RDHR >> (8*(idx - 4))) + curr &= 0xFF + shift = 0x80 + for i in range(0, 8): + bit_sum = curr & shift + temp_chk = checksum & 0x80 + if (bit_sum != 0): + bit_sum = 0x1C + if (temp_chk != 0): + bit_sum = 1 + checksum = checksum << 1 + temp_chk = checksum | 1 + bit_sum ^= temp_chk + else: + if (temp_chk != 0): + bit_sum = 0x1D + checksum = checksum << 1 + bit_sum ^= checksum + checksum = bit_sum + shift = shift >> 1 + return ~checksum & 0xFF + class TestChryslerSafety(unittest.TestCase): + cnt_torque_meas = 0 + cnt_gas = 0 + cnt_cruise = 0 + cnt_brake = 0 + @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety @@ -28,6 +58,36 @@ def _button_msg(self, buttons): to_send[0].RDLR = buttons return to_send + def _cruise_msg(self, active): + to_send = make_msg(0, 500) + to_send[0].RDLR = 0x380000 if active else 0 + to_send[0].RDHR |= (self.cnt_cruise % 16) << 20 + to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + self.__class__.cnt_cruise += 1 + return to_send + + def _speed_msg(self, speed): + speed = int(speed / 0.071028) + to_send = make_msg(0, 514, 4) + to_send[0].RDLR = ((speed & 0xFF0) >> 4) + ((speed & 0xF) << 12) + \ + ((speed & 0xFF0) << 12) + ((speed & 0xF) << 28) + return to_send + + def _gas_msg(self, gas): + to_send = make_msg(0, 308) + to_send[0].RDHR = (gas & 0x7F) << 8 + to_send[0].RDHR |= (self.cnt_gas % 16) << 20 + self.__class__.cnt_gas += 1 + return to_send + + def _brake_msg(self, brake): + to_send = make_msg(0, 320) + to_send[0].RDLR = 5 if brake else 0 + to_send[0].RDHR |= (self.cnt_brake % 16) << 20 + to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + self.__class__.cnt_brake += 1 + return to_send + def _set_prev_torque(self, t): self.safety.set_chrysler_desired_torque_last(t) self.safety.set_chrysler_rt_torque_last(t) @@ -36,6 +96,9 @@ def _set_prev_torque(self, t): def _torque_meas_msg(self, torque): to_send = make_msg(0, 544) to_send[0].RDHR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8) + to_send[0].RDHR |= (self.cnt_torque_meas % 16) << 20 + to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + self.__class__.cnt_torque_meas += 1 return to_send def _torque_msg(self, torque): @@ -44,10 +107,10 @@ def _torque_msg(self, torque): return to_send def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) + StdTest.test_spam_can_buses(self, TX_MSGS) def test_relay_malfunction(self): - test_relay_malfunction(self, 0x292) + StdTest.test_relay_malfunction(self, 0x292) def test_default_controls_not_allowed(self): self.assertFalse(self.safety.get_controls_allowed()) @@ -63,23 +126,33 @@ def test_steer_safety_check(self): self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) + StdTest.test_manually_enable_controls_allowed(self) def test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x1F4) - to_push[0].RDLR = 0x380000 - + to_push = self._cruise_msg(True) self.safety.safety_rx_hook(to_push) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x1F4) - to_push[0].RDLR = 0 - + to_push = self._cruise_msg(False) self.safety.set_controls_allowed(1) self.safety.safety_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed()) + def test_gas_disable(self): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._speed_msg(2.2)) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.safety_rx_hook(self._speed_msg(2.3)) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_brake_disengage(self): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, 0) + def test_non_realtime_limit_up(self): self.safety.set_controls_allowed(True) diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py index 06872dede5b1e4..34d92357c79d9f 100644 --- a/tests/safety/test_gm.py +++ b/tests/safety/test_gm.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses +from panda.tests.safety.common import StdTest, make_msg MAX_RATE_UP = 7 MAX_RATE_DOWN = 17 @@ -90,10 +90,10 @@ def _torque_msg(self, torque): return to_send def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) + StdTest.test_spam_can_buses(self, TX_MSGS) def test_relay_malfunction(self): - test_relay_malfunction(self, 384) + StdTest.test_relay_malfunction(self, 384) def test_default_controls_not_allowed(self): self.assertFalse(self.safety.get_controls_allowed()) @@ -116,29 +116,9 @@ def test_cancel_button(self): self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN)) self.assertFalse(self.safety.get_controls_allowed()) - def test_disengage_on_brake(self): - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_allow_brake_at_zero_speed(self): - # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) - self.safety.set_controls_allowed(1) - - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._brake_msg(False)) - - def test_not_allow_brake_when_moving(self): - # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) - self.safety.safety_rx_hook(self._speed_msg(100)) - self.safety.set_controls_allowed(1) - - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertFalse(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._brake_msg(False)) + def test_brake_disengage(self): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, 0) def test_disengage_on_gas(self): self.safety.set_controls_allowed(1) @@ -182,7 +162,7 @@ def test_steer_safety_check(self): self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) + StdTest.test_manually_enable_controls_allowed(self) def test_non_realtime_limit_up(self): self.safety.set_gm_torque_driver(0, 0) diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index 87f708930402a9..adf02fd5cc0dbb 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -3,14 +3,15 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, \ - test_manually_enable_controls_allowed, \ - test_spam_can_buses, MAX_WRONG_COUNTERS +from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS MAX_BRAKE = 255 INTERCEPTOR_THRESHOLD = 328 -TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]] +N_TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]] +BH_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness +BG_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe + HONDA_N_HW = 0 HONDA_BG_HW = 1 @@ -30,39 +31,41 @@ def honda_checksum(msg, addr, len_msg): class TestHondaSafety(unittest.TestCase): + cnt_speed = 0 + cnt_gas = 0 + cnt_button = 0 + @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, 0) cls.safety.init_tests_honda() - cls.cnt_speed = 0 - cls.cnt_gas = 0 - cls.cnt_button = 0 def _speed_msg(self, speed): - to_send = make_msg(0, 0x158) + bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 + to_send = make_msg(bus, 0x158) to_send[0].RDLR = speed to_send[0].RDHR |= (self.cnt_speed % 4) << 28 to_send[0].RDHR |= honda_checksum(to_send[0], 0x158, 8) << 24 - self.cnt_speed += 1 + self.__class__.cnt_speed += 1 return to_send def _button_msg(self, buttons, addr): - honda_hw = self.safety.get_honda_hw() - bus = 1 if honda_hw == HONDA_BH_HW else 0 + bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 to_send = make_msg(bus, addr) to_send[0].RDLR = buttons << 5 to_send[0].RDHR |= (self.cnt_button % 4) << 28 to_send[0].RDHR |= honda_checksum(to_send[0], addr, 8) << 24 - self.cnt_button += 1 + self.__class__.cnt_button += 1 return to_send def _brake_msg(self, brake): - to_send = make_msg(0, 0x17C) + bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 + to_send = make_msg(bus, 0x17C) to_send[0].RDHR = 0x200000 if brake else 0 to_send[0].RDHR |= (self.cnt_gas % 4) << 28 to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24 - self.cnt_gas += 1 + self.__class__.cnt_gas += 1 return to_send def _alt_brake_msg(self, brake): @@ -71,11 +74,12 @@ def _alt_brake_msg(self, brake): return to_send def _gas_msg(self, gas): - to_send = make_msg(0, 0x17C) + bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0 + to_send = make_msg(bus, 0x17C) to_send[0].RDLR = 1 if gas else 0 to_send[0].RDHR |= (self.cnt_gas % 4) << 28 to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24 - self.cnt_gas += 1 + self.__class__.cnt_gas += 1 return to_send def _send_brake_msg(self, brake): @@ -91,39 +95,48 @@ def _send_interceptor_msg(self, gas, addr): return to_send def _send_steer_msg(self, steer): - to_send = make_msg(0, 0xE4, 6) + bus = 2 if self.safety.get_honda_hw() == HONDA_BG_HW else 0 + to_send = make_msg(bus, 0xE4, 6) to_send[0].RDLR = steer return to_send def test_spam_can_buses(self): - self.safety.set_honda_hw(HONDA_N_HW) - test_spam_can_buses(self, TX_MSGS) + hw_type = self.safety.get_honda_hw() + if hw_type == HONDA_N_HW: + tx_msgs = N_TX_MSGS + elif hw_type == HONDA_BH_HW: + tx_msgs = BH_TX_MSGS + elif hw_type == HONDA_BG_HW: + tx_msgs = BG_TX_MSGS + StdTest.test_spam_can_buses(self, tx_msgs) def test_relay_malfunction(self): - test_relay_malfunction(self, 0xE4) + hw = self.safety.get_honda_hw() + bus = 2 if hw == HONDA_BG_HW else 0 + StdTest.test_relay_malfunction(self, 0xE4, bus=bus) def test_default_controls_not_allowed(self): self.assertFalse(self.safety.get_controls_allowed()) def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) + StdTest.test_manually_enable_controls_allowed(self) def test_resume_button(self): RESUME_BTN = 4 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x296)) self.assertTrue(self.safety.get_controls_allowed()) def test_set_button(self): SET_BTN = 3 self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) self.assertTrue(self.safety.get_controls_allowed()) def test_cancel_button(self): CANCEL_BTN = 2 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x296)) self.assertFalse(self.safety.get_controls_allowed()) def test_sample_speed(self): @@ -132,9 +145,9 @@ def test_sample_speed(self): self.assertEqual(1, self.safety.get_honda_moving()) def test_prev_brake(self): - self.assertFalse(self.safety.get_honda_brake_pressed_prev()) + self.assertFalse(self.safety.get_brake_pressed_prev()) self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertTrue(self.safety.get_honda_brake_pressed_prev()) + self.assertTrue(self.safety.get_brake_pressed_prev()) def test_disengage_on_brake(self): self.safety.set_controls_allowed(1) @@ -152,29 +165,15 @@ def test_alt_disengage_on_brake(self): self.safety.safety_rx_hook(self._alt_brake_msg(1)) self.assertTrue(self.safety.get_controls_allowed()) - def test_allow_brake_at_zero_speed(self): - # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) - self.safety.set_controls_allowed(1) - - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._brake_msg(False)) # reset no brakes - - def test_not_allow_brake_when_moving(self): - # Brake was already pressed - self.safety.safety_rx_hook(self._brake_msg(True)) - self.safety.safety_rx_hook(self._speed_msg(100)) - self.safety.set_controls_allowed(1) - - self.safety.safety_rx_hook(self._brake_msg(True)) - self.assertFalse(self.safety.get_controls_allowed()) + def test_brake_disengage(self): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, 0) def test_prev_gas(self): self.safety.safety_rx_hook(self._gas_msg(False)) - self.assertFalse(self.safety.get_honda_gas_prev()) + self.assertFalse(self.safety.get_gas_pressed_prev()) self.safety.safety_rx_hook(self._gas_msg(True)) - self.assertTrue(self.safety.get_honda_gas_prev()) + self.assertTrue(self.safety.get_gas_pressed_prev()) def test_prev_gas_interceptor(self): self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) @@ -215,29 +214,32 @@ def test_allow_engage_with_gas_interceptor_pressed(self): self.safety.set_gas_interceptor_detected(False) def test_brake_safety_check(self): - for fwd_brake in [False, True]: - self.safety.set_honda_fwd_brake(fwd_brake) - for brake in np.arange(0, MAX_BRAKE + 10, 1): + hw = self.safety.get_honda_hw() + if hw == HONDA_N_HW: + for fwd_brake in [False, True]: + self.safety.set_honda_fwd_brake(fwd_brake) + for brake in np.arange(0, MAX_BRAKE + 10, 1): + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + if fwd_brake: + send = False # block openpilot brake msg when fwd'ing stock msg + elif controls_allowed: + send = MAX_BRAKE >= brake >= 0 + else: + send = brake == 0 + self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake))) + self.safety.set_honda_fwd_brake(False) + + def test_gas_interceptor_safety_check(self): + if self.safety.get_honda_hw() == HONDA_N_HW: + for gas in np.arange(0, 4000, 100): for controls_allowed in [True, False]: self.safety.set_controls_allowed(controls_allowed) - if fwd_brake: - send = False # block openpilot brake msg when fwd'ing stock msg - elif controls_allowed: - send = MAX_BRAKE >= brake >= 0 + if controls_allowed: + send = True else: - send = brake == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake))) - self.safety.set_honda_fwd_brake(False) - - def test_gas_interceptor_safety_check(self): - for gas in np.arange(0, 4000, 100): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if controls_allowed: - send = True - else: - send = gas == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200))) + send = gas == 0 + self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200))) def test_steer_safety_check(self): self.safety.set_controls_allowed(0) @@ -245,12 +247,12 @@ def test_steer_safety_check(self): self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000))) def test_spam_cancel_safety_check(self): - RESUME_BTN = 4 - SET_BTN = 3 - CANCEL_BTN = 2 - BUTTON_MSG = 0x296 - for hw in [HONDA_BG_HW, HONDA_BH_HW]: - self.safety.set_honda_hw(hw) + hw = self.safety.get_honda_hw() + if hw != HONDA_N_HW: + RESUME_BTN = 4 + SET_BTN = 3 + CANCEL_BTN = 2 + BUTTON_MSG = 0x296 self.safety.set_controls_allowed(0) self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG))) self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) @@ -260,12 +262,16 @@ def test_spam_cancel_safety_check(self): self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) def test_rx_hook(self): + # checksum checks SET_BTN = 3 for msg in ["btn1", "btn2", "gas", "speed"]: self.safety.set_controls_allowed(1) if msg == "btn1": - to_push = self._button_msg(SET_BTN, 0x1A6) + if self.safety.get_honda_hw() == HONDA_N_HW: + to_push = self._button_msg(SET_BTN, 0x1A6) # only in Honda_NIDEC + else: + continue if msg == "btn2": to_push = self._button_msg(SET_BTN, 0x296) if msg == "gas": @@ -273,23 +279,23 @@ def test_rx_hook(self): if msg == "speed": to_push = self._speed_msg(0) self.assertTrue(self.safety.safety_rx_hook(to_push)) - to_push[0].RDHR = 0 + to_push[0].RDHR = 0 # invalidate checksum self.assertFalse(self.safety.safety_rx_hook(to_push)) self.assertFalse(self.safety.get_controls_allowed()) # counter # reset wrong_counters to zero by sending valid messages for i in range(MAX_WRONG_COUNTERS + 1): - self.cnt_speed = 0 - self.cnt_gas = 0 - self.cnt_button = 0 + self.__class__.cnt_speed += 1 + self.__class__.cnt_gas += 1 + self.__class__.cnt_button += 1 if i < MAX_WRONG_COUNTERS: self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) self.safety.safety_rx_hook(self._speed_msg(0)) self.safety.safety_rx_hook(self._gas_msg(0)) else: - self.assertFalse(self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))) + self.assertFalse(self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))) self.assertFalse(self.safety.safety_rx_hook(self._speed_msg(0))) self.assertFalse(self.safety.safety_rx_hook(self._gas_msg(0))) self.assertFalse(self.safety.get_controls_allowed()) @@ -297,10 +303,10 @@ def test_rx_hook(self): # restore counters for future tests with a couple of good messages for i in range(2): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) self.safety.safety_rx_hook(self._speed_msg(0)) self.safety.safety_rx_hook(self._gas_msg(0)) - self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)) self.assertTrue(self.safety.get_controls_allowed()) @@ -309,8 +315,6 @@ def test_fwd_hook(self): msgs = list(range(0x1, 0x800)) fwd_brake = [False, True] - self.safety.set_honda_hw(HONDA_N_HW) - for f in fwd_brake: self.safety.set_honda_fwd_brake(f) blocked_msgs = [0xE4, 0x194, 0x33D] @@ -332,5 +336,43 @@ def test_fwd_hook(self): self.safety.set_honda_fwd_brake(False) +class TestHondaBoschGiraffeSafety(TestHondaSafety): + @classmethod + def setUp(cls): + TestHondaSafety.setUp() + cls.safety = libpandasafety_py.libpandasafety + cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0) + cls.safety.init_tests_honda() + + def test_fwd_hook(self): + buss = range(0x0, 0x3) + msgs = range(0x1, 0x800) + hw = self.safety.get_honda_hw() + bus_rdr_cam = 2 if hw == HONDA_BH_HW else 1 + bus_rdr_car = 0 if hw == HONDA_BH_HW else 2 + bus_pt = 1 if hw == HONDA_BH_HW else 0 + + blocked_msgs = [0xE4, 0x33D] + for b in buss: + for m in msgs: + if b == bus_pt: + fwd_bus = -1 + elif b == bus_rdr_cam: + fwd_bus = -1 if m in blocked_msgs else bus_rdr_car + elif b == bus_rdr_car: + fwd_bus = bus_rdr_cam + + # assume len 8 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + + +class TestHondaBoschHarnessSafety(TestHondaBoschGiraffeSafety): + @classmethod + def setUp(cls): + TestHondaBoschGiraffeSafety.setUp() + cls.safety = libpandasafety_py.libpandasafety + cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_HARNESS, 0) + cls.safety.init_tests_honda() + if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_honda_bosch.py b/tests/safety/test_honda_bosch.py deleted file mode 100755 index 99c8d35464d956..00000000000000 --- a/tests/safety/test_honda_bosch.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python3 -import unittest -from panda import Panda -from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import make_msg, test_spam_can_buses -from panda.tests.safety.test_honda import HONDA_BG_HW, HONDA_BH_HW - -MAX_BRAKE = 255 - -H_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness -G_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe - - -class TestHondaSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0) - cls.safety.init_tests_honda() - - def test_spam_can_buses(self): - for hw in [HONDA_BG_HW, HONDA_BH_HW]: - self.safety.set_honda_hw(hw) - test_spam_can_buses(self, H_TX_MSGS if hw == HONDA_BH_HW else G_TX_MSGS) - - def test_fwd_hook(self): - buss = range(0x0, 0x3) - msgs = range(0x1, 0x800) - for hw in [HONDA_BG_HW, HONDA_BH_HW]: - self.safety.set_honda_hw(hw) - bus_rdr_cam = 2 if hw == HONDA_BH_HW else 1 - bus_rdr_car = 0 if hw == HONDA_BH_HW else 2 - bus_pt = 1 if hw == HONDA_BH_HW else 0 - - blocked_msgs = [0xE4, 0x33D] - for b in buss: - for m in msgs: - if b == bus_pt: - fwd_bus = -1 - elif b == bus_rdr_cam: - fwd_bus = -1 if m in blocked_msgs else bus_rdr_car - elif b == bus_rdr_car: - fwd_bus = bus_rdr_cam - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - - -if __name__ == "__main__": - unittest.main() diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 9067ec9842870a..2d022759e102f2 100644 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses +from panda.tests.safety.common import StdTest, make_msg MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 @@ -15,6 +15,8 @@ DRIVER_TORQUE_ALLOWANCE = 50; DRIVER_TORQUE_FACTOR = 2; +SPEED_THRESHOLD = 30 # ~1kph + TX_MSGS = [[832, 0], [1265, 0]] def twos_comp(val, bits): @@ -41,6 +43,22 @@ def _button_msg(self, buttons): to_send[0].RDLR = buttons return to_send + def _gas_msg(self, val): + to_send = make_msg(0, 608) + to_send[0].RDHR = (val & 0x3) << 30; + return to_send + + def _brake_msg(self, brake): + to_send = make_msg(0, 916) + to_send[0].RDHR = brake << 23; + return to_send + + def _speed_msg(self, speed): + to_send = make_msg(0, 902) + to_send[0].RDLR = speed & 0x3FFF; + to_send[0].RDHR = (speed & 0x3FFF) << 16; + return to_send + def _set_prev_torque(self, t): self.safety.set_hyundai_desired_torque_last(t) self.safety.set_hyundai_rt_torque_last(t) @@ -56,10 +74,10 @@ def _torque_msg(self, torque): return to_send def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) + StdTest.test_spam_can_buses(self, TX_MSGS) def test_relay_malfunction(self): - test_relay_malfunction(self, 832) + StdTest.test_relay_malfunction(self, 832) def test_default_controls_not_allowed(self): self.assertFalse(self.safety.get_controls_allowed()) @@ -75,7 +93,7 @@ def test_steer_safety_check(self): self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) + StdTest.test_manually_enable_controls_allowed(self) def test_enable_control_allowed_from_cruise(self): to_push = make_msg(0, 1057) @@ -89,6 +107,17 @@ def test_disable_control_allowed_from_cruise(self): self.safety.safety_rx_hook(to_push) self.assertFalse(self.safety.get_controls_allowed()) + def test_disengage_on_gas(self): + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._gas_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_brake_disengage(self): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD) + def test_non_realtime_limit_up(self): self.safety.set_hyundai_torque_driver(0, 0) self.safety.set_controls_allowed(True) diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py new file mode 100644 index 00000000000000..ab826fffbd2a03 --- /dev/null +++ b/tests/safety/test_nissan.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from panda import Panda +from panda.tests.safety import libpandasafety_py +from panda.tests.safety.common import StdTest, make_msg + +ANGLE_MAX_BP = [1.3, 10., 30.] +ANGLE_MAX_V = [540., 120., 23.] +ANGLE_DELTA_BP = [0., 5., 15.] +ANGLE_DELTA_V = [5., .8, .15] # windup limit +ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + +TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2]] + +def twos_comp(val, bits): + if val >= 0: + return val + else: + return (2**bits) + val + +def sign(a): + if a > 0: + return 1 + else: + return -1 + + +class TestNissanSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) + cls.safety.init_tests_nissan() + + def _angle_meas_msg(self, angle): + to_send = make_msg(0, 0x2) + angle = int(angle * -10) + t = twos_comp(angle, 16) + to_send[0].RDLR = t & 0xFFFF + + return to_send + + def _set_prev_angle(self, t): + t = int(t * -100) + self.safety.set_nissan_desired_angle_last(t) + + def _angle_meas_msg_array(self, angle): + for i in range(6): + self.safety.safety_rx_hook(self._angle_meas_msg(angle)) + + def _lkas_state_msg(self, state): + to_send = make_msg(0, 0x1b6) + to_send[0].RDHR = (state & 0x1) << 6 + + return to_send + + def _lkas_control_msg(self, angle, state): + to_send = make_msg(0, 0x169) + angle = int((angle - 1310) * -100) + to_send[0].RDLR = ((angle & 0x3FC00) >> 10) | ((angle & 0x3FC) << 6) | ((angle & 0x3) << 16) + to_send[0].RDHR = ((state & 0x1) << 20) + + return to_send + + def _speed_msg(self, speed): + to_send = make_msg(0, 0x29a) + speed = int(speed / 0.00555 * 3.6) + to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8) + + return to_send + + def _brake_msg(self, brake): + to_send = make_msg(1, 0x454) + to_send[0].RDLR = ((brake & 0x1) << 23) + + return to_send + + def _send_gas_cmd(self, gas): + to_send = make_msg(0, 0x15c) + to_send[0].RDHR = ((gas & 0x3fc) << 6) | ((gas & 0x3) << 22) + + return to_send + + def _acc_button_cmd(self, buttons): + to_send = make_msg(2, 0x20b) + to_send[0].RDLR = (buttons << 8) + + return to_send + + def test_spam_can_buses(self): + StdTest.test_spam_can_buses(self, TX_MSGS) + + def test_angle_cmd_when_enabled(self): + + # when controls are allowed, angle cmd rate limit is enforced + # test 1: no limitations if we stay within limits + speeds = [0., 1., 5., 10., 15., 100.] + angles = [-300, -100, -10, 0, 10, 100, 300] + for a in angles: + for s in speeds: + max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) + max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) + angle_lim = np.interp(s, ANGLE_MAX_BP, ANGLE_MAX_V) + + # first test against false positives + self._angle_meas_msg_array(a) + self.safety.safety_rx_hook(self._speed_msg(s)) + + self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self.safety.set_controls_allowed(1) + + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( + np.clip(a + sign(a) * max_delta_up, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.safety_tx_hook( + self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( + np.clip(a - sign(a) * max_delta_down, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + + # now inject too high rates + self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * + (max_delta_up + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.safety_tx_hook( + self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) * + (max_delta_down + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + + # Check desired steer should be the same as steer angle when controls are off + self.safety.set_controls_allowed(0) + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 0))) + + def test_angle_cmd_when_disabled(self): + self.safety.set_controls_allowed(0) + + self._set_prev_angle(0) + self.assertFalse(self.safety.safety_tx_hook(self._lkas_control_msg(0, 1))) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_brake_disengage(self): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, 0) + + def test_gas_rising_edge(self): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._send_gas_cmd(100)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_acc_buttons(self): + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x2)) # Cancel button + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_tx_hook(self._acc_button_cmd(0x1)) # ProPilot button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x4)) # Follow Distance button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x8)) # Set button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x10)) # Res button + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.safety.safety_tx_hook(self._acc_button_cmd(0x20)) # No button pressed + self.assertFalse(self.safety.get_controls_allowed()) + + def test_relay_malfunction(self): + StdTest.test_relay_malfunction(self, 0x169) + + def test_fwd_hook(self): + + buss = list(range(0x0, 0x3)) + msgs = list(range(0x1, 0x800)) + + blocked_msgs = [0x169,0x2b1,0x4cc] + for b in buss: + for m in msgs: + if b == 0: + fwd_bus = 2 + elif b == 1: + fwd_bus = -1 + elif b == 2: + fwd_bus = -1 if m in blocked_msgs else 0 + + # assume len 8 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + +if __name__ == "__main__": + unittest.main() diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index e18d7515cb1345..d68090d19eb4d9 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses +from panda.tests.safety.common import StdTest, make_msg MAX_RATE_UP = 50 MAX_RATE_DOWN = 70 @@ -15,7 +15,10 @@ DRIVER_TORQUE_ALLOWANCE = 60; DRIVER_TORQUE_FACTOR = 10; -TX_MSGS = [[0x122, 0], [0x164, 0], [0x221, 0], [0x322, 0]] +SPEED_THRESHOLD = 20 # 1kph (see dbc file) + +TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]] +TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]] def twos_comp(val, bits): if val >= 0: @@ -29,7 +32,23 @@ def sign(a): else: return -1 +def subaru_checksum(msg, addr, len_msg): + checksum = addr + (addr >> 8) + for i in range(len_msg): + if i < 4: + checksum += (msg.RDLR >> (8 * i)) + else: + checksum += (msg.RDHR >> (8 * (i - 4))) + return checksum & 0xff + + class TestSubaruSafety(unittest.TestCase): + cnt_gas = 0 + cnt_torque_driver = 0 + cnt_cruise = 0 + cnt_speed = 0 + cnt_brake = 0 + @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety @@ -42,38 +61,105 @@ def _set_prev_torque(self, t): def _torque_driver_msg(self, torque): t = twos_comp(torque, 11) - to_send = make_msg(0, 0x119) - to_send[0].RDLR = ((t & 0x7FF) << 16) + if self.safety.get_subaru_global(): + to_send = make_msg(0, 0x119) + to_send[0].RDLR = ((t & 0x7FF) << 16) + to_send[0].RDLR |= (self.cnt_torque_driver & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x119, 8) + self.__class__.cnt_torque_driver += 1 + else: + to_send = make_msg(0, 0x371) + to_send[0].RDLR = (t & 0x7) << 29 + to_send[0].RDHR = (t >> 3) & 0xFF + return to_send + + def _speed_msg(self, speed): + speed &= 0x1FFF + to_send = make_msg(0, 0x13a) + to_send[0].RDLR = speed << 12 + to_send[0].RDHR = speed << 6 + to_send[0].RDLR |= (self.cnt_speed & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x13a, 8) + self.__class__.cnt_speed += 1 + return to_send + + def _brake_msg(self, brake): + to_send = make_msg(0, 0x139) + to_send[0].RDHR = (brake << 4) & 0xFFF + to_send[0].RDLR |= (self.cnt_brake & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x139, 8) + self.__class__.cnt_brake += 1 return to_send def _torque_msg(self, torque): - to_send = make_msg(0, 0x122) t = twos_comp(torque, 13) - to_send[0].RDLR = (t << 16) + if self.safety.get_subaru_global(): + to_send = make_msg(0, 0x122) + to_send[0].RDLR = (t << 16) + else: + to_send = make_msg(0, 0x164) + to_send[0].RDLR = (t << 8) + return to_send + + def _gas_msg(self, gas): + if self.safety.get_subaru_global(): + to_send = make_msg(0, 0x40) + to_send[0].RDHR = gas & 0xFF + to_send[0].RDLR |= (self.cnt_gas & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x40, 8) + self.__class__.cnt_gas += 1 + else: + to_send = make_msg(0, 0x140) + to_send[0].RDLR = gas & 0xFF + return to_send + + def _cruise_msg(self, cruise): + if self.safety.get_subaru_global(): + to_send = make_msg(0, 0x240) + to_send[0].RDHR = cruise << 9 + to_send[0].RDLR |= (self.cnt_cruise & 0xF) << 8 + to_send[0].RDLR |= subaru_checksum(to_send, 0x240, 8) + self.__class__.cnt_cruise += 1 + else: + to_send = make_msg(0, 0x144) + to_send[0].RDHR = cruise << 17 return to_send + def _set_torque_driver(self, min_t, max_t): + for i in range(0, 5): + self.safety.safety_rx_hook(self._torque_driver_msg(min_t)) + self.safety.safety_rx_hook(self._torque_driver_msg(max_t)) + def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) + StdTest.test_spam_can_buses(self, TX_MSGS if self.safety.get_subaru_global() else TX_L_MSGS) def test_relay_malfunction(self): - test_relay_malfunction(self, 0x122) + StdTest.test_relay_malfunction(self, 0x122 if self.safety.get_subaru_global() else 0x164) def test_default_controls_not_allowed(self): self.assertFalse(self.safety.get_controls_allowed()) def test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x240) - to_push[0].RDHR = 1 << 9 - self.safety.safety_rx_hook(to_push) + self.safety.safety_rx_hook(self._cruise_msg(True)) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x240) - to_push[0].RDHR = 0 self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) + self.safety.safety_rx_hook(self._cruise_msg(False)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_disengage_on_gas(self): + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._gas_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_brake_disengage(self): + if (self.safety.get_subaru_global()): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD) + def test_steer_safety_check(self): for enabled in [0, 1]: for t in range(-3000, 3000): @@ -85,10 +171,10 @@ def test_steer_safety_check(self): self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) + StdTest.test_manually_enable_controls_allowed(self) def test_non_realtime_limit_up(self): - self.safety.set_subaru_torque_driver(0, 0) + self._set_torque_driver(0, 0) self.safety.set_controls_allowed(True) self._set_prev_torque(0) @@ -103,7 +189,7 @@ def test_non_realtime_limit_up(self): self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): - self.safety.set_subaru_torque_driver(0, 0) + self._set_torque_driver(0, 0) self.safety.set_controls_allowed(True) def test_against_torque_driver(self): @@ -112,33 +198,36 @@ def test_against_torque_driver(self): for sign in [-1, 1]: for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): t *= -sign - self.safety.set_subaru_torque_driver(t, t) + self._set_torque_driver(t, t) self._set_prev_torque(MAX_STEER * sign) self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign))) - self.safety.set_subaru_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self._set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER))) + # arbitrary high driver torque to ensure max steer torque is allowed + max_driver_torque = int(MAX_STEER / DRIVER_TORQUE_FACTOR + DRIVER_TORQUE_ALLOWANCE + 1) + # spot check some individual cases for sign in [-1, 1]: driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign delta = 1 * sign self._set_prev_torque(torque_desired) - self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque) + self._set_torque_driver(-driver_torque, -driver_torque) self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired))) self._set_prev_torque(torque_desired + delta) - self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque) + self._set_torque_driver(-driver_torque, -driver_torque) self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign) self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign) self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) self._set_prev_torque(MAX_STEER * sign) - self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign) self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) @@ -148,7 +237,7 @@ def test_realtime_limits(self): for sign in [-1, 1]: self.safety.init_tests_subaru() self._set_prev_torque(0) - self.safety.set_subaru_torque_driver(0, 0) + self._set_torque_driver(0, 0) for t in np.arange(0, MAX_RT_DELTA, 1): t *= sign self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) @@ -168,7 +257,7 @@ def test_realtime_limits(self): def test_fwd_hook(self): buss = list(range(0x0, 0x3)) msgs = list(range(0x1, 0x800)) - blocked_msgs = [290, 356, 545, 802] + blocked_msgs = [290, 545, 802] if self.safety.get_subaru_global() else [356, 545, 802] for b in buss: for m in msgs: if b == 0: @@ -181,6 +270,12 @@ def test_fwd_hook(self): # assume len 8 self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) +class TestSubaruLegacySafety(TestSubaruSafety): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0) + cls.safety.init_tests_subaru() if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index 493fb1f62b0ae3..bc0795595e700a 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses +from panda.tests.safety.common import StdTest, make_msg MAX_RATE_UP = 10 MAX_RATE_DOWN = 25 @@ -15,6 +15,8 @@ MAX_RT_DELTA = 375 RT_INTERVAL = 250000 +STANDSTILL_THRESHOLD = 100 # 1kph + MAX_TORQUE_ERROR = 350 INTERCEPTOR_THRESHOLD = 475 @@ -62,7 +64,7 @@ def _torque_meas_msg(self, torque): t = twos_comp(torque, 16) to_send = make_msg(0, 0x260) to_send[0].RDHR = (t & 0xff00) | ((t & 0xFF) << 16) - to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24) + to_send[0].RDHR |= toyota_checksum(to_send[0], 0x260, 8) << 24 return to_send def _torque_msg(self, torque): @@ -77,6 +79,21 @@ def _accel_msg(self, accel): to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) return to_send + def _speed_msg(self, s): + offset = (0x6f << 8) + 0x1a # there is a 0x1a6f offset in the signal + to_send = make_msg(0, 0xaa) + to_send[0].RDLR = ((s & 0xFF) << 8 | (s >> 8)) + offset + to_send[0].RDLR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16) + to_send[0].RDHR = ((s & 0xFF) << 8 | (s >> 8)) + offset + to_send[0].RDHR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16) + return to_send + + def _brake_msg(self, brake): + to_send = make_msg(0, 0x226) + to_send[0].RDHR = brake << 5 + to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24 + return to_send + def _send_gas_msg(self, gas): to_send = make_msg(0, 0x2C1) to_send[0].RDHR = (gas & 0xFF) << 16 @@ -96,16 +113,16 @@ def _pcm_cruise_msg(self, cruise_on): return to_send def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) + StdTest.test_spam_can_buses(self, TX_MSGS) def test_relay_malfunction(self): - test_relay_malfunction(self, 0x2E4) + StdTest.test_relay_malfunction(self, 0x2E4) def test_default_controls_not_allowed(self): self.assertFalse(self.safety.get_controls_allowed()) def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) + StdTest.test_manually_enable_controls_allowed(self) def test_enable_control_allowed_from_cruise(self): self.safety.safety_rx_hook(self._pcm_cruise_msg(False)) @@ -121,7 +138,7 @@ def test_disable_control_allowed_from_cruise(self): def test_prev_gas(self): for g in range(0, 256): self.safety.safety_rx_hook(self._send_gas_msg(g)) - self.assertEqual(g, self.safety.get_toyota_gas_prev()) + self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev()) def test_prev_gas_interceptor(self): self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) @@ -155,6 +172,10 @@ def test_disengage_on_gas_interceptor(self): self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) self.safety.set_gas_interceptor_detected(False) + def test_brake_disengage(self): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, STANDSTILL_THRESHOLD) + def test_allow_engage_with_gas_interceptor_pressed(self): self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) self.safety.set_controls_allowed(1) @@ -304,6 +325,5 @@ def test_fwd_hook(self): self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_toyota_ipas.py b/tests/safety/test_toyota_ipas.py deleted file mode 100644 index 3fcca95f1ca53c..00000000000000 --- a/tests/safety/test_toyota_ipas.py +++ /dev/null @@ -1,243 +0,0 @@ -#!/usr/bin/env python3 -import unittest -import numpy as np -from panda import Panda -from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import make_msg -from panda.tests.safety.test_toyota import toyota_checksum - -IPAS_OVERRIDE_THRESHOLD = 200 - -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit - -def twos_comp(val, bits): - if val >= 0: - return val - else: - return (2**bits) + val - -def sign(a): - if a > 0: - return 1 - else: - return -1 - - -class TestToyotaSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA_IPAS, 66) - cls.safety.init_tests_toyota() - - def _torque_driver_msg(self, torque): - to_send = make_msg(0, 0x260) - t = twos_comp(torque, 16) - to_send[0].RDLR = t | ((t & 0xFF) << 16) - to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24) - return to_send - - def _torque_driver_msg_array(self, torque): - for i in range(6): - self.safety.safety_rx_hook(self._torque_driver_msg(torque)) - - def _angle_meas_msg(self, angle): - to_send = make_msg(0, 0x25) - t = twos_comp(angle, 12) - to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) - return to_send - - def _angle_meas_msg_array(self, angle): - for i in range(6): - self.safety.safety_rx_hook(self._angle_meas_msg(angle)) - - def _ipas_state_msg(self, state): - to_send = make_msg(0, 0x262) - to_send[0].RDLR = state & 0xF - return to_send - - def _ipas_control_msg(self, angle, state): - # note: we command 2/3 of the angle due to CAN conversion - to_send = make_msg(0, 0x266) - t = twos_comp(angle, 12) - to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) - to_send[0].RDLR |= ((state & 0xf) << 4) - - return to_send - - def _speed_msg(self, speed): - to_send = make_msg(0, 0xB4) - speed = int(speed * 100 * 3.6) - - to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00) - return to_send - - def test_ipas_override(self): - - ## angle control is not active - self.safety.set_controls_allowed(1) - - # 3 consecutive msgs where driver exceeds threshold but angle_control isn't active - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) - self.assertTrue(self.safety.get_controls_allowed()) - - self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) - self.assertTrue(self.safety.get_controls_allowed()) - - # ipas state is override - self.safety.safety_rx_hook(self._ipas_state_msg(5)) - self.assertTrue(self.safety.get_controls_allowed()) - - ## now angle control is active - self.safety.safety_tx_hook(self._ipas_control_msg(0, 0)) - self.safety.safety_rx_hook(self._ipas_state_msg(0)) - - # 3 consecutive msgs where driver does exceed threshold - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) - self.assertFalse(self.safety.get_controls_allowed()) - - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) - self.assertFalse(self.safety.get_controls_allowed()) - - # ipas state is override and torque isn't overriding any more - self.safety.set_controls_allowed(1) - self._torque_driver_msg_array(0) - self.safety.safety_rx_hook(self._ipas_state_msg(5)) - self.assertFalse(self.safety.get_controls_allowed()) - - # 3 consecutive msgs where driver does not exceed threshold and - # ipas state is not override - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._ipas_state_msg(0)) - self.assertTrue(self.safety.get_controls_allowed()) - - self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD) - self.assertTrue(self.safety.get_controls_allowed()) - - self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD) - self.assertTrue(self.safety.get_controls_allowed()) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_when_disabled(self): - - self.safety.set_controls_allowed(0) - - # test angle cmd too far from actual - angle_refs = [-10, 10] - deltas = list(range(-2, 3)) - expected_results = [False, True, True, True, False] - - for a in angle_refs: - self._angle_meas_msg_array(a) - for i, d in enumerate(deltas): - self.assertEqual(expected_results[i], self.safety.safety_tx_hook(self._ipas_control_msg(a + d, 1))) - - # test ipas state cmd enabled - self._angle_meas_msg_array(0) - self.assertEqual(0, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3))) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_when_enabled(self): - - # ipas angle cmd should pass through when controls are enabled - - self.safety.set_controls_allowed(1) - self._angle_meas_msg_array(0) - self.safety.safety_rx_hook(self._speed_msg(0.1)) - - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(4, 1))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 3))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-4, 3))) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-8, 3))) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_rate_when_disabled(self): - - # as long as the command is close to the measured, no rate limit is enforced when - # controls are disabled - self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._angle_meas_msg(0)) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(0, 1))) - self.safety.safety_rx_hook(self._angle_meas_msg(100)) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(100, 1))) - self.safety.safety_rx_hook(self._angle_meas_msg(-100)) - self.assertEqual(1, self.safety.safety_tx_hook(self._ipas_control_msg(-100, 1))) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_cmd_rate_when_enabled(self): - - # when controls are allowed, angle cmd rate limit is enforced - # test 1: no limitations if we stay within limits - speeds = [0., 1., 5., 10., 15., 100.] - angles = [-300, -100, -10, 0, 10, 100, 300] - for a in angles: - for s in speeds: - - # first test against false positives - self._angle_meas_msg_array(a) - self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._speed_msg(s)) - max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) - max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - - # now inject too high rates - self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * - (max_delta_up + 1), 1))) - self.assertFalse(self.safety.get_controls_allowed()) - self.safety.set_controls_allowed(1) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._ipas_control_msg(a, 1))) - self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(False, self.safety.safety_tx_hook(self._ipas_control_msg(a - sign(a) * - (max_delta_down + 1), 1))) - self.assertFalse(self.safety.get_controls_allowed()) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - def test_angle_measured_rate(self): - - speeds = [0., 1., 5., 10., 15., 100.] - angles = [-300, -100, -10, 0, 10, 100, 300] - angles = [10] - for a in angles: - for s in speeds: - self._angle_meas_msg_array(a) - self.safety.safety_tx_hook(self._ipas_control_msg(a, 1)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._speed_msg(s)) - #max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) - #max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) - self.safety.safety_rx_hook(self._angle_meas_msg(a)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._angle_meas_msg(a + 150)) - self.assertFalse(self.safety.get_controls_allowed()) - - # reset no angle control at the end of the test - self.safety.reset_angle_control() - - -if __name__ == "__main__": - unittest.main() diff --git a/tests/safety/test_volkswagen.py b/tests/safety/test_volkswagen.py deleted file mode 100644 index db58cdc5811a45..00000000000000 --- a/tests/safety/test_volkswagen.py +++ /dev/null @@ -1,226 +0,0 @@ -#!/usr/bin/env python3 -import unittest -import numpy as np -from panda import Panda -from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses - -MAX_RATE_UP = 4 -MAX_RATE_DOWN = 10 -MAX_STEER = 250 - -MAX_RT_DELTA = 75 -RT_INTERVAL = 250000 - -DRIVER_TORQUE_ALLOWANCE = 80 -DRIVER_TORQUE_FACTOR = 3 - -TX_MSGS = [[0x126, 0], [0x12B, 0], [0x12B, 2], [0x397, 0]] - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -class TestVolkswagenSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN, 0) - cls.safety.init_tests_volkswagen() - - def _set_prev_torque(self, t): - self.safety.set_volkswagen_desired_torque_last(t) - self.safety.set_volkswagen_rt_torque_last(t) - - def _torque_driver_msg(self, torque): - to_send = make_msg(0, 0x9F) - t = abs(torque) - to_send[0].RDHR = ((t & 0x1FFF) << 8) - if torque < 0: - to_send[0].RDHR |= 0x1 << 23 - return to_send - - def _torque_msg(self, torque): - to_send = make_msg(0, 0x126) - t = abs(torque) - to_send[0].RDLR = (t & 0xFFF) << 16 - if torque < 0: - to_send[0].RDLR |= 0x1 << 31 - return to_send - - def _gas_msg(self, gas): - to_send = make_msg(0, 0x121) - to_send[0].RDLR = (gas & 0xFF) << 12 - return to_send - - def _button_msg(self, bit): - to_send = make_msg(2, 0x12B) - to_send[0].RDLR = 1 << bit - return to_send - - def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) - - def test_relay_malfunction(self): - test_relay_malfunction(self, 0x126) - - def test_prev_gas(self): - for g in range(0, 256): - self.safety.safety_rx_hook(self._gas_msg(g)) - self.assertEqual(g, self.safety.get_volkswagen_gas_prev()) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) - - def test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x122) - to_push[0].RDHR = 0x30000000 - self.safety.safety_rx_hook(to_push) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x122) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_disengage_on_gas(self): - self.safety.safety_rx_hook(self._gas_msg(0)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._gas_msg(1)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - - - def test_steer_safety_check(self): - for enabled in [0, 1]: - for t in range(-500, 500): - self.safety.set_controls_allowed(enabled) - self._set_prev_torque(t) - if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(t))) - else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - def test_manually_enable_controls_allowed(self): - test_manually_enable_controls_allowed(self) - - def test_spam_cancel_safety_check(self): - BIT_CANCEL = 13 - BIT_RESUME = 19 - BIT_SET = 16 - self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._button_msg(BIT_CANCEL))) - self.assertFalse(self.safety.safety_tx_hook(self._button_msg(BIT_RESUME))) - self.assertFalse(self.safety.safety_tx_hook(self._button_msg(BIT_SET))) - # do not block resume if we are engaged already - self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._button_msg(BIT_RESUME))) - - def test_non_realtime_limit_up(self): - self.safety.set_volkswagen_torque_driver(0, 0) - self.safety.set_controls_allowed(True) - - self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) - self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP))) - - self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) - self.safety.set_controls_allowed(True) - self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) - - def test_non_realtime_limit_down(self): - self.safety.set_volkswagen_torque_driver(0, 0) - self.safety.set_controls_allowed(True) - - def test_against_torque_driver(self): - self.safety.set_controls_allowed(True) - - for sign in [-1, 1]: - for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): - t *= -sign - self.safety.set_volkswagen_torque_driver(t, t) - self._set_prev_torque(MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign))) - - self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER))) - - # spot check some individual cases - for sign in [-1, 1]: - driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign - torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign - delta = 1 * sign - self._set_prev_torque(torque_desired) - self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired))) - self._set_prev_torque(torque_desired + delta) - self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) - - self._set_prev_torque(MAX_STEER * sign) - self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) - self._set_prev_torque(MAX_STEER * sign) - self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) - self._set_prev_torque(MAX_STEER * sign) - self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) - - - def test_realtime_limits(self): - self.safety.set_controls_allowed(True) - - for sign in [-1, 1]: - self.safety.init_tests_volkswagen() - self._set_prev_torque(0) - self.safety.set_volkswagen_torque_driver(0, 0) - for t in np.arange(0, MAX_RT_DELTA, 1): - t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - - self._set_prev_torque(0) - for t in np.arange(0, MAX_RT_DELTA, 1): - t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - # Increase timer to update rt_torque_last - self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - - - def test_fwd_hook(self): - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - blocked_msgs_0to2 = [] - blocked_msgs_2to0 = [0x126, 0x397] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = -1 if m in blocked_msgs_0to2 else 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs_2to0 else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - - -if __name__ == "__main__": - unittest.main() diff --git a/tests/safety/test_volkswagen_mqb.py b/tests/safety/test_volkswagen_mqb.py new file mode 100644 index 00000000000000..a413fd445b05a9 --- /dev/null +++ b/tests/safety/test_volkswagen_mqb.py @@ -0,0 +1,397 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +import crcmod +from panda import Panda +from panda.tests.safety import libpandasafety_py +from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS + +MAX_RATE_UP = 4 +MAX_RATE_DOWN = 10 +MAX_STEER = 300 +MAX_RT_DELTA = 75 +RT_INTERVAL = 250000 + +DRIVER_TORQUE_ALLOWANCE = 80 +DRIVER_TORQUE_FACTOR = 3 + +MSG_ESP_19 = 0xB2 # RX from ABS, for wheel speeds +MSG_EPS_01 = 0x9F # RX from EPS, for driver steering torque +MSG_ESP_05 = 0x106 # RX from ABS, for brake light state +MSG_TSK_06 = 0x120 # RX from ECU, for ACC status from drivetrain coordinator +MSG_MOTOR_20 = 0x121 # RX from ECU, for driver throttle input +MSG_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque +MSG_GRA_ACC_01 = 0x12B # TX by OP, ACC control buttons for cancel/resume +MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts + +# Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration +TX_MSGS = [[MSG_HCA_01, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2], [MSG_LDW_02, 0]] + +def sign(a): + if a > 0: + return 1 + else: + return -1 + +# Python crcmod works differently somehow from every other CRC calculator. The +# implied leading 1 on the polynomial isn't a problem, but to get the right +# result for CRC-8H2F/AUTOSAR, we have to feed it initCrc 0x00 instead of 0xFF. +volkswagen_crc_8h2f = crcmod.mkCrcFun(0x12F, initCrc=0x00, rev=False, xorOut=0xFF) + +def volkswagen_mqb_crc(msg, addr, len_msg): + # This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation of + # this algorithm for a version with explanatory comments. + msg_bytes = msg.RDLR.to_bytes(4, 'little') + msg.RDHR.to_bytes(4, 'little') + counter = (msg.RDLR & 0xF00) >> 8 + if addr == MSG_EPS_01: + magic_pad = b'\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5'[counter] + elif addr == MSG_ESP_05: + magic_pad = b'\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07'[counter] + elif addr == MSG_TSK_06: + magic_pad = b'\xC4\xE2\x4F\xE4\xF8\x2F\x56\x81\x9F\xE5\x83\x44\x05\x3F\x97\xDF'[counter] + elif addr == MSG_MOTOR_20: + magic_pad = b'\xE9\x65\xAE\x6B\x7B\x35\xE5\x5F\x4E\xC7\x86\xA2\xBB\xDD\xEB\xB4'[counter] + elif addr == MSG_HCA_01: + magic_pad = b'\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA'[counter] + elif addr == MSG_GRA_ACC_01: + magic_pad = b'\x6A\x38\xB4\x27\x22\xEF\xE1\xBB\xF8\x80\x84\x49\xC7\x9E\x1E\x2B'[counter] + else: + magic_pad = None + return volkswagen_crc_8h2f(msg_bytes[1:len_msg] + magic_pad.to_bytes(1, 'little')) + +class TestVolkswagenMqbSafety(unittest.TestCase): + cnt_eps_01 = 0 + cnt_esp_05 = 0 + cnt_tsk_06 = 0 + cnt_motor_20 = 0 + cnt_hca_01 = 0 + cnt_gra_acc_01 = 0 + + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0) + cls.safety.init_tests_volkswagen() + + def _set_prev_torque(self, t): + self.safety.set_volkswagen_desired_torque_last(t) + self.safety.set_volkswagen_rt_torque_last(t) + + # Wheel speeds _esp_19_msg + def _speed_msg(self, speed): + wheel_speed_scaled = int(speed / 0.0075) + to_send = make_msg(0, MSG_ESP_19) + to_send[0].RDLR = wheel_speed_scaled | (wheel_speed_scaled << 16) + to_send[0].RDHR = wheel_speed_scaled | (wheel_speed_scaled << 16) + return to_send + + # Brake light switch _esp_05_msg + def _brake_msg(self, brake): + to_send = make_msg(0, MSG_ESP_05) + to_send[0].RDLR = (0x1 << 26) if brake else 0 + to_send[0].RDLR |= (self.cnt_esp_05 % 16) << 8 + to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_ESP_05, 8) + self.__class__.cnt_esp_05 += 1 + return to_send + + # Driver steering input torque + def _eps_01_msg(self, torque): + to_send = make_msg(0, MSG_EPS_01) + t = abs(torque) + to_send[0].RDHR = ((t & 0x1FFF) << 8) + if torque < 0: + to_send[0].RDHR |= 0x1 << 23 + to_send[0].RDLR |= (self.cnt_eps_01 % 16) << 8 + to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_EPS_01, 8) + self.__class__.cnt_eps_01 += 1 + return to_send + + # openpilot steering output torque + def _hca_01_msg(self, torque): + to_send = make_msg(0, MSG_HCA_01) + t = abs(torque) + to_send[0].RDLR = (t & 0xFFF) << 16 + if torque < 0: + to_send[0].RDLR |= 0x1 << 31 + to_send[0].RDLR |= (self.cnt_hca_01 % 16) << 8 + to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_HCA_01, 8) + self.__class__.cnt_hca_01 += 1 + return to_send + + # ACC engagement status + def _tsk_06_msg(self, status): + to_send = make_msg(0, MSG_TSK_06) + to_send[0].RDLR = (status & 0x7) << 24 + to_send[0].RDLR |= (self.cnt_tsk_06 % 16) << 8 + to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_TSK_06, 8) + self.__class__.cnt_tsk_06 += 1 + return to_send + + # Driver throttle input + def _motor_20_msg(self, gas): + to_send = make_msg(0, MSG_MOTOR_20) + to_send[0].RDLR = (gas & 0xFF) << 12 + to_send[0].RDLR |= (self.cnt_motor_20 % 16) << 8 + to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_MOTOR_20, 8) + self.__class__.cnt_motor_20 += 1 + return to_send + + # Cruise control buttons + def _gra_acc_01_msg(self, bit): + to_send = make_msg(2, MSG_GRA_ACC_01) + to_send[0].RDLR = 1 << bit + to_send[0].RDLR |= (self.cnt_gra_acc_01 % 16) << 8 + to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_GRA_ACC_01, 8) + self.__class__.cnt_gra_acc_01 += 1 + return to_send + + def test_spam_can_buses(self): + StdTest.test_spam_can_buses(self, TX_MSGS) + + def test_relay_malfunction(self): + StdTest.test_relay_malfunction(self, MSG_HCA_01) + + def test_prev_gas(self): + for g in range(0, 256): + self.safety.safety_rx_hook(self._motor_20_msg(g)) + self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev()) + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + def test_enable_control_allowed_from_cruise(self): + self.safety.set_controls_allowed(0) + self.safety.safety_rx_hook(self._tsk_06_msg(3)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_disable_control_allowed_from_cruise(self): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._tsk_06_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_sample_speed(self): + # Stationary + self.safety.safety_rx_hook(self._speed_msg(0)) + self.assertEqual(0, self.safety.get_volkswagen_moving()) + # 1 km/h, just under 0.3 m/s safety grace threshold + self.safety.safety_rx_hook(self._speed_msg(1)) + self.assertEqual(0, self.safety.get_volkswagen_moving()) + # 2 km/h, just over 0.3 m/s safety grace threshold + self.safety.safety_rx_hook(self._speed_msg(2)) + self.assertEqual(1, self.safety.get_volkswagen_moving()) + # 144 km/h, openpilot V_CRUISE_MAX + self.safety.safety_rx_hook(self._speed_msg(144)) + self.assertEqual(1, self.safety.get_volkswagen_moving()) + + def test_prev_brake(self): + self.assertFalse(self.safety.get_brake_pressed_prev()) + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_brake_pressed_prev()) + + def test_brake_disengage(self): + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, 1) + + def test_disengage_on_gas(self): + self.safety.safety_rx_hook(self._motor_20_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._motor_20_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_allow_engage_with_gas_pressed(self): + self.safety.safety_rx_hook(self._motor_20_msg(1)) + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._motor_20_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._motor_20_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_steer_safety_check(self): + for enabled in [0, 1]: + for t in range(-500, 500): + self.safety.set_controls_allowed(enabled) + self._set_prev_torque(t) + if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(t))) + else: + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t))) + + def test_manually_enable_controls_allowed(self): + StdTest.test_manually_enable_controls_allowed(self) + + def test_spam_cancel_safety_check(self): + BIT_CANCEL = 13 + BIT_RESUME = 19 + BIT_SET = 16 + self.safety.set_controls_allowed(0) + self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_CANCEL))) + self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME))) + self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_SET))) + # do not block resume if we are engaged already + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME))) + + def test_non_realtime_limit_up(self): + self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + self._set_prev_torque(0) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP))) + self._set_prev_torque(0) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP))) + + self._set_prev_torque(0) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP + 1))) + self.safety.set_controls_allowed(True) + self._set_prev_torque(0) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP - 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + def test_against_torque_driver(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): + t *= -sign + self.safety.set_volkswagen_torque_driver(t, t) + self._set_prev_torque(MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(MAX_STEER * sign))) + + self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_STEER))) + + # spot check some individual cases + for sign in [-1, 1]: + driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign + torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign + delta = 1 * sign + self._set_prev_torque(torque_desired) + self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired))) + self._set_prev_torque(torque_desired + delta) + self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(torque_desired + delta))) + + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(0))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + + def test_realtime_limits(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self.safety.init_tests_volkswagen() + self._set_prev_torque(0) + self.safety.set_volkswagen_torque_driver(0, 0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t))) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) + + self._set_prev_torque(0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t))) + + # Increase timer to update rt_torque_last + self.safety.set_timer(RT_INTERVAL + 1) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) + + def test_torque_measurements(self): + self.safety.safety_rx_hook(self._eps_01_msg(50)) + self.safety.safety_rx_hook(self._eps_01_msg(-50)) + self.safety.safety_rx_hook(self._eps_01_msg(0)) + self.safety.safety_rx_hook(self._eps_01_msg(0)) + self.safety.safety_rx_hook(self._eps_01_msg(0)) + self.safety.safety_rx_hook(self._eps_01_msg(0)) + + self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min()) + self.assertEqual(50, self.safety.get_volkswagen_torque_driver_max()) + + self.safety.safety_rx_hook(self._eps_01_msg(0)) + self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max()) + self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min()) + + self.safety.safety_rx_hook(self._eps_01_msg(0)) + self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max()) + self.assertEqual(0, self.safety.get_volkswagen_torque_driver_min()) + + def test_rx_hook(self): + # checksum checks + # TODO: Would be ideal to check ESP_19 as well, but it has no checksum + # or counter, and I'm not sure if we can easily validate Panda's simple + # temporal reception-rate check here. + for msg in [MSG_EPS_01, MSG_ESP_05, MSG_TSK_06, MSG_MOTOR_20]: + self.safety.set_controls_allowed(1) + if msg == MSG_EPS_01: + to_push = self._eps_01_msg(0) + if msg == MSG_ESP_05: + to_push = self._brake_msg(False) + if msg == MSG_TSK_06: + to_push = self._tsk_06_msg(3) + if msg == MSG_MOTOR_20: + to_push = self._motor_20_msg(0) + self.assertTrue(self.safety.safety_rx_hook(to_push)) + to_push[0].RDHR ^= 0xFF + self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) + + # counter + # reset wrong_counters to zero by sending valid messages + for i in range(MAX_WRONG_COUNTERS + 1): + self.__class__.cnt_eps_01 += 1 + self.__class__.cnt_esp_05 += 1 + self.__class__.cnt_tsk_06 += 1 + self.__class__.cnt_motor_20 += 1 + if i < MAX_WRONG_COUNTERS: + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._eps_01_msg(0)) + self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.safety_rx_hook(self._tsk_06_msg(3)) + self.safety.safety_rx_hook(self._motor_20_msg(0)) + else: + self.assertFalse(self.safety.safety_rx_hook(self._eps_01_msg(0))) + self.assertFalse(self.safety.safety_rx_hook(self._brake_msg(False))) + self.assertFalse(self.safety.safety_rx_hook(self._tsk_06_msg(3))) + self.assertFalse(self.safety.safety_rx_hook(self._motor_20_msg(0))) + self.assertFalse(self.safety.get_controls_allowed()) + + # restore counters for future tests with a couple of good messages + for i in range(2): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._eps_01_msg(0)) + self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.safety_rx_hook(self._tsk_06_msg(3)) + self.safety.safety_rx_hook(self._motor_20_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_fwd_hook(self): + buss = list(range(0x0, 0x3)) + msgs = list(range(0x1, 0x800)) + blocked_msgs_0to2 = [] + blocked_msgs_2to0 = [MSG_HCA_01, MSG_LDW_02] + for b in buss: + for m in msgs: + if b == 0: + fwd_bus = -1 if m in blocked_msgs_0to2 else 2 + elif b == 1: + fwd_bus = -1 + elif b == 2: + fwd_bus = -1 if m in blocked_msgs_2to0 else 0 + + # assume len 8 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/safety_replay/Dockerfile b/tests/safety_replay/Dockerfile index f5175c74582005..b0b5f0c2710bfa 100644 --- a/tests/safety_replay/Dockerfile +++ b/tests/safety_replay/Dockerfile @@ -25,7 +25,8 @@ RUN mkdir /openpilot WORKDIR /openpilot RUN git clone https://github.com/commaai/cereal.git || true WORKDIR /openpilot/cereal -RUN git checkout f7043fde062cbfd49ec90af669901a9caba52de9 +RUN git pull +RUN git checkout bb2cc7572de99becce1bfbae63f3b38d5464e162 COPY . /openpilot/panda WORKDIR /openpilot/panda/tests/safety_replay diff --git a/tests/safety_replay/test_safety_replay.py b/tests/safety_replay/test_safety_replay.py index fd36b248e396e5..81b9f5b13e9d54 100755 --- a/tests/safety_replay/test_safety_replay.py +++ b/tests/safety_replay/test_safety_replay.py @@ -18,7 +18,8 @@ ("02ec6bea180a4d36|2019-04-17--11-21-35.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SANTA_FE ("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER, 0), # CHRYSLER.PACIFICA_2018_HYBRID ("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU, 0), # SUBARU.IMPREZA - ("b0c9d2329ad1606b|2019-11-17--17-06-13.bz2", Panda.SAFETY_VOLKSWAGEN, 0), # VOLKSWAGEN.GOLF + ("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF + ("fbbfa6af821552b9|2020-03-03--08-09-43.bz2", Panda.SAFETY_NISSAN, 0), # NISSAN.XTRAIL ] if __name__ == "__main__":