Cadillac (#119)

* added some steer safety to Cadillac

* bug fixes

* added cadillac full steering safety. To be tested

* RT checks and max checks working. Need to test driver torque based limits

* cadillac steer safety should be done

* unneded lines

board/safety/safety_cadillac.h

@@ -1,18 +1,39 @@
 const int CADILLAC_STEER_MAX = 150; // 1s
 const int CADILLAC_IGNITION_TIMEOUT = 1000000; // 1s
+// real time torque limit to prevent controls spamming
+// the real time limit is 1500/sec
+const int CADILLAC_MAX_RT_DELTA = 75;       // max delta torque allowed for real time checks
+const int32_t CADILLAC_RT_INTERVAL = 250000;    // 250ms between real time checks
+const int CADILLAC_MAX_RATE_UP = 2;
+const int CADILLAC_MAX_RATE_DOWN = 5;
+const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50;
+const int CADILLAC_DRIVER_TORQUE_FACTOR = 4;
 
 int cadillac_ign = 0;
 int cadillac_cruise_engaged_last = 0;
+uint32_t cadillac_ts_ign_last = 0;
+int cadillac_rt_torque_last = 0;
+int cadillac_desired_torque_last = 0;
 uint32_t cadillac_ts_last = 0;
 
+struct sample_t cadillac_torque_driver;         // last 3 driver torques measured
+
 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 == 356) {
+    int torque_driver_new = ((to_push->RDLR & 0x3) << 8) | ((to_push->RDLR >> 8) & 0xFF);
+    torque_driver_new = to_signed(torque_driver_new, 11);
+
+    // update array of sample
+    update_sample(&cadillac_torque_driver, torque_driver_new);
+  }
+
   // this message isn't all zeros when ignition is on
   if ((addr == 0x160) && (bus_number == 0) && to_push->RDLR) {
     cadillac_ign = 1;
-    cadillac_ts_last = TIM2->CNT; // reset timer when ign is received
+    cadillac_ts_ign_last = TIM2->CNT; // reset timer when ign is received
   }
 
   // enter controls on rising edge of ACC, exit controls on ACC off
@@ -32,14 +53,79 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   // 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 > CADILLAC_STEER_MAX) || (lkas_cmd < -CADILLAC_STEER_MAX))) {
-      return 0;
-    } else if (!controls_allowed && lkas_cmd) return 0;
+    int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
+    int violation = 0;
+    uint32_t ts = TIM2->CNT;
+    desired_torque = to_signed(desired_torque, 14);
+
+    if (controls_allowed) {
+
+      // *** global torque limit check ***
+      if ((desired_torque > CADILLAC_STEER_MAX) || (desired_torque < -CADILLAC_STEER_MAX)) {
+        violation = 1;
+      }
+
+      // *** torque rate limit check ***
+      int highest_allowed_torque = max(cadillac_desired_torque_last, 0) + CADILLAC_MAX_RATE_UP;
+      int lowest_allowed_torque = min(cadillac_desired_torque_last, 0) - CADILLAC_MAX_RATE_UP;
+
+      int driver_torque_max_limit = CADILLAC_STEER_MAX + 
+                                    (CADILLAC_DRIVER_TORQUE_ALLOWANCE + cadillac_torque_driver.max) * 
+                                    CADILLAC_DRIVER_TORQUE_FACTOR;
+      int driver_torque_min_limit = -CADILLAC_STEER_MAX + 
+                                    (-CADILLAC_DRIVER_TORQUE_ALLOWANCE + cadillac_torque_driver.max) * 
+                                    CADILLAC_DRIVER_TORQUE_FACTOR;
+
+      // if we've exceeded the applied torque, we must start moving toward 0
+      highest_allowed_torque = min(highest_allowed_torque,
+                                   max(cadillac_desired_torque_last - CADILLAC_MAX_RATE_DOWN,
+                                       max(driver_torque_max_limit, 0)));
+      lowest_allowed_torque = max(lowest_allowed_torque,
+                                  min(cadillac_desired_torque_last + CADILLAC_MAX_RATE_DOWN,
+                                      min(driver_torque_min_limit, 0)));
+
+      // check for violation
+      if ((desired_torque < lowest_allowed_torque) || (desired_torque > highest_allowed_torque)) {
+        violation = 1;
+      }
+
+      //// used next time
+      cadillac_desired_torque_last = desired_torque;
+
+      // *** torque real time rate limit check ***
+      int highest_rt_torque = max(cadillac_rt_torque_last, 0) + CADILLAC_MAX_RT_DELTA;
+      int lowest_rt_torque = min(cadillac_rt_torque_last, 0) - CADILLAC_MAX_RT_DELTA;
+
+
+      // check for violation
+      if ((desired_torque < lowest_rt_torque) || (desired_torque > highest_rt_torque)) {
+        violation = 1;
+      }
+
+      // every RT_INTERVAL set the new limits
+      uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
+      if (ts_elapsed > RT_INTERVAL) {
+        cadillac_rt_torque_last = desired_torque;
+        cadillac_ts_last = ts;
+      }
+    }
+
+    // no torque if controls is not allowed
+    if (!controls_allowed && (desired_torque != 0)) {
+      violation = 1;
+    }
+
+   // reset to 0 if either controls is not allowed or there's a violation
+   if (violation || !controls_allowed) {
+      cadillac_desired_torque_last = 0;
+      cadillac_rt_torque_last = 0;
+      cadillac_ts_last = ts;
+    }
+
+    if (violation) {
+      return false;
+    }
+
   }
   return true;
 }
@@ -50,14 +136,13 @@ static void cadillac_init(int16_t param) {
 
 static int cadillac_ign_hook() {
   uint32_t ts = TIM2->CNT;
-  uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
+  uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_ign_last);
   if (ts_elapsed > CADILLAC_IGNITION_TIMEOUT) {
     cadillac_ign = 0;
   }
   return cadillac_ign;
 }
 
-// Placeholder file, actual safety is TODO.
 const safety_hooks cadillac_hooks = {
   .init = cadillac_init,
   .rx = cadillac_rx_hook,

tests/safety/libpandasafety_py.py

@@ -39,6 +39,7 @@
 void init_tests_toyota(void);
 void set_timer(int t);
 void set_torque_meas(int min, int max);
+void set_cadillac_torque_driver(int min, int max);
 void set_rt_torque_last(int t);
 void set_desired_torque_last(int t);
 int get_torque_meas_min(void);
@@ -52,6 +53,13 @@
 int get_brake_prev(void);
 int get_gas_prev(void);
 
+void init_tests_cadillac(void);
+void cadillac_init(int16_t param);
+void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
+int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
+void set_cadillac_desired_torque_last(int t);
+void set_cadillac_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);
 

tests/safety/test.c

@@ -23,6 +23,7 @@ typedef struct
 } TIM_TypeDef;
 
 struct sample_t torque_meas;
+struct sample_t cadillac_torque_driver;
 
 TIM_TypeDef timer;
 TIM_TypeDef *TIM2 = &timer;
@@ -63,6 +64,11 @@ void set_torque_meas(int min, int max){
   torque_meas.max = max;
 }
 
+void set_cadillac_torque_driver(int min, int max){
+  cadillac_torque_driver.min = min;
+  cadillac_torque_driver.max = max;
+}
+
 int get_torque_meas_min(void){
   return torque_meas.min;
 }
@@ -75,10 +81,18 @@ void set_rt_torque_last(int t){
   rt_torque_last = t;
 }
 
+void set_cadillac_rt_torque_last(int t){
+  cadillac_rt_torque_last = t;
+}
+
 void set_desired_torque_last(int t){
   desired_torque_last = t;
 }
 
+void set_cadillac_desired_torque_last(int t){
+  cadillac_desired_torque_last = t;
+}
+
 int get_ego_speed(void){
   return ego_speed;
 }
@@ -100,6 +114,15 @@ void init_tests_toyota(void){
   set_timer(0);
 }
 
+void init_tests_cadillac(void){
+  cadillac_torque_driver.min = 0;
+  cadillac_torque_driver.max = 0;
+  cadillac_desired_torque_last = 0;
+  cadillac_rt_torque_last = 0;
+  cadillac_ts_last = 0;
+  set_timer(0);
+}
+
 void init_tests_honda(void){
   ego_speed = 0;
   gas_interceptor_detected = 0;