Safety: made rate limit check also common

Author: Commaremote

board/safety.h

@@ -12,8 +12,10 @@ int safety_ignition_hook();
 uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
 int to_signed(int d, int bits);
 void update_sample(struct sample_t *sample, int sample_new);
-int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 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 rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 
 typedef void (*safety_hook_init)(int16_t param);
 typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@@ -144,17 +146,33 @@ void update_sample(struct sample_t *sample, int sample_new) {
   }
 }
 
+int max_limit_check(int val, const int MAX) {
+  return (val > MAX) | (val < -MAX);
+}
+
+// check that commanded value isn't too far from measured
+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) {
+
+  // *** val rate limit check ***
+  int16_t highest_allowed_val = max(val_last, 0) + MAX_RATE_UP;
+  int16_t lowest_allowed_val = min(val_last, 0) - MAX_RATE_UP;
+
+  // if we've exceeded the meas val, we must start moving toward 0
+  highest_allowed_val = min(highest_allowed_val, max(val_last - MAX_RATE_DOWN, max(val_meas->max, 0) + MAX_ERROR));
+  lowest_allowed_val = max(lowest_allowed_val, min(val_last + MAX_RATE_DOWN, min(val_meas->min, 0) - MAX_ERROR));
+
+  // check for violation
+  return (val < lowest_allowed_val) || (val > highest_allowed_val);
+}
+
 // 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) {
 
   // *** torque real time rate limit check ***
   int16_t highest_val = max(val_last, 0) + MAX_RT_DELTA;
   int16_t lowest_val = min(val_last, 0) - MAX_RT_DELTA;
 
-  // return 1 if violation
+  // check for violation
   return (val < lowest_val) || (val > highest_val);
 }
-
-int max_limit_check(int val, const int MAX) {
-  return (val > MAX) | (val < -MAX);
-}

board/safety/safety_toyota.h

@@ -92,17 +92,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
         violation |= max_limit_check(desired_torque, MAX_TORQUE);
 
         // *** torque rate limit check ***
-        int16_t highest_allowed_torque = max(desired_torque_last, 0) + MAX_RATE_UP;
-        int16_t lowest_allowed_torque = min(desired_torque_last, 0) - MAX_RATE_UP;
-
-        // if we've exceeded the applied torque, we must start moving toward 0
-        highest_allowed_torque = min(highest_allowed_torque, max(desired_torque_last - MAX_RATE_DOWN, max(torque_meas.max, 0) + MAX_TORQUE_ERROR));
-        lowest_allowed_torque = max(lowest_allowed_torque, min(desired_torque_last + MAX_RATE_DOWN, min(torque_meas.min, 0) - MAX_TORQUE_ERROR));
-
-        // check for violation
-        if ((desired_torque < lowest_allowed_torque) || (desired_torque > highest_allowed_torque)) {
-          violation = 1;
-        }
+        violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, MAX_RATE_UP, MAX_RATE_DOWN, MAX_TORQUE_ERROR);
 
         // used next time
         desired_torque_last = desired_torque;