diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 37059b8..299e6f1 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -7,6 +7,7 @@ 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}, {0x221, 0}, {0x322, 0}}; const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}}; @@ -16,6 +17,8 @@ const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_M AddrCheckStruct subaru_rx_checks[] = { {.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 @@ -30,8 +33,10 @@ const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_r 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_gas_last = false; +bool subaru_brake_last = false; bool subaru_global = false; struct sample_t subaru_torque_driver; // last few driver torques measured @@ -92,6 +97,22 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { subaru_cruise_engaged_last = cruise_engaged; } + // 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 = (GET_BYTES_48(to_push) & 0xFFF0) > 0; + if (brake && (!subaru_brake_last || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) { + controls_allowed = 0; + } + subaru_brake_last = brake; + } + // exit controls on rising edge of gas press if (((addr == 0x40) && subaru_global) || ((addr == 0x140) && !subaru_global)) { diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index 4fbe337..4ae4bd2 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -15,6 +15,8 @@ RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 60; DRIVER_TORQUE_FACTOR = 10; +SPEED_THRESHOLD = 20 # 1kph (see dbc file) + TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]] TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]] @@ -44,6 +46,8 @@ class TestSubaruSafety(unittest.TestCase): cnt_gas = 0 cnt_torque_driver = 0 cnt_cruise = 0 + cnt_speed = 0 + cnt_brake = 0 @classmethod def setUp(cls): @@ -69,6 +73,24 @@ class TestSubaruSafety(unittest.TestCase): 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): t = twos_comp(torque, 13) if self.safety.get_subaru_global(): @@ -133,6 +155,33 @@ class TestSubaruSafety(unittest.TestCase): self.safety.safety_rx_hook(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_allow_brake_at_zero_speed(self): + # Brake was already pressed + if (self.safety.get_subaru_global()): + self.safety.safety_rx_hook(self._brake_msg(1)) + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._brake_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._brake_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + # rising edge of brake should disengage + self.safety.safety_rx_hook(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._brake_msg(0)) # reset no brakes + + def test_not_allow_brake_when_moving(self): + # Brake was already pressed + if (self.safety.get_subaru_global()): + self.safety.safety_rx_hook(self._brake_msg(1)) + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._speed_msg(SPEED_THRESHOLD)) + self.safety.safety_rx_hook(self._brake_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._speed_msg(SPEED_THRESHOLD + 1)) + self.safety.safety_rx_hook(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._speed_msg(0)) + def test_steer_safety_check(self): for enabled in [0, 1]: for t in range(-3000, 3000):