From ccf75c456f4450f40eba86ef5206d0c18677c16a Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Wed, 19 Feb 2020 15:37:38 -0500 Subject: [PATCH] Volkswagen safety updates: Phase 1 (#444) * Checkpoint Panda refactoring updates * Rename MQB safety tests in preparation for PQ * Refactor MQB tests, add missing torque test * Bring in MQB init without CRC LUT setup * Fix to ACC_06 test case * Fix to ACC_06 test case * Tweak comment for clarity * Drop superfluous return --- board/safety.h | 4 +- board/safety/safety_volkswagen.h | 227 +++++++++--------- python/__init__.py | 2 +- tests/safety/libpandasafety_py.py | 4 +- tests/safety/test.c | 8 + ...t_volkswagen.py => test_volkswagen_mqb.py} | 133 ++++++---- tests/safety_replay/test_safety_replay.py | 2 +- 7 files changed, 218 insertions(+), 162 deletions(-) rename tests/safety/{test_volkswagen.py => test_volkswagen_mqb.py} (56%) diff --git a/board/safety.h b/board/safety.h index 7fd057b..b4c94ef 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_volkswagen.h b/board/safety/safety_volkswagen.h index 102cb22..af15a9f 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 int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static void volkswagen_mqb_init(int16_t param) { + UNUSED(param); - bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN, + 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_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); + 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 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_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; + } - // 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 + // 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; + } - // 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; - } + // 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]))) { + if (!msg_allowed(addr, bus, VOLKSWAGEN_MQB_TX_MSGS, VOLKSWAGEN_MQB_TX_MSGS_LEN) || relay_malfunction) { tx = 0; } - if (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 ea8dea1..ea2b2d3 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 77131c1..4dfaef1 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -91,10 +91,12 @@ void set_subaru_rt_torque_last(int t); 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 59611d3..8de48f6 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; } 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 db58cdc..9f5b461 100644 --- a/tests/safety/test_volkswagen.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -3,19 +3,27 @@ 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 +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 @@ class TestVolkswagenSafety(unittest.TestCase): 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 @@ class TestVolkswagenSafety(unittest.TestCase): 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 @@ class TestVolkswagenSafety(unittest.TestCase): 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 @@ class TestVolkswagenSafety(unittest.TestCase): 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 @@ class TestVolkswagenSafety(unittest.TestCase): 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 fd36b24..1120827 100755 --- a/tests/safety_replay/test_safety_replay.py +++ b/tests/safety_replay/test_safety_replay.py @@ -18,7 +18,7 @@ logs = [ ("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__":