From dad5858b8cfdb26962b22dd133b0ff4643eace5f Mon Sep 17 00:00:00 2001 From: rbiasini Date: Wed, 26 Feb 2020 18:19:18 -0800 Subject: [PATCH] Chrysler: add brakepress cancellation (#451) --- board/safety/safety_chrysler.h | 12 ++++++++++++ tests/safety/test_chrysler.py | 33 +++++++++++++++++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 7914ead..d969a4f 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -5,6 +5,7 @@ const int CHRYSLER_MAX_RATE_UP = 3; const int CHRYSLER_MAX_RATE_DOWN = 3; const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor const int CHRYSLER_GAS_THRSLD = 30; // 7% more than 2m/s +const int CHRYSLER_STANDSTILL_THRSLD = 10; // about 1m/s const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}}; // TODO: do checksum and counter checks @@ -13,6 +14,7 @@ AddrCheckStruct chrysler_rx_checks[] = { {.addr = {514}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, {.addr = {500}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, {.addr = {308}, .bus = 0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, + {.addr = {320}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, }; const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]); @@ -20,6 +22,7 @@ int chrysler_rt_torque_last = 0; int chrysler_desired_torque_last = 0; int chrysler_cruise_engaged_last = 0; bool chrysler_gas_prev = false; +bool chrysler_brake_prev = false; int chrysler_speed = 0; uint32_t chrysler_ts_last = 0; struct sample_t chrysler_torque_meas; // last few torques measured @@ -113,6 +116,15 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { chrysler_gas_prev = gas; } + // exit controls on rising edge of brake press + if ((addr == 320) && (bus == 0)) { + bool brake = (GET_BYTE(to_push, 0) & 0x7) == 5; + if (brake && (!chrysler_brake_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) { + controls_allowed = 0; + } + chrysler_brake_prev = brake; + } + // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) { relay_malfunction = true; diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index 0e7b716..b2e87e0 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -50,6 +50,7 @@ class TestChryslerSafety(unittest.TestCase): cls.cnt_torque_meas = 0 cls.cnt_gas = 0 cls.cnt_cruise = 0 + cls.cnt_brake = 0 def _button_msg(self, buttons): to_send = make_msg(0, 571) @@ -78,6 +79,14 @@ class TestChryslerSafety(unittest.TestCase): self.cnt_gas += 1 return to_send + def _brake_msg(self, brake): + to_send = make_msg(0, 320) + to_send[0].RDLR = 5 if brake else 0 + to_send[0].RDHR |= (self.cnt_brake % 16) << 20 + to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24 + self.cnt_brake += 1 + return to_send + def _set_prev_torque(self, t): self.safety.set_chrysler_desired_torque_last(t) self.safety.set_chrysler_rt_torque_last(t) @@ -139,6 +148,30 @@ class TestChryslerSafety(unittest.TestCase): self.safety.safety_rx_hook(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_brake_disable(self): + self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + self.safety.safety_rx_hook(self._brake_msg(True)) + self.safety.safety_rx_hook(self._speed_msg(1)) + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + self.safety.safety_rx_hook(self._speed_msg(1)) + self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + def test_non_realtime_limit_up(self): self.safety.set_controls_allowed(True)