Chrysler safety controls (#130)

* Chrysler safety model and tests.

* fix comments

* when ACC is canceled, disallow controls

* update SAFETY_CHRYSLER value in Python API and add other SAFETY values

board/safety.h

@@ -62,6 +62,7 @@ int controls_allowed = 0;
 #include "safety/safety_ford.h"
 #include "safety/safety_cadillac.h"
 #include "safety/safety_hyundai.h"
+#include "safety/safety_chrysler.h"
 #include "safety/safety_elm327.h"
 
 const safety_hooks *current_hooks = &nooutput_hooks;
@@ -102,6 +103,7 @@ typedef struct {
 #define SAFETY_CADILLAC 6
 #define SAFETY_HYUNDAI 7
 #define SAFETY_TESLA 8
+#define SAFETY_CHRYSLER 9
 #define SAFETY_TOYOTA_IPAS 0x1335
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
@@ -116,6 +118,7 @@ const safety_hook_config safety_hook_registry[] = {
   {SAFETY_FORD, &ford_hooks},
   {SAFETY_CADILLAC, &cadillac_hooks},
   {SAFETY_HYUNDAI, &hyundai_hooks},
+  {SAFETY_CHRYSLER, &chrysler_hooks},
   {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
 #ifdef PANDA
   {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},

board/safety/safety_chrysler.h

@@ -0,0 +1,136 @@
+// board enforces
+//   in-state
+//      ACC is active (green)
+//   out-state
+//      brake pressed
+//      stock LKAS ECU is online
+//      ACC is not active (white or disabled)
+
+// chrysler_: namespacing
+int chrysler_speed = 0;
+
+// silence everything if stock ECUs are still online
+int chrysler_lkas_detected = 0;
+int chrysler_desired_torque_last = 0;
+
+static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+  int bus_number = (to_push->RDTR >> 4) & 0xFF;
+  uint32_t addr;
+  if (to_push->RIR & 4) {
+    // Extended
+    // Not looked at, but have to be separated
+    // to avoid address collision
+    addr = to_push->RIR >> 3;
+  } else {
+    // Normal
+    addr = to_push->RIR >> 21;
+  }
+
+  if (addr == 0x144 && bus_number == 0) {
+    chrysler_speed = ((to_push->RDLR & 0xFF000000) >> 16) | (to_push->RDHR & 0xFF);
+  }
+
+  // check if stock LKAS ECU is still online
+  if (addr == 0x292 && bus_number == 0) {
+    chrysler_lkas_detected = 1;
+    controls_allowed = 0;
+  }
+
+  // ["ACC_2"]['ACC_STATUS_2'] == 7 for active (green) Adaptive Cruise Control
+  if (addr == 0x1f4 && bus_number == 0) {
+    if (((to_push->RDLR & 0x380000) >> 19) == 7) {
+      controls_allowed = 1;
+    } else {
+      controls_allowed = 0;
+    }
+  }
+
+  // exit controls on brake press by human
+  if (addr == 0x140) {
+    if (to_push->RDLR & 0x4) {
+      controls_allowed = 0;
+    }
+  }
+}
+
+// if controls_allowed
+//     allow steering up to limit
+// else
+//     block all commands that produce actuation
+static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+  // There can be only one! (LKAS)
+  if (chrysler_lkas_detected) {
+    return 0;
+  }
+
+  uint32_t addr;
+  if (to_send->RIR & 4) {
+    // Extended
+    addr = to_send->RIR >> 3;
+  } else {
+    // Normal
+    addr = to_send->RIR >> 21;
+  }
+
+  // LKA STEER: Too large of values cause the steering actuator ECU to silently
+  // fault and no longer actuate the wheel until the car is rebooted.
+  if (addr == 0x292) {
+    int rdlr = to_send->RDLR;
+    int straight = 1024;
+    int steer = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8) - straight;
+    int max_steer = 230;
+    int max_rate = 50; // ECU is fine with 100, but 3 is typical.
+    if (steer > max_steer) {
+      return false;
+    }
+    if (steer < -max_steer) {
+      return false;
+    }
+    if (!controls_allowed && steer != 0) {
+      // If controls not allowed, only allow steering to move closer to 0.
+      if (chrysler_desired_torque_last == 0) {
+	return false;
+      }
+      if ((chrysler_desired_torque_last > 0) && (steer >= chrysler_desired_torque_last)) {
+	return false;
+      }
+      if ((chrysler_desired_torque_last < 0) && (steer <= chrysler_desired_torque_last)) {
+	return false;
+      }
+    }
+    if (steer < (chrysler_desired_torque_last - max_rate)) {
+      return false;
+    }
+    if (steer > (chrysler_desired_torque_last + max_rate)) {
+      return false;
+    }
+    
+    chrysler_desired_torque_last = steer;
+  }
+
+  // 1 allows the message through
+  return true;
+}
+
+static int chrysler_tx_lin_hook(int lin_num, uint8_t *data, int len) {
+  // LIN is not used.
+  return false;
+}
+
+static void chrysler_init(int16_t param) {
+  controls_allowed = 0;
+}
+
+static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
+const safety_hooks chrysler_hooks = {
+  .init = chrysler_init,
+  .rx = chrysler_rx_hook,
+  .tx = chrysler_tx_hook,
+  .tx_lin = chrysler_tx_lin_hook,
+  .ignition = default_ign_hook,
+  .fwd = chrysler_fwd_hook,
+};
+

python/__init__.py

@@ -105,7 +105,14 @@ class Panda(object):
   SAFETY_NOOUTPUT = 0
   SAFETY_HONDA = 1
   SAFETY_TOYOTA = 2
+  SAFETY_GM = 3
   SAFETY_HONDA_BOSCH = 4
+  SAFETY_FORD = 5
+  SAFETY_CADILLAC = 6
+  SAFETY_HYUNDAI = 7
+  SAFETY_TESLA = 8
+  SAFETY_CHRYSLER = 9
+  SAFETY_TOYOTA_IPAS = 0x1335
   SAFETY_TOYOTA_NOLIMITS = 0x1336
   SAFETY_ALLOUTPUT = 0x1337
   SAFETY_ELM327 = 0xE327

tests/safety/libpandasafety_py.py

@@ -81,6 +81,12 @@ void set_hyundai_rt_torque_last(int t);
 void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
 int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
 
+void init_tests_chrysler(void);
+void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
+int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
+void chrysler_init(int16_t param);
+void set_chrysler_desired_torque_last(int t);
+
 """)
 
 libpandasafety = ffi.dlopen(libpandasafety_fn)

tests/safety/test.c

@@ -121,6 +121,10 @@ void set_hyundai_desired_torque_last(int t){
   hyundai_desired_torque_last = t;
 }
 
+void set_chrysler_desired_torque_last(int t){
+  chrysler_desired_torque_last = t;
+}
+
 int get_ego_speed(void){
   return ego_speed;
 }
@@ -184,6 +188,10 @@ void init_tests_honda(void){
   gas_prev = 0;
 }
 
+void init_tests_chrysler(void){
+  chrysler_desired_torque_last = 0;
+  set_timer(0);
+}
 
 void set_gmlan_digital_output(int to_set){
 }

tests/safety/test_chrysler.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python2
+import unittest
+import numpy as np
+import libpandasafety_py
+
+
+class TestChryslerSafety(unittest.TestCase):
+  @classmethod
+  def setUp(cls):
+    cls.safety = libpandasafety_py.libpandasafety
+    cls.safety.chrysler_init(0)
+    cls.safety.init_tests_chrysler()
+
+  def _acc_msg(self, active):
+    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
+    to_send[0].RIR = 0x1f4 << 21
+    to_send[0].RDLR = 0xfe3fff1f if active else 0xfe0fff1f
+    return to_send
+    
+
+  def _brake_msg(self, brake):
+    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
+    to_send[0].RIR = 0x140 << 21
+    to_send[0].RDLR = 0x485 if brake else 0x480
+    return to_send
+
+  def _steer_msg(self, angle):
+    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
+    to_send[0].RIR = 0x292 << 21
+    c_angle = (1024 + angle)
+    to_send[0].RDLR = 0x10 | ((c_angle & 0xf00) >> 8) | ((c_angle & 0xff) << 8)
+    return to_send
+
+  def test_default_controls_not_allowed(self):
+    self.assertFalse(self.safety.get_controls_allowed())
+
+  def test_acc_enables_controls(self):
+    self.safety.set_controls_allowed(0)
+    self.safety.chrysler_rx_hook(self._acc_msg(0))
+    self.assertFalse(self.safety.get_controls_allowed())
+    self.safety.chrysler_rx_hook(self._acc_msg(1))
+    self.assertTrue(self.safety.get_controls_allowed())
+    self.safety.chrysler_rx_hook(self._acc_msg(0))
+    self.assertFalse(self.safety.get_controls_allowed())
+
+  def test_disengage_on_brake(self):
+    self.safety.set_controls_allowed(1)
+    self.safety.chrysler_rx_hook(self._brake_msg(0))
+    self.assertTrue(self.safety.get_controls_allowed())
+    self.safety.chrysler_rx_hook(self._brake_msg(1))
+    self.assertFalse(self.safety.get_controls_allowed())
+
+  def test_steer_calculation(self):
+    self.assertEqual(0x14, self._steer_msg(0)[0].RDLR)  # straight, no steering
+
+  def test_steer_tx(self):
+    self.safety.set_controls_allowed(1)
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0)))
+    self.safety.set_chrysler_desired_torque_last(227)
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(230)))
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(231)))
+    self.safety.set_chrysler_desired_torque_last(-227)
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(-231)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(-230)))
+    # verify max change
+    self.safety.set_chrysler_desired_torque_last(0)
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(230)))
+    
+    self.safety.set_controls_allowed(0)
+    self.safety.set_chrysler_desired_torque_last(0)
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(3)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0)))
+    # verify when controls not allowed we can still go back towards 0
+    self.safety.set_chrysler_desired_torque_last(10)
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(10)))
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(11)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(7)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(4)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0)))
+    self.safety.set_chrysler_desired_torque_last(-10)
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(-10)))
+    self.assertFalse(self.safety.chrysler_tx_hook(self._steer_msg(-11)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(-7)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(-4)))
+    self.assertTrue(self.safety.chrysler_tx_hook(self._steer_msg(0)))
+
+if __name__ == "__main__":
+  unittest.main()