Chrysler: add brakepress cancellation (#451)

commaai · Feb 27, 2020 · dad5858 · dad5858
1 parent db94a5b
commit dad5858
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 0 deletions.
diff --git 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,13 +14,15 @@ 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]);
 
 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
@@ -50,6 +50,7 @@ def setUp(cls):
     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 @@ def _gas_msg(self, gas):
     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 @@ def test_gas_disable(self):
     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)