From 48d0d0c782de3f72c69d3fe74bfdac2b01d9e547 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Tue, 29 Oct 2019 15:11:42 -0700 Subject: [PATCH] VW button spam: fix safety and add tests (#306) * VW button spam: fix safety and add tests * button spam is actually sent on bus 2 * Fix safety test. Still need to add gas safety test * added gas safety test to VW and using consistent nomenclature * style fixes --- board/safety/safety_volkswagen.h | 89 ++++++++++++++++++------------- tests/safety/libpandasafety_py.py | 1 + tests/safety/test.c | 22 ++++---- tests/safety/test_volkswagen.py | 55 ++++++++++++++++++- 4 files changed, 121 insertions(+), 46 deletions(-) diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index 4dc29120de6f36..bb184cd91f5418 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -1,18 +1,20 @@ -const int VW_MAX_STEER = 250; // 2.5 nm -const int VW_MAX_RT_DELTA = 75; // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 125 ; 125 * 1.5 for safety pad = 75 -const uint32_t VW_RT_INTERVAL = 250000; // 250ms between real time checks -const int VW_MAX_RATE_UP = 4; // 5.0 nm/s available rate of change from the steering rack -const int VW_MAX_RATE_DOWN = 10; // 5.0 nm/s available rate of change from the steering rack -const int VW_DRIVER_TORQUE_ALLOWANCE = 80; -const int VW_DRIVER_TORQUE_FACTOR = 3; - -struct sample_t vw_torque_driver; // last few driver torques measured -int vw_rt_torque_last = 0; -int vw_desired_torque_last = 0; -uint32_t vw_ts_last = 0; +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_DRIVER_TORQUE_ALLOWANCE = 80; +const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3; + +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; // 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 @@ -23,6 +25,7 @@ static void volkswagen_init(int16_t param) { UNUSED(param); // May use param in the future to indicate MQB vs PQ35/PQ46/NMS vs MLB, or wiring configuration. controls_allowed = 0; } + static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -36,20 +39,30 @@ static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { torque_driver_new *= -1; } - update_sample(&vw_torque_driver, torque_driver_new); + 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; + 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) && long_controls_allowed) { + controls_allowed = 0; + } + volkswagen_gas_prev = gas; + } } 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; // Safety check for HCA_01 Heading Control Assist torque. @@ -67,22 +80,22 @@ static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (controls_allowed) { // *** global torque limit check *** - violation |= max_limit_check(desired_torque, VW_MAX_STEER, -VW_MAX_STEER); + violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER); // *** torque rate limit check *** - violation |= driver_limit_check(desired_torque, vw_desired_torque_last, &vw_torque_driver, - VW_MAX_STEER, VW_MAX_RATE_UP, VW_MAX_RATE_DOWN, - VW_DRIVER_TORQUE_ALLOWANCE, VW_DRIVER_TORQUE_FACTOR); - vw_desired_torque_last = desired_torque; + 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, vw_rt_torque_last, VW_MAX_RT_DELTA); + 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, vw_ts_last); - if (ts_elapsed > VW_RT_INTERVAL) { - vw_rt_torque_last = desired_torque; - vw_ts_last = ts; + 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; } } @@ -93,9 +106,9 @@ static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - vw_desired_torque_last = 0; - vw_rt_torque_last = 0; - vw_ts_last = ts; + volkswagen_desired_torque_last = 0; + volkswagen_rt_torque_last = 0; + volkswagen_ts_last = ts; } if (violation) { @@ -103,6 +116,15 @@ static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } } + // FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off. + // This avoids unintended engagements while still allowing resume spam + if ((bus == 2) && (addr == MSG_GRA_ACC_01) && !controls_allowed) { + // disallow resume and set: bits 16 and 19 + if ((GET_BYTE(to_send, 2) & 0x9) != 0) { + tx = 0; + } + } + // 1 allows the message through return tx; } @@ -113,18 +135,13 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { // NOTE: Will need refactoring for other bus layouts, such as no-forwarding at camera or J533 running-gear CAN - switch(bus_num) { + switch (bus_num) { case 0: - if(addr == MSG_GRA_ACC_01) { - // OP intercepts, filters, and updates the cruise-control button messages before they reach the ACC radar. - bus_fwd = -1; - } else { - // Forward all remaining traffic from J533 gateway to Extended CAN devices. - bus_fwd = 2; - } + // Forward all traffic from J533 gateway to Extended CAN devices. + bus_fwd = 2; break; case 2: - if((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) { + 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. bus_fwd = -1; } else { diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 87fb0db1da9c72..819fcada37e3d2 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -100,6 +100,7 @@ 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 cffc94906e6d60..c61f64077c63b1 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -163,8 +163,8 @@ void set_subaru_torque_driver(int min, int max){ } void set_volkswagen_torque_driver(int min, int max){ - vw_torque_driver.min = min; - vw_torque_driver.max = max; + volkswagen_torque_driver.min = min; + volkswagen_torque_driver.max = max; } int get_chrysler_torque_meas_min(void){ @@ -212,7 +212,7 @@ void set_subaru_rt_torque_last(int t){ } void set_volkswagen_rt_torque_last(int t){ - vw_rt_torque_last = t; + volkswagen_rt_torque_last = t; } void set_toyota_desired_torque_last(int t){ @@ -240,7 +240,11 @@ void set_subaru_desired_torque_last(int t){ } void set_volkswagen_desired_torque_last(int t){ - vw_desired_torque_last = t; + volkswagen_desired_torque_last = t; +} + +int get_volkswagen_gas_prev(void){ + return volkswagen_gas_prev; } bool get_honda_moving(void){ @@ -338,11 +342,11 @@ void init_tests_subaru(void){ void init_tests_volkswagen(void){ init_tests(); - vw_torque_driver.min = 0; - vw_torque_driver.max = 0; - vw_desired_torque_last = 0; - vw_rt_torque_last = 0; - vw_ts_last = 0; + volkswagen_torque_driver.min = 0; + volkswagen_torque_driver.max = 0; + volkswagen_desired_torque_last = 0; + volkswagen_rt_torque_last = 0; + volkswagen_ts_last = 0; set_timer(0); } diff --git a/tests/safety/test_volkswagen.py b/tests/safety/test_volkswagen.py index cc341ae8489f70..aa535cdac9cbf1 100644 --- a/tests/safety/test_volkswagen.py +++ b/tests/safety/test_volkswagen.py @@ -58,6 +58,26 @@ def _torque_msg(self, torque): to_send[0].RDLR |= 0x1 << 31 return to_send + def _gas_msg(self, gas): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x121 << 21 + to_send[0].RDLR = (gas & 0xFF) << 12 + + return to_send + + def _button_msg(self, bit): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x12B << 21 + to_send[0].RDLR = 1 << bit + to_send[0].RDTR = 2 << 4 + + return to_send + + 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()) @@ -78,6 +98,27 @@ 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): + for long_controls_allowed in [0, 1]: + self.safety.set_long_controls_allowed(long_controls_allowed) + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._gas_msg(1)) + if long_controls_allowed: + self.assertFalse(self.safety.get_controls_allowed()) + else: + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_long_controls_allowed(True) + + 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): @@ -94,6 +135,18 @@ def test_manually_enable_controls_allowed(self): self.safety.set_controls_allowed(0) self.assertFalse(self.safety.get_controls_allowed()) + 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) @@ -175,7 +228,7 @@ def test_realtime_limits(self): def test_fwd_hook(self): buss = list(range(0x0, 0x2)) msgs = list(range(0x1, 0x800)) - blocked_msgs_0to2 = [0x12B] + blocked_msgs_0to2 = [] blocked_msgs_2to0 = [0x122, 0x397] for b in buss: for m in msgs: