Safety: made the driver steer check common so it can be shared across…

… multiple safety files
commaai · Jun 13, 2018 · bf5db45 · bf5db45
1 parent ef079e6
commit bf5db45
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 22 deletions.
diff --git a/board/safety.h b/board/safety.h
@@ -15,6 +15,9 @@ void update_sample(struct sample_t *sample, int sample_new);
 int max_limit_check(int val, const int MAX);
 int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
   const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
+int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
+  const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN, 
+  const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
 int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 
 typedef void (*safety_hook_init)(int16_t param);
@@ -166,6 +169,28 @@ int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
   return (val < lowest_allowed_val) || (val > highest_allowed_val);
 }
 
+// check that commanded value isn't fighting against driver
+int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
+  const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
+  const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {
+
+  int highest_allowed = max(val_last, 0) + MAX_RATE_UP;
+  int lowest_allowed = min(val_last, 0) - MAX_RATE_UP;
+
+  int driver_max_limit = MAX + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
+  int driver_min_limit = -MAX + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
+
+  // if we've exceeded the applied torque, we must start moving toward 0
+  highest_allowed = min(highest_allowed, max(val_last - MAX_RATE_DOWN,
+                                             max(driver_max_limit, 0)));
+  lowest_allowed = max(lowest_allowed, min(val_last + MAX_RATE_DOWN,
+                                           min(driver_min_limit, 0)));
+
+  // check for violation
+  return (val < lowest_allowed) || (val > highest_allowed);
+}
+
+
 // real time check, mainly used for steer torque rate limiter
 int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
 

diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h
@@ -74,28 +74,10 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       violation |= max_limit_check(desired_torque, CADILLAC_STEER_MAX);
 
       // *** torque rate limit check ***
-      int highest_allowed_torque = max(cadillac_desired_torque_last[idx], 0) + CADILLAC_MAX_RATE_UP;
-      int lowest_allowed_torque = min(cadillac_desired_torque_last[idx], 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.min) *
-                                    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[idx] - CADILLAC_MAX_RATE_DOWN,
-                                       max(driver_torque_max_limit, 0)));
-      lowest_allowed_torque = max(lowest_allowed_torque,
-                                  min(cadillac_desired_torque_last[idx] + 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;
-      }
+      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_DRIVER_TORQUE_ALLOWANCE, CADILLAC_DRIVER_TORQUE_FACTOR);
 
       // used next time
       cadillac_desired_torque_last[idx] = desired_torque;