Chrysler: disengage on gas press (commaai#442)

* add gas cancellation for chrysler
* added regr test too
* also add bus checks to chrysler. SHould have been there already

Author: rbiasini

board/safety/safety_chrysler.h

@@ -4,18 +4,23 @@ const uint32_t CHRYSLER_RT_INTERVAL = 250000;  // 250ms between real time checks
 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 AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}};
 
 // TODO: do checksum and counter checks
 AddrCheckStruct chrysler_rx_checks[] = {
   {.addr = {544}, .bus = 0, .expected_timestep = 10000U},
+  {.addr = {514}, .bus = 0, .expected_timestep = 10000U},
   {.addr = {500}, .bus = 0, .expected_timestep = 20000U},
+  {.addr = {308}, .bus = 0, .expected_timestep = 20000U},  // verify ts
 };
 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;
+int chrysler_speed = 0;
 uint32_t chrysler_ts_last = 0;
 struct sample_t chrysler_torque_meas;         // last few torques measured
 
@@ -29,15 +34,15 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     int addr = GET_ADDR(to_push);
 
     // Measured eps torque
-    if (addr == 544) {
+    if ((addr == 544) && (bus == 0)) {
       int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;
 
       // update array of samples
       update_sample(&chrysler_torque_meas, torque_meas_new);
     }
 
     // enter controls on rising edge of ACC, exit controls on ACC off
-    if (addr == 0x1F4) {
+    if ((addr == 500) && (bus == 0)) {
       int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
       if (cruise_engaged && !chrysler_cruise_engaged_last) {
         controls_allowed = 1;
@@ -48,7 +53,21 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       chrysler_cruise_engaged_last = cruise_engaged;
     }
 
-    // TODO: add gas pressed check
+    // update speed
+    if ((addr == 514) && (bus == 0)) {
+      int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
+      int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
+      chrysler_speed = (speed_l + speed_r) / 2;
+    }
+
+    // exit controls on rising edge of gas press
+    if ((addr == 308) && (bus == 0)) {
+      bool gas = (GET_BYTE(to_push, 5) & 0x7F) != 0;
+      if (gas && !chrysler_gas_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
+        controls_allowed = 0;
+      }
+      chrysler_gas_prev = gas;
+    }
 
     // check if stock camera ECU is on bus 0
     if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {

tests/safety/test.c

@@ -312,6 +312,8 @@ void init_tests_hyundai(void){
 
 void init_tests_chrysler(void){
   init_tests();
+  chrysler_gas_prev = false;
+  chrysler_speed = 0;
   chrysler_torque_meas.min = 0;
   chrysler_torque_meas.max = 0;
   chrysler_desired_torque_last = 0;

tests/safety/test_chrysler.py

@@ -28,6 +28,18 @@ def _button_msg(self, buttons):
     to_send[0].RDLR = buttons
     return to_send
 
+  def _speed_msg(self, speed):
+    speed = int(speed / 0.071028)
+    to_send = make_msg(0, 514, 4)
+    to_send[0].RDLR = ((speed & 0xFF0) >> 4) + ((speed & 0xF) << 12) + \
+                      ((speed & 0xFF0) << 12) + ((speed & 0xF) << 28)
+    return to_send
+
+  def _gas_msg(self, gas):
+    to_send = make_msg(0, 308)
+    to_send[0].RDHR = (gas & 0x7F) << 8
+    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)
@@ -80,6 +92,16 @@ def test_disable_control_allowed_from_cruise(self):
     self.safety.safety_rx_hook(to_push)
     self.assertFalse(self.safety.get_controls_allowed())
 
+  def test_gas_disable(self):
+    self.safety.set_controls_allowed(1)
+    self.safety.safety_rx_hook(self._speed_msg(2.2))
+    self.safety.safety_rx_hook(self._gas_msg(1))
+    self.assertTrue(self.safety.get_controls_allowed())
+    self.safety.safety_rx_hook(self._gas_msg(0))
+    self.safety.safety_rx_hook(self._speed_msg(2.3))
+    self.safety.safety_rx_hook(self._gas_msg(1))
+    self.assertFalse(self.safety.get_controls_allowed())
+
   def test_non_realtime_limit_up(self):
     self.safety.set_controls_allowed(True)