From 1966bdf348eb0b45a06c4fa9cf9675e08e680d08 Mon Sep 17 00:00:00 2001
From: Commaremote <31535535+commaremote@user.noreply.github.com>
Date: Wed, 13 Jun 2018 00:56:29 -0700
Subject: [PATCH] Safety: made real time rate limit check a shared function

---
 board/safety.h                 | 12 ++++++++++++
 board/safety/safety_cadillac.h |  9 +--------
 board/safety/safety_toyota.h   |  9 +--------
 3 files changed, 14 insertions(+), 16 deletions(-)

diff --git a/board/safety.h b/board/safety.h
index 14ca2c34d..cd3489554 100644
--- a/board/safety.h
+++ b/board/safety.h
@@ -12,6 +12,7 @@ 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);
 
 typedef void (*safety_hook_init)(int16_t param);
 typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@@ -141,3 +142,14 @@ void update_sample(struct sample_t *sample, int sample_new) {
     if (sample->values[i] > sample->max) sample->max = sample->values[i];
   }
 }
+
+// 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
+        return (val < lowest_val) || (val > highest_val);
+}
diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h
index 6ea41ccbf..6660b0387 100644
--- a/board/safety/safety_cadillac.h
+++ b/board/safety/safety_cadillac.h
@@ -103,14 +103,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       cadillac_desired_torque_last[idx] = 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;
-      }
+      violation |= rt_rate_limit_check(desired_torque, cadillac_rt_torque_last, CADILLAC_MAX_RT_DELTA);
 
       // every RT_INTERVAL set the new limits
       uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last);
diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
index 78b6d2154..83b96822d 100644
--- a/board/safety/safety_toyota.h
+++ b/board/safety/safety_toyota.h
@@ -108,15 +108,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
         // used next time
         desired_torque_last = desired_torque;
 
-
         // *** torque real time rate limit check ***
-        int16_t highest_rt_torque = max(rt_torque_last, 0) + MAX_RT_DELTA;
-        int16_t lowest_rt_torque = min(rt_torque_last, 0) - MAX_RT_DELTA;
-
-        // check for violation
-        if ((desired_torque < lowest_rt_torque) || (desired_torque > highest_rt_torque)) {
-          violation = 1;
-        }
+        violation |= rt_rate_limit_check(desired_torque, rt_torque_last, MAX_RT_DELTA);
 
         // every RT_INTERVAL set the new limits
         uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);