From bf5db45ab1156a2aef6ed45b3d7a6ae90536f227 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Wed, 13 Jun 2018 15:23:56 -0700 Subject: [PATCH] Safety: made the driver steer check common so it can be shared across multiple safety files --- board/safety.h | 25 +++++++++++++++++++++++++ board/safety/safety_cadillac.h | 26 ++++---------------------- 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/board/safety.h b/board/safety.h index 9ea1e6e661bf02..411d757a9bccae 100644 --- 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 index 94c356d01ec7bc..f930c4ff40a242 100644 --- 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;