From c0e22490415449173ba889e488b3ae5726aa9c04 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Thu, 20 Feb 2020 09:53:24 -0800 Subject: [PATCH] Squashed 'panda/' changes from 769ade051..b2ffaae60 b2ffaae60 Chrysler: disengage on gas press (#442) 2ebbe3616 Subaru: disengage on gas press (#446) ccf75c456 Volkswagen safety updates: Phase 1 (#444) git-subtree-dir: panda git-subtree-split: b2ffaae60ee73581f58b0cab757d5ac09db9dd5c --- board/safety.h | 4 +- board/safety/safety_chrysler.h | 25 +- board/safety/safety_subaru.h | 12 +- board/safety/safety_toyota.h | 2 +- board/safety/safety_volkswagen.h | 235 +++++++++--------- python/__init__.py | 2 +- tests/safety/libpandasafety_py.py | 4 +- tests/safety/test.c | 10 + tests/safety/test_chrysler.py | 22 ++ tests/safety/test_subaru.py | 12 + ...t_volkswagen.py => test_volkswagen_mqb.py} | 133 ++++++---- tests/safety_replay/test_safety_replay.py | 2 +- 12 files changed, 292 insertions(+), 171 deletions(-) rename tests/safety/{test_volkswagen.py => test_volkswagen_mqb.py} (56%) diff --git a/board/safety.h b/board/safety.h index 7fd057b5249837..b4c94ef4054218 100644 --- a/board/safety.h +++ b/board/safety.h @@ -31,7 +31,7 @@ #define SAFETY_TESLA 10U #define SAFETY_SUBARU 11U #define SAFETY_MAZDA 13U -#define SAFETY_VOLKSWAGEN 15U +#define SAFETY_VOLKSWAGEN_MQB 15U #define SAFETY_TOYOTA_IPAS 16U #define SAFETY_ALLOUTPUT 17U #define SAFETY_GM_ASCM 18U @@ -185,7 +185,7 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_CHRYSLER, &chrysler_hooks}, {SAFETY_SUBARU, &subaru_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}, diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 293839b7f62d98..dd37f05a3dbd67 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -4,18 +4,23 @@ 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 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 = {514}, .bus = 0, .expected_timestep = 10000U}, {.addr = {500}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {308}, .bus = 0, .expected_timestep = 20000U}, // verify ts }; 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; +bool chrysler_gas_prev = false; +int chrysler_speed = 0; uint32_t chrysler_ts_last = 0; struct sample_t chrysler_torque_meas; // last few torques measured @@ -29,7 +34,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); // Measured eps torque - if (addr == 544) { + if ((addr == 544) && (bus == 0)) { int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U; // update array of samples @@ -37,7 +42,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) && (bus == 0)) { int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7; if (cruise_engaged && !chrysler_cruise_engaged_last) { controls_allowed = 1; @@ -48,7 +53,21 @@ 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) && (bus == 0)) { + int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4); + int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4); + chrysler_speed = (speed_l + speed_r) / 2; + } + + // exit controls on rising edge of gas press + if ((addr == 308) && (bus == 0)) { + bool gas = (GET_BYTE(to_push, 5) & 0x7F) != 0; + if (gas && !chrysler_gas_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) { + controls_allowed = 0; + } + chrysler_gas_prev = gas; + } // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) { diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 885e2731d523c2..505b4b8568e35d 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -12,6 +12,7 @@ const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x164, 0}, {0x221, 0}, {0x322, 0} // TODO: do checksum and counter checks after adding the signals to the outback dbc file AddrCheckStruct subaru_rx_checks[] = { + {.addr = { 0x40, 0x140}, .bus = 0, .expected_timestep = 10000U}, {.addr = {0x119, 0x371}, .bus = 0, .expected_timestep = 20000U}, {.addr = {0x240, 0x144}, .bus = 0, .expected_timestep = 50000U}, }; @@ -21,6 +22,7 @@ int subaru_cruise_engaged_last = 0; int subaru_rt_torque_last = 0; int subaru_desired_torque_last = 0; uint32_t subaru_ts_last = 0; +bool subaru_gas_last = false; struct sample_t subaru_torque_driver; // last few driver torques measured static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { @@ -53,7 +55,15 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { subaru_cruise_engaged_last = cruise_engaged; } - // TODO: enforce cancellation on gas pressed + // exit controls on rising edge of gas press + if (((addr == 0x40) || (addr == 0x140)) && (bus == 0)) { + int byte = (addr == 0x40) ? 4 : 0; + bool gas = GET_BYTE(to_push, byte) != 0; + if (gas && !subaru_gas_last) { + controls_allowed = 0; + } + subaru_gas_last = gas; + } if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) { relay_malfunction = true; diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 4bf0a5de43d067..16900f00d01131 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -106,7 +106,7 @@ 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; + int gas = GET_BYTE(to_push, 6); if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) { controls_allowed = 0; } diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index 102cb22b57b04f..af15a9f8488986 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -1,142 +1,156 @@ -// 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 - +// Safety-relevant steering constants for Volkswagen const int VOLKSWAGEN_MAX_STEER = 250; // 2.5 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}}; +// Safety-relevant CAN messages for the Volkswagen MQB platform +#define MSG_EPS_01 0x09F // RX from EPS, for driver steering torque +#define MSG_MOTOR_20 0x121 // RX from ECU, for driver throttle input +#define MSG_ACC_06 0x122 // RX from ACC radar, for status and engagement +#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]); // TODO: do checksum and counter checks -AddrCheckStruct volkswagen_rx_checks[] = { +AddrCheckStruct volkswagen_mqb_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}, }; +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; +int volkswagen_torque_msg = 0; +int volkswagen_lane_msg = 0; + +static void volkswagen_mqb_init(int16_t param) { + UNUSED(param); -static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + controls_allowed = false; + relay_malfunction = false; + volkswagen_torque_msg = MSG_HCA_01; + volkswagen_lane_msg = MSG_LDW_02; +} + +static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN, + bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_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; - } + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // Update driver input torque samples + // Signal: EPS_01.Driver_Strain (absolute torque) + // Signal: EPS_01.Driver_Strain_VZ (direction) + if ((bus == 0) && (addr == MSG_EPS_01)) { + 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); + } + + // Update ACC status from radar for controls-allowed state + // Signal: ACC_06.ACC_Status_ACC + if ((bus == 0) && (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 + // Signal: Motor_20.MO_Fahrpedalrohwert_01 + if ((bus == 0) && (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 there are HCA messages on bus 0 not sent by OP, there's a relay problem + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) { + relay_malfunction = true; + } } return valid; } -static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { +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; + } + } + + // 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_TX_MSGS, sizeof(VOLKSWAGEN_TX_MSGS)/sizeof(VOLKSWAGEN_TX_MSGS[0]))) { - tx = 0; - } - - if (relay_malfunction) { + 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. + // 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) { - bool violation = false; - 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; } - 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; - } - } - - // 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; - } - - if (violation) { + if (volkswagen_steering_check(desired_torque)) { tx = 0; } } @@ -158,25 +172,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 +196,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/python/__init__.py b/python/__init__.py index ea8dea13d93874..ea2b2d36d9af26 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -123,7 +123,7 @@ class Panda(object): SAFETY_TESLA = 10 SAFETY_SUBARU = 11 SAFETY_MAZDA = 13 - SAFETY_VOLKSWAGEN = 15 + SAFETY_VOLKSWAGEN_MQB = 15 SAFETY_TOYOTA_IPAS = 16 SAFETY_ALLOUTPUT = 17 SAFETY_GM_ASCM = 18 diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 77131c1a3c8e53..4dfaef148e1f61 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -91,10 +91,12 @@ void set_subaru_torque_driver(int min, int max); void init_tests_volkswagen(void); +int get_volkswagen_gas_prev(void); +int get_volkswagen_torque_driver_min(void); +int get_volkswagen_torque_driver_max(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); """) diff --git a/tests/safety/test.c b/tests/safety/test.c index 59611d3131c67e..85b87ff78ba071 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -148,6 +148,14 @@ void set_volkswagen_torque_driver(int min, int max){ 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; } @@ -304,6 +312,8 @@ void init_tests_hyundai(void){ void init_tests_chrysler(void){ init_tests(); + chrysler_gas_prev = false; + chrysler_speed = 0; chrysler_torque_meas.min = 0; chrysler_torque_meas.max = 0; chrysler_desired_torque_last = 0; diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index 2c472706fe1c99..09720bfd01a212 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -28,6 +28,18 @@ def _button_msg(self, buttons): to_send[0].RDLR = buttons 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 + 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) @@ -80,6 +92,16 @@ def test_disable_control_allowed_from_cruise(self): 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_non_realtime_limit_up(self): self.safety.set_controls_allowed(True) diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index e18d7515cb1345..0f0dbcb1e21ac1 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -52,6 +52,11 @@ def _torque_msg(self, torque): to_send[0].RDLR = (t << 16) return to_send + def _gas_msg(self, gas): + to_send = make_msg(0, 0x40) + to_send[0].RDHR = gas & 0xFF + return to_send + def test_spam_can_buses(self): test_spam_can_buses(self, TX_MSGS) @@ -74,6 +79,13 @@ 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_steer_safety_check(self): for enabled in [0, 1]: for t in range(-3000, 3000): diff --git a/tests/safety/test_volkswagen.py b/tests/safety/test_volkswagen_mqb.py similarity index 56% rename from tests/safety/test_volkswagen.py rename to tests/safety/test_volkswagen_mqb.py index db58cdc5811a45..9f5b461043cd31 100644 --- a/tests/safety/test_volkswagen.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -3,19 +3,27 @@ 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 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]] +MSG_EPS_01 = 0x9F # RX from EPS, for driver steering torque +MSG_ACC_06 = 0x122 # RX from ACC radar, for status and engagement +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: @@ -23,40 +31,50 @@ def sign(a): else: return -1 -class TestVolkswagenSafety(unittest.TestCase): +class TestVolkswagenMqbSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN, 0) + 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) - def _torque_driver_msg(self, torque): - to_send = make_msg(0, 0x9F) + # 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 return to_send - def _torque_msg(self, torque): - to_send = make_msg(0, 0x126) + # 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 return to_send - def _gas_msg(self, gas): - to_send = make_msg(0, 0x121) + # ACC engagement status + def _acc_06_msg(self, status): + to_send = make_msg(0, MSG_ACC_06) + to_send[0].RDHR = (status & 0x7) << 28 + 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 return to_send - def _button_msg(self, bit): - to_send = make_msg(2, 0x12B) + # 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 return to_send @@ -64,52 +82,49 @@ def test_spam_can_buses(self): test_spam_can_buses(self, TX_MSGS) def test_relay_malfunction(self): - test_relay_malfunction(self, 0x126) + test_relay_malfunction(self, MSG_HCA_01) def test_prev_gas(self): for g in range(0, 256): - self.safety.safety_rx_hook(self._gas_msg(g)) + self.safety.safety_rx_hook(self._motor_20_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.safety.set_controls_allowed(0) + self.safety.safety_rx_hook(self._acc_06_msg(3)) 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.safety.safety_rx_hook(self._acc_06_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) def test_disengage_on_gas(self): - self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.safety_rx_hook(self._motor_20_msg(0)) self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(1)) + 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._gas_msg(1)) + self.safety.safety_rx_hook(self._motor_20_msg(1)) self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(1)) + self.safety.safety_rx_hook(self._motor_20_msg(1)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(1)) + 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._torque_msg(t))) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t))) def test_manually_enable_controls_allowed(self): test_manually_enable_controls_allowed(self) @@ -119,27 +134,27 @@ def test_spam_cancel_safety_check(self): 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))) + 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._button_msg(BIT_RESUME))) + 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._torque_msg(MAX_RATE_UP))) + 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._torque_msg(-MAX_RATE_UP))) + 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._torque_msg(MAX_RATE_UP + 1))) + 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._torque_msg(-MAX_RATE_UP - 1))) + 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) @@ -153,10 +168,10 @@ def test_against_torque_driver(self): 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.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._torque_msg(-MAX_STEER))) + self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -165,21 +180,20 @@ def test_against_torque_driver(self): 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.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._torque_msg(torque_desired + delta))) + 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._torque_msg((MAX_STEER - MAX_RATE_DOWN) * 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._torque_msg(0))) + 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._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * 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) @@ -190,25 +204,44 @@ def test_realtime_limits(self): 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.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._torque_msg(t))) + 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._torque_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 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_fwd_hook(self): buss = list(range(0x0, 0x3)) msgs = list(range(0x1, 0x800)) blocked_msgs_0to2 = [] - blocked_msgs_2to0 = [0x126, 0x397] + blocked_msgs_2to0 = [MSG_HCA_01, MSG_LDW_02] for b in buss: for m in msgs: if b == 0: diff --git a/tests/safety_replay/test_safety_replay.py b/tests/safety_replay/test_safety_replay.py index fd36b248e396e5..1120827417f239 100755 --- a/tests/safety_replay/test_safety_replay.py +++ b/tests/safety_replay/test_safety_replay.py @@ -18,7 +18,7 @@ ("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 + ("b0c9d2329ad1606b|2019-11-17--17-06-13.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF ] if __name__ == "__main__":