Cadillac: added max steer safety

Author: rbiasini

board/safety.h

@@ -3,6 +3,7 @@ int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
 int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
 int safety_ignition_hook();
 uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
+int to_signed(int d, int bits);
 
 typedef void (*safety_hook_init)(int16_t param);
 typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@@ -109,3 +110,11 @@ int safety_set_mode(uint16_t mode, int16_t param) {
 uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
   return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts;
 }
+
+// convert a trimmed integer to signed 32 bit int
+int to_signed(int d, int bits) {
+  if (d >= (1 << (bits - 1))) {
+    d -= (1 << bits);
+  }
+  return d;
+}

board/safety/safety_cadillac.h

@@ -1,20 +1,49 @@
+const int STEER_MAX = 150; // 1s
 const int CADILLAC_IGNITION_TIMEOUT = 1000000; // 1s
+
 int cadillac_can_seen = 0;
+int cadillac_cruise_engaged_last = 0;
 uint32_t cadillac_ts_last = 0;
 
 static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int bus_number = (to_push->RDTR >> 4) & 0xFF;
   uint32_t addr = to_push->RIR >> 21;
 
-  if (addr == 0x135 && bus_number == 0) {
+  if ((addr == 0x135) && (bus_number == 0)) {
     cadillac_can_seen = 1;
     cadillac_ts_last = TIM2->CNT; // reset timer when gear msg is received
   }
+
+  // enter controls on rising edge of ACC, exit controls on ACC off
+  if ((addr == 0x370) && (bus_number == 0)) {
+    int cruise_engaged = to_push->RDLR & 0x800000;  // bit 23
+    if (cruise_engaged && !cadillac_cruise_engaged_last) {
+      controls_allowed = 1;
+    } else if (!cruise_engaged) {
+      controls_allowed = 0;
+    }
+    cadillac_cruise_engaged_last = cruise_engaged;
+  }
+}
+
+static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+  uint32_t addr = to_send->RIR >> 21;
+
+  // block steering cmd above 150
+  if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
+    int lkas_cmd = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
+    lkas_cmd = to_signed(lkas_cmd, 14);
+    // block message is controls are allowed and lkas command exceeds max, or
+    // if controls aren't allowed and lkas cmd isn't 0
+    if (controls_allowed && ((lkas_cmd > STEER_MAX) || (lkas_cmd < -STEER_MAX))) {
+      return 0;
+    } else if (!controls_allowed && lkas_cmd) return 0;
+  }
+  return true;
 }
 
 static void cadillac_init(int16_t param) {
   cadillac_can_seen = 0;
-  controls_allowed = 1;
 }
 
 static int cadillac_ign_hook() {
@@ -31,7 +60,7 @@ static int cadillac_ign_hook() {
 const safety_hooks cadillac_hooks = {
   .init = cadillac_init,
   .rx = cadillac_rx_hook,
-  .tx = alloutput_tx_hook,
+  .tx = cadillac_tx_hook,
   .tx_lin = alloutput_tx_lin_hook,
   .ignition = cadillac_ign_hook,
   .fwd = alloutput_fwd_hook,

board/safety/safety_toyota_ipas.h

@@ -35,13 +35,6 @@ uint32_t ts_angle_last = 0;
 
 int controls_allowed_last = 0;
 
-int to_signed(int d, int bits) {
-  if (d >= (1 << (bits - 1))) {
-    d -= (1 << bits);
-  }
-  return d;
-}
-
 // interp function that holds extreme values
 float interpolate(struct lookup_t xy, float x) {
   int size = sizeof(xy.x) / sizeof(xy.x[0]);