added gm safety for steering (#123)

rbiasini · web-flow · commit 38a9ea9ad91f · 2018-06-13T23:13:59.000-07:00
* added gm safety for steering
* safety tests done for safety_gm
diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h
@@ -1,4 +1,4 @@
-const int CADILLAC_STEER_MAX = 150; // 1s
+const int CADILLAC_MAX_STEER = 150; // 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
@@ -14,8 +14,7 @@ int cadillac_rt_torque_last = 0;
 int cadillac_desired_torque_last[4] = {0};      // 4 torque messages
 uint32_t cadillac_ts_last = 0;
 int cadillac_supercruise_on = 0;
-
-struct sample_t cadillac_torque_driver;         // last 3 driver torques measured
+struct sample_t cadillac_torque_driver;         // last few driver torques measured
 
 int cadillac_get_torque_idx(uint32_t addr) {
   if (addr==0x151) return 0;
@@ -60,7 +59,7 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   uint32_t addr = to_send->RIR >> 21;
 
-  // block steering cmd above 150
+  // steer cmd checks
   if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
     int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
     int violation = 0;
@@ -71,12 +70,12 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     if (controls_allowed) {
 
       // *** global torque limit check ***
-      violation |= max_limit_check(desired_torque, CADILLAC_STEER_MAX);
+      violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER);
 
       // *** torque rate limit check ***
       int desired_torque_last = cadillac_desired_torque_last[idx];
       violation |= driver_limit_check(desired_torque, desired_torque_last, &cadillac_torque_driver,
-        CADILLAC_STEER_MAX, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN,
+        CADILLAC_MAX_STEER, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN,
         CADILLAC_DRIVER_TORQUE_ALLOWANCE, CADILLAC_DRIVER_TORQUE_FACTOR);
 
       // used next time
@@ -87,7 +86,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
       // every RT_INTERVAL set the new limits
       uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
-      if (ts_elapsed > RT_INTERVAL) {
+      if (ts_elapsed > CADILLAC_RT_INTERVAL) {
         cadillac_rt_torque_last = desired_torque;
         cadillac_ts_last = ts;
       }
@@ -98,8 +97,8 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       violation = 1;
     }
 
-   // reset to 0 if either controls is not allowed or there's a violation
-   if (violation || !controls_allowed) {
+    // reset to 0 if either controls is not allowed or there's a violation
+    if (violation || !controls_allowed) {
       cadillac_desired_torque_last[idx] = 0;
       cadillac_rt_torque_last = 0;
       cadillac_ts_last = ts;
diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h
@@ -8,15 +8,24 @@
 //      brake rising edge
 //      brake > 0mph
 
-// gm_: poor man's namespacing
+const int GM_MAX_STEER = 255;
+const int GM_MAX_RT_DELTA = 128;          // max delta torque allowed for real time checks
+const int32_t GM_RT_INTERVAL = 250000;    // 250ms between real time checks
+const int GM_MAX_RATE_UP = 7;
+const int GM_MAX_RATE_DOWN = 17;
+const int GM_DRIVER_TORQUE_ALLOWANCE = 50;
+const int GM_DRIVER_TORQUE_FACTOR = 4;
+
 int gm_brake_prev = 0;
 int gm_gas_prev = 0;
 int gm_speed = 0;
-
 // silence everything if stock ECUs are still online
 int gm_ascm_detected = 0;
-
 int gm_ignition_started = 0;
+int gm_rt_torque_last = 0;
+int gm_desired_torque_last = 0;
+uint32_t gm_ts_last = 0;
+struct sample_t gm_torque_driver;         // last few driver torques measured
 
 static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int bus_number = (to_push->RDTR >> 4) & 0xFF;
@@ -31,6 +40,13 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     addr = to_push->RIR >> 21;
   }
 
+  if (addr == 388) {
+    int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF);
+    torque_driver_new = to_signed(torque_driver_new, 11);
+    // update array of samples
+    update_sample(&gm_torque_driver, torque_driver_new);
+  }
+
   if (addr == 0x1f1 && bus_number == 0) {
     //Bit 5 should be ignition "on"
     //Backup plan is Bit 2 (accessory power)
@@ -136,13 +152,49 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   // LKA STEER: safety check
   if (addr == 384) {
     int rdlr = to_send->RDLR;
-    int steer = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8);
-    steer = to_signed(steer, 11);
-    int max_steer = 255;
+    int desired_torque = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8);
+    uint32_t ts = TIM2->CNT;
+    int violation = 0;
+    desired_torque = to_signed(desired_torque, 11);
+
     if (current_controls_allowed) {
-      if ((steer > max_steer) || (steer < -max_steer)) return 0;
-    } else {
-      if (steer != 0) return 0;
+
+      // *** global torque limit check ***
+      violation |= max_limit_check(desired_torque, GM_MAX_STEER);
+
+      // *** torque rate limit check ***
+      violation |= driver_limit_check(desired_torque, gm_desired_torque_last, &gm_torque_driver,
+        GM_MAX_STEER, GM_MAX_RATE_UP, GM_MAX_RATE_DOWN,
+        GM_DRIVER_TORQUE_ALLOWANCE, GM_DRIVER_TORQUE_FACTOR);
+
+      // used next time
+      gm_desired_torque_last = desired_torque;
+
+      // *** torque real time rate limit check ***
+      violation |= rt_rate_limit_check(desired_torque, gm_rt_torque_last, GM_MAX_RT_DELTA);
+
+      // every RT_INTERVAL set the new limits
+      uint32_t ts_elapsed = get_ts_elapsed(ts, gm_ts_last);
+      if (ts_elapsed > GM_RT_INTERVAL) {
+        gm_rt_torque_last = desired_torque;
+        gm_ts_last = ts;
+      }
+    }
+
+    // no torque if controls is not allowed
+    if (!current_controls_allowed && (desired_torque != 0)) {
+      violation = 1;
+    }
+
+    // reset to 0 if either controls is not allowed or there's a violation
+    if (violation || !current_controls_allowed) {
+      gm_desired_torque_last = 0;
+      gm_rt_torque_last = 0;
+      gm_ts_last = ts;
+    }
+
+    if (violation) {
+      return false;
     }
   }
 
diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py
@@ -40,6 +40,7 @@
 void set_timer(int t);
 void set_torque_meas(int min, int max);
 void set_cadillac_torque_driver(int min, int max);
+void set_gm_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);
@@ -62,6 +63,13 @@
 void set_cadillac_desired_torque_last(int t);
 void set_cadillac_rt_torque_last(int t);
 
+void init_tests_gm(void);
+void gm_init(int16_t param);
+void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
+int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
+void set_gm_desired_torque_last(int t);
+void set_gm_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);
 
diff --git a/tests/safety/test.c b/tests/safety/test.c
@@ -24,6 +24,7 @@ typedef struct
 
 struct sample_t torque_meas;
 struct sample_t cadillac_torque_driver;
+struct sample_t gm_torque_driver;
 
 TIM_TypeDef timer;
 TIM_TypeDef *TIM2 = &timer;
@@ -69,6 +70,11 @@ void set_cadillac_torque_driver(int min, int max){
   cadillac_torque_driver.max = max;
 }
 
+void set_gm_torque_driver(int min, int max){
+  gm_torque_driver.min = min;
+  gm_torque_driver.max = max;
+}
+
 int get_torque_meas_min(void){
   return torque_meas.min;
 }
@@ -85,6 +91,10 @@ void set_cadillac_rt_torque_last(int t){
   cadillac_rt_torque_last = t;
 }
 
+void set_gm_rt_torque_last(int t){
+  gm_rt_torque_last = t;
+}
+
 void set_desired_torque_last(int t){
   desired_torque_last = t;
 }
@@ -93,6 +103,11 @@ void set_cadillac_desired_torque_last(int t){
   for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = t;
 }
 
+void set_gm_desired_torque_last(int t){
+  gm_desired_torque_last = t;
+}
+
+
 int get_ego_speed(void){
   return ego_speed;
 }
@@ -131,6 +146,15 @@ void init_tests_cadillac(void){
   set_timer(0);
 }
 
+void init_tests_gm(void){
+  gm_torque_driver.min = 0;
+  gm_torque_driver.max = 0;
+  gm_desired_torque_last = 0;
+  gm_rt_torque_last = 0;
+  gm_ts_last = 0;
+  set_timer(0);
+}
+
 void init_tests_honda(void){
   ego_speed = 0;
   gas_interceptor_detected = 0;
diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py
diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py
