Merge panda subtree

panda/VERSION

@@ -1 +1 @@
-v1.1.6
+v1.1.7

panda/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},

panda/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,
+};
+

panda/board/safety/safety_honda.h

@@ -138,15 +138,6 @@ static void honda_init(int16_t param) {
   honda_alt_brake_msg = false;
 }
 
-const safety_hooks honda_hooks = {
-  .init = honda_init,
-  .rx = honda_rx_hook,
-  .tx = honda_tx_hook,
-  .tx_lin = nooutput_tx_lin_hook,
-  .ignition = default_ign_hook,
-  .fwd = nooutput_fwd_hook,
-};
-
 static void honda_bosch_init(int16_t param) {
   controls_allowed = 0;
   bosch_hardware = true;
@@ -154,6 +145,22 @@ static void honda_bosch_init(int16_t param) {
   honda_alt_brake_msg = param == 1 ? true : false;
 }
 
+static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  // fwd from car to camera. also fwd certain msgs from camera to car
+  // 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX,
+  // 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud,
+  // 0x39f is radar hud
+  int addr = to_fwd->RIR>>21;
+  if (bus_num == 0) {
+    return 2;
+  } else if (bus_num == 2 && addr != 0xE4 && addr != 0x194 && addr != 0x1FA &&
+             addr != 0x30C && addr != 0x33D && addr != 0x39F) {
+    return 0;
+  }
+
+  return -1;
+}
+
 static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   if (bus_num == 1 || bus_num == 2) {
     int addr = to_fwd->RIR>>21;
@@ -162,6 +169,15 @@ static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   return -1;
 }
 
+const safety_hooks honda_hooks = {
+  .init = honda_init,
+  .rx = honda_rx_hook,
+  .tx = honda_tx_hook,
+  .tx_lin = nooutput_tx_lin_hook,
+  .ignition = default_ign_hook,
+  .fwd = honda_fwd_hook,
+};
+
 const safety_hooks honda_bosch_hooks = {
   .init = honda_bosch_init,
   .rx = honda_rx_hook,

panda/board/safety/safety_toyota.h

@@ -1,5 +1,5 @@
-int toyota_no_dsu_car = 0;                // ch-r and camry don't have the DSU
 int toyota_giraffe_switch_1 = 0;          // is giraffe switch 1 high?
+int toyota_camera_forwarded = 0;          // should we forward the camera bus?
 
 // global torque limit
 const int TOYOTA_MAX_TORQUE = 1500;       // max torque cmd allowed ever
@@ -49,8 +49,8 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   // enter controls on rising edge of ACC, exit controls on ACC off
   if ((to_push->RIR>>21) == 0x1D2) {
-    // 4 bits: 55-52
-    int cruise_engaged = to_push->RDHR & 0xF00000;
+    // 5th bit is CRUISE_ACTIVE
+    int cruise_engaged = to_push->RDLR & 0x20;
     if (cruise_engaged && !toyota_cruise_engaged_last) {
       controls_allowed = 1;
     } else if (!cruise_engaged) {
@@ -60,16 +60,15 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   }
 
   int bus = (to_push->RDTR >> 4) & 0xF;
-  // 0x680 is a radar msg only found in dsu-less cars
-  if ((to_push->RIR>>21) == 0x680 && (bus == 1)) {
-    toyota_no_dsu_car = 1;
+  // msgs are only on bus 2 if panda is connected to frc
+  if (bus == 2) {
+    toyota_camera_forwarded = 1;
   }
 
   // 0x2E4 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high
   if ((to_push->RIR>>21) == 0x2E4 && (bus == 0)) {
     toyota_giraffe_switch_1 = 1;
   }
-
 }
 
 static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
@@ -150,13 +149,15 @@ static void toyota_init(int16_t param) {
   controls_allowed = 0;
   toyota_actuation_limits = 1;
   toyota_giraffe_switch_1 = 0;
+  toyota_camera_forwarded = 0;
   toyota_dbc_eps_torque_factor = param;
 }
 
 static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
-  // forward cam to radar and viceversa if car is dsu-less, except lkas cmd and hud
-  if ((bus_num == 0 || bus_num == 2) && toyota_no_dsu_car && !toyota_giraffe_switch_1) {
+  // forward cam to radar and viceversa if car, except lkas cmd and hud
+  // don't forward when switch 1 is high
+  if ((bus_num == 0 || bus_num == 2) && toyota_camera_forwarded && !toyota_giraffe_switch_1) {
     int addr = to_fwd->RIR>>21;
     bool is_lkas_msg = (addr == 0x2E4 || addr == 0x412) && bus_num == 2;
     return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2);
@@ -177,6 +178,7 @@ static void toyota_nolimits_init(int16_t param) {
   controls_allowed = 0;
   toyota_actuation_limits = 0;
   toyota_giraffe_switch_1 = 0;
+  toyota_camera_forwarded = 0;
   toyota_dbc_eps_torque_factor = param;
 }
 

panda/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

panda/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)

panda/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){
 }

panda/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()

panda/tests/safety/test_toyota.py

@@ -129,7 +129,7 @@ class TestToyotaSafety(unittest.TestCase):
   def test_enable_control_allowed_from_cruise(self):
     to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
     to_push[0].RIR = 0x1D2 << 21
-    to_push[0].RDHR = 0xF00000
+    to_push[0].RDLR = 0x20
 
     self.safety.toyota_rx_hook(to_push)
     self.assertTrue(self.safety.get_controls_allowed())
@@ -137,7 +137,7 @@ class TestToyotaSafety(unittest.TestCase):
   def test_disable_control_allowed_from_cruise(self):
     to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
     to_push[0].RIR = 0x1D2 << 21
-    to_push[0].RDHR = 0
+    to_push[0].RDLR = 0
 
     self.safety.set_controls_allowed(1)
     self.safety.toyota_rx_hook(to_push)
@@ -373,7 +373,7 @@ class TestToyotaSafety(unittest.TestCase):
         self.assertTrue(self.safety.get_controls_allowed())
 
         # now inject too high rates
-        self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * 
+        self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) *
                                                                                   (max_delta_up + 1), 1)))
         self.assertFalse(self.safety.get_controls_allowed())
         self.safety.set_controls_allowed(1)
@@ -381,7 +381,7 @@ class TestToyotaSafety(unittest.TestCase):
         self.assertTrue(self.safety.get_controls_allowed())
         self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1)))
         self.assertTrue(self.safety.get_controls_allowed())
-        self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * 
+        self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) *
                                                                                   (max_delta_down + 1), 1)))
         self.assertFalse(self.safety.get_controls_allowed())