diff --git a/board/safety.h b/board/safety.h index 4b5a84084845dd..15aea6db8d1b14 100644 --- a/board/safety.h +++ b/board/safety.h @@ -5,6 +5,11 @@ struct sample_t { int max; } sample_t_default = {{0}, 0, 0}; +struct lookup_t { + float x[3]; + float y[3]; +}; + void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); int safety_tx_lin_hook(int lin_num, uint8_t *data, int len); @@ -16,9 +21,10 @@ int max_limit_check(int val, const int MAX, const int MIN); 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, 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); +float interpolate(struct lookup_t xy, float x); typedef void (*safety_hook_init)(int16_t param); typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push); @@ -201,3 +207,29 @@ int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) { // check for violation return (val < lowest_val) || (val > highest_val); } + + +// interp function that holds extreme values +float interpolate(struct lookup_t xy, float x) { + int size = sizeof(xy.x) / sizeof(xy.x[0]); + // x is lower than the first point in the x array. Return the first point + if (x <= xy.x[0]) { + return xy.y[0]; + + } else { + // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp + for (int i=0; i < size-1; i++) { + if (x < xy.x[i+1]) { + float x0 = xy.x[i]; + float y0 = xy.y[i]; + float dx = xy.x[i+1] - x0; + float dy = xy.y[i+1] - y0; + // dx should not be zero as xy.x is supposed ot be monotonic + if (dx <= 0.) dx = 0.0001; + return dy * (x - x0) / dx + y0; + } + } + // if no such point is found, then x > xy.x[size-1]. Return last point + return xy.y[size - 1]; + } +} diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h index dc265fa03e55c7..1627ece3a5a126 100644 --- a/board/safety/safety_toyota_ipas.h +++ b/board/safety/safety_toyota_ipas.h @@ -4,11 +4,6 @@ // IPAS override const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value -struct lookup_t { - float x[3]; - float y[3]; -}; - // 2m/s are added to be less restrictive const struct lookup_t LOOKUP_ANGLE_RATE_UP = { {2., 7., 17.}, @@ -35,31 +30,6 @@ uint32_t ts_angle_last = 0; int controls_allowed_last = 0; -// interp function that holds extreme values -float interpolate(struct lookup_t xy, float x) { - int size = sizeof(xy.x) / sizeof(xy.x[0]); - // x is lower than the first point in the x array. Return the first point - if (x <= xy.x[0]) { - return xy.y[0]; - - } else { - // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp - for (int i=0; i < size-1; i++) { - if (x < xy.x[i+1]) { - float x0 = xy.x[i]; - float y0 = xy.y[i]; - float dx = xy.x[i+1] - x0; - float dy = xy.y[i+1] - y0; - // dx should not be zero as xy.x is supposed ot be monotonic - if (dx <= 0.) dx = 0.0001; - return dy * (x - x0) / dx + y0; - } - } - // if no such point is found, then x > xy.x[size-1]. Return last point - return xy.y[size - 1]; - } -} - static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check standard toyota stuff as well @@ -146,15 +116,15 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int delta_angle_down = (int) (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG + 1.); int highest_desired_angle = desired_angle_last + (desired_angle_last > 0? delta_angle_up:delta_angle_down); int lowest_desired_angle = desired_angle_last - (desired_angle_last > 0? delta_angle_down:delta_angle_up); - if ((desired_angle > highest_desired_angle) || + if ((desired_angle > highest_desired_angle) || (desired_angle < lowest_desired_angle)){ violation = 1; controls_allowed = 0; } } - + // desired steer angle should be the same as steer angle measured when controls are off - if ((!controls_allowed) && + if ((!controls_allowed) && ((desired_angle < (angle_meas.min - 1)) || (desired_angle > (angle_meas.max + 1)) || (ipas_state_cmd != 1))) {