From 04809e1329b1831b0b51ea85c4b01ff1d2af506d Mon Sep 17 00:00:00 2001 From: rbiasini Date: Sat, 7 Mar 2020 21:26:03 -0800 Subject: [PATCH] Hyundai brake check (#464) * WIP: adding brake check * Added Hyundai brake tests --- board/safety/safety_hyundai.h | 23 ++++++++++++++++++++- tests/safety/test_hyundai.py | 38 +++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index e3d38b0..5186508 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -1,16 +1,19 @@ const int HYUNDAI_MAX_STEER = 255; // like stock const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks -const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks +const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks const int HYUNDAI_MAX_RATE_UP = 3; const int HYUNDAI_MAX_RATE_DOWN = 7; const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50; const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2; +const int HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}}; // TODO: do checksum and counter checks AddrCheckStruct hyundai_rx_checks[] = { {.addr = {608}, .bus = 0, .expected_timestep = 10000U}, {.addr = {897}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {902}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {916}, .bus = 0, .expected_timestep = 10000U}, {.addr = {1057}, .bus = 0, .expected_timestep = 20000U}, }; const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]); @@ -18,7 +21,9 @@ const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_c int hyundai_rt_torque_last = 0; int hyundai_desired_torque_last = 0; int hyundai_cruise_engaged_last = 0; +int hyundai_speed = 0; bool hyundai_gas_last = false; +bool hyundai_brake_last = false; uint32_t hyundai_ts_last = 0; struct sample_t hyundai_torque_driver; // last few driver torques measured @@ -58,6 +63,22 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { hyundai_gas_last = gas; } + // sample subaru wheel speed, averaging opposite corners + if (addr == 902) { + hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL + hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL + hyundai_speed /= 2; + } + + // exit controls on rising edge of brake press + if (addr == 916) { + bool brake = (GET_BYTE(to_push, 6) >> 7) != 0; + if (brake && (!hyundai_brake_last || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) { + controls_allowed = 0; + } + hyundai_brake_last = brake; + } + // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) { relay_malfunction = true; diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 8f45a86..8dd9f9e 100644 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -15,6 +15,8 @@ RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 50; DRIVER_TORQUE_FACTOR = 2; +SPEED_THRESHOLD = 30 # ~1kph + TX_MSGS = [[832, 0], [1265, 0]] def twos_comp(val, bits): @@ -46,6 +48,17 @@ class TestHyundaiSafety(unittest.TestCase): to_send[0].RDHR = (val & 0x3) << 30; return to_send + def _brake_msg(self, brake): + to_send = make_msg(0, 916) + to_send[0].RDHR = brake << 23; + return to_send + + def _speed_msg(self, speed): + to_send = make_msg(0, 902) + to_send[0].RDLR = speed & 0x3FFF; + to_send[0].RDHR = (speed & 0x3FFF) << 16; + return to_send + def _set_prev_torque(self, t): self.safety.set_hyundai_desired_torque_last(t) self.safety.set_hyundai_rt_torque_last(t) @@ -101,6 +114,31 @@ class TestHyundaiSafety(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 + 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 + 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_non_realtime_limit_up(self): self.safety.set_hyundai_torque_driver(0, 0) self.safety.set_controls_allowed(True)