diff --git a/board/safety.h b/board/safety.h index 22c9721..b52171e 100644 --- a/board/safety.h +++ b/board/safety.h @@ -39,6 +39,7 @@ #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; @@ -201,6 +202,7 @@ 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_MQB, &volkswagen_mqb_hooks}, {SAFETY_NOOUTPUT, &nooutput_hooks}, diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 505b4b8..bdd7ef9 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -8,65 +8,85 @@ const int SUBARU_MAX_RATE_DOWN = 70; const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60; const int SUBARU_DRIVER_TORQUE_FACTOR = 10; -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 = { 0x40, 0x140}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {0x119, 0x371}, .bus = 0, .expected_timestep = 20000U}, - {.addr = {0x240, 0x144}, .bus = 0, .expected_timestep = 50000U}, + {.addr = { 0x40}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {0x119}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {0x240}, .bus = 0, .expected_timestep = 50000U}, +}; +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; uint32_t subaru_ts_last = 0; bool subaru_gas_last = false; +bool subaru_global = false; struct sample_t subaru_torque_driver; // last few driver torques measured 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, + NULL, NULL, NULL); + } 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); 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); - 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; - int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1); - if (cruise_engaged && !subaru_cruise_engaged_last) { - controls_allowed = 1; + if (bus == 0) { + if (((addr == 0x119) && subaru_global) || + ((addr == 0x371) && !subaru_global)) { + int bit_shift = subaru_global ? 16 : 29; + int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF); + torque_driver_new = to_signed(torque_driver_new, 11); + update_sample(&subaru_torque_driver, torque_driver_new); } - if (!cruise_engaged) { - controls_allowed = 0; - } - subaru_cruise_engaged_last = cruise_engaged; - } - // 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; + // enter controls on rising edge of ACC, exit controls on ACC off + 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; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + subaru_cruise_engaged_last = cruise_engaged; } - subaru_gas_last = gas; - } - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) { - relay_malfunction = true; + // 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 = 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) && + (((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) { + relay_malfunction = true; + } } } return valid; @@ -77,7 +97,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; } @@ -86,8 +107,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; @@ -151,7 +173,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 } @@ -161,8 +185,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, @@ -170,3 +208,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/python/__init__.py b/python/__init__.py index 25f7d62..a137174 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -130,6 +130,7 @@ class Panda(object): 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/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index bb371cd..35eb5a5 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -88,6 +88,7 @@ void set_chrysler_torque_meas(int min, int max); void init_tests_subaru(void); void set_subaru_desired_torque_last(int t); void set_subaru_rt_torque_last(int t); +bool get_subaru_global(void); void set_subaru_torque_driver(int min, int max); void init_tests_volkswagen(void); diff --git a/tests/safety/test.c b/tests/safety/test.c index 4cec596..50f0ee5 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -109,6 +109,10 @@ 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; } diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index 0f0dbcb..6fa71d0 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -15,7 +15,8 @@ RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 60; DRIVER_TORQUE_FACTOR = 10; -TX_MSGS = [[0x122, 0], [0x164, 0], [0x221, 0], [0x322, 0]] +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: @@ -42,41 +43,58 @@ class TestSubaruSafety(unittest.TestCase): 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) + else: + to_send = make_msg(0, 0x371) + to_send[0].RDLR = ((t & 0x7FF) << 29) 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): - to_send = make_msg(0, 0x40) - to_send[0].RDHR = gas & 0xFF + if self.safety.get_subaru_global(): + to_send = make_msg(0, 0x40) + to_send[0].RDHR = gas & 0xFF + 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 + else: + to_send = make_msg(0, 0x144) + to_send[0].RDHR = cruise << 17 return to_send def test_spam_can_buses(self): - test_spam_can_buses(self, TX_MSGS) + 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) + 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): @@ -180,7 +198,7 @@ class TestSubaruSafety(unittest.TestCase): 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: @@ -193,6 +211,12 @@ class TestSubaruSafety(unittest.TestCase): # 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()