diff --git a/board/safety.h b/board/safety.h index 14ca2c34df4dc5..cd34895544ea0c 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 6ea41ccbf1dfc4..6660b0387a2329 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 78b6d2154aec6a..83b96822d0c5f5 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);