From 367c9ad2429437ec0bd24af4b99f1a15f7428b60 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Wed, 11 Apr 2018 14:31:45 -0700 Subject: [PATCH] add safety toyota ipas --- board/safety.h | 3 + board/safety/safety_toyota.h | 46 +++-- board/safety/safety_toyota_ipas.h | 303 ++++++++++++++++++++++++++++++ 3 files changed, 335 insertions(+), 17 deletions(-) create mode 100644 board/safety/safety_toyota_ipas.h diff --git a/board/safety.h b/board/safety.h index 3e0b0cf38a1386..b337851d2a49ef 100644 --- a/board/safety.h +++ b/board/safety.h @@ -23,6 +23,7 @@ int controls_allowed = 0; #include "safety/safety_defaults.h" #include "safety/safety_honda.h" #include "safety/safety_toyota.h" +#include "safety/safety_toyota_ipas.h" #include "safety/safety_gm.h" #include "safety/safety_elm327.h" @@ -52,6 +53,7 @@ typedef struct { #define SAFETY_NOOUTPUT 0 #define SAFETY_HONDA 1 #define SAFETY_TOYOTA 2 +#define SAFETY_TOYOTA_IPAS 0x1335 #define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_GM 3 #define SAFETY_HONDA_BOSCH 4 @@ -64,6 +66,7 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_HONDA_BOSCH, &honda_bosch_hooks}, {SAFETY_TOYOTA, &toyota_hooks}, {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks}, + {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, {SAFETY_GM, &gm_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, {SAFETY_ELM327, &elm327_hooks}, diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 1a12eb8346bb53..3fa3b95950157f 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -1,6 +1,10 @@ // track the torque measured for limiting -int16_t torque_meas[3] = {0, 0, 0}; // last 3 motor torques produced by the eps -int16_t torque_meas_min = 0, torque_meas_max = 0; +struct sample_t { + int values[3]; + int min; + int max; +} sample_t_default = {{0, 0, 0}, 0, 0}; +struct sample_t torque_meas; // last 3 motor torques produced by the eps // global torque limit const int32_t MAX_TORQUE = 1500; // max torque cmd allowed ever @@ -30,6 +34,24 @@ int16_t rt_torque_last = 0; // last desired torque for real time chec uint32_t ts_last = 0; int cruise_engaged_last = 0; // cruise state +uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) { + return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts; +} + +void update_sample(struct sample_t *sample, int sample_new) { + for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) { + sample->values[i] = sample->values[i-1]; + } + sample->values[0] = sample_new; + + // get the minimum and maximum measured torque over the last 3 frames + sample->min = sample->max = sample->values[0]; + for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) { + if (sample->values[i] < sample->min) sample->min = sample->values[i]; + if (sample->values[i] > sample->max) sample->max = sample->values[i]; + } +} + static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // get eps motor torque (0.66 factor in dbc) if ((to_push->RIR>>21) == 0x260) { @@ -38,18 +60,8 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // increase torque_meas by 1 to be conservative on rounding int torque_meas_new = ((int)(torque_meas_new_16) * dbc_eps_torque_factor / 100) + (torque_meas_new_16 > 0 ? 1 : -1); - // shift the array - for (int i = sizeof(torque_meas)/sizeof(torque_meas[0]) - 1; i > 0; i--) { - torque_meas[i] = torque_meas[i-1]; - } - torque_meas[0] = torque_meas_new; - - // get the minimum and maximum measured torque over the last 3 frames - torque_meas_min = torque_meas_max = torque_meas[0]; - for (int i = 1; i < sizeof(torque_meas)/sizeof(torque_meas[0]); i++) { - if (torque_meas[i] < torque_meas_min) torque_meas_min = torque_meas[i]; - if (torque_meas[i] > torque_meas_max) torque_meas_max = torque_meas[i]; - } + // update array of sample + update_sample(&torque_meas, torque_meas_new); } // exit controls on ACC off @@ -102,8 +114,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { 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)); + 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)) { @@ -124,7 +136,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts; + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > RT_INTERVAL) { rt_torque_last = desired_torque; ts_last = ts; diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h new file mode 100644 index 00000000000000..1e5ee8a512d022 --- /dev/null +++ b/board/safety/safety_toyota_ipas.h @@ -0,0 +1,303 @@ +// uses tons from safety_toyota +// TODO: refactor to repeat less code + +// IPAS override +const int32_t 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.}, + {5., .8, .15}}; + +const struct lookup_t LOOKUP_ANGLE_RATE_DOWN = { + {2., 7., 17.}, + {5., 3.5, .4}}; + +const float RT_ANGLE_FUDGE = 1.5; // for RT checks allow 50% more angle change +const float CAN_TO_DEG = 2. / 3.; // convert angles from CAN unit to degrees + +int ipas_state = 1; // 1 disabled, 3 executing angle control, 5 override +int angle_control = 0; // 1 if direct angle control packets are seen +float speed = 0.; + +struct sample_t angle_meas; // last 3 steer angles +struct sample_t torque_driver; // last 3 driver steering torque + +// state of angle limits +int16_t desired_angle_last = 0; // last desired steer angle +int16_t rt_angle_last = 0; // last desired torque for real time check +uint32_t ts_angle_last = 0; + +int controls_allowed_last = 0; + +int to_signed(int d, int bits) { + if (d >= (1 << (bits - 1))) { + d -= (1 << bits); + } + return d; +} + +// 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) { + + // EPS torque sensor + if ((to_push->RIR>>21) == 0x260) { + // get eps motor torque (see dbc_eps_torque_factor in dbc) + int16_t torque_meas_new_16 = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF)); + + // increase torque_meas by 1 to be conservative on rounding + int torque_meas_new = ((int)(torque_meas_new_16) * dbc_eps_torque_factor / 100) + (torque_meas_new_16 > 0 ? 1 : -1); + + // update array of sample + update_sample(&torque_meas, torque_meas_new); + + // get driver steering torque + int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF)); + + // update array of samples + update_sample(&torque_driver, torque_driver_new); + } + + // get steer angle + if ((to_push->RIR>>21) == 0x25) { + int angle_meas_new = ((to_push->RDLR & 0xf) << 8) + ((to_push->RDLR & 0xff00) >> 8); + uint32_t ts = TIM2->CNT; + + angle_meas_new = to_signed(angle_meas_new, 12); + + // update array of samples + update_sample(&angle_meas, angle_meas_new); + + // *** angle real time check + // add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz + int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG + 1.))); + int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG + 1.))); + int highest_rt_angle = rt_angle_last + (rt_angle_last > 0? rt_delta_angle_up:rt_delta_angle_down); + int lowest_rt_angle = rt_angle_last - (rt_angle_last > 0? rt_delta_angle_down:rt_delta_angle_up); + + // every RT_INTERVAL or when controls are turned on, set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); + if ((ts_elapsed > RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) { + rt_angle_last = angle_meas_new; + ts_angle_last = ts; + } + + // check for violation + if (angle_control && + ((angle_meas_new < lowest_rt_angle) || + (angle_meas_new > highest_rt_angle))) { + controls_allowed = 0; + } + + controls_allowed_last = controls_allowed; + } + + // get speed + if ((to_push->RIR>>21) == 0xb4) { + speed = ((float) (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF))) * 0.01 / 3.6; + } + + // enter controls on rising edge of ACC, exit controls on ACC off + if ((to_push->RIR>>21) == 0x1D2) { + // 4 bits: 55-52 + int cruise_engaged = to_push->RDHR & 0xF00000; + if (cruise_engaged && (!cruise_engaged_last)) { + controls_allowed = 1; + } else if (!cruise_engaged) { + controls_allowed = 0; + } + cruise_engaged_last = cruise_engaged; + } + + // get ipas state + if ((to_push->RIR>>21) == 0x262) { + ipas_state = (to_push->RDLR & 0xf); + } + + // exit controls on high steering override + if (angle_control && ((torque_driver.min > IPAS_OVERRIDE_THRESHOLD) || + (torque_driver.max < -IPAS_OVERRIDE_THRESHOLD) || + (ipas_state==5))) { + controls_allowed = 0; + } +} + +static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + + // Check if msg is sent on BUS 0 + if (((to_send->RDTR >> 4) & 0xF) == 0) { + + // ACCEL: safety check on byte 1-2 + if ((to_send->RIR>>21) == 0x343) { + int16_t desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF); + if (controls_allowed && actuation_limits) { + if ((desired_accel > MAX_ACCEL) || (desired_accel < MIN_ACCEL)) { + return 0; + } + } else if (!controls_allowed && (desired_accel != 0)) { + return 0; + } + } + + // STEER ANGLE + if ((to_send->RIR>>21) == 0x266) { + + angle_control = 1; // we are in angle control mode + int desired_angle = ((to_send->RDLR & 0xf) << 8) + ((to_send->RDLR & 0xff00) >> 8); + int ipas_state_cmd = ((to_send->RDLR & 0xff) >> 4); + int16_t violation = 0; + + desired_angle = to_signed(desired_angle, 12); + + if (controls_allowed) { + // add 1 to not false trigger the violation + int delta_angle_up = (int) (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG + 1.); + 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) || + (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) && + ((desired_angle < (angle_meas.min - 1)) || + (desired_angle > (angle_meas.max + 1)) || + (ipas_state_cmd != 1))) { + violation = 1; + } + + desired_angle_last = desired_angle; + + if (violation) { + return false; + } + } + + // STEER TORQUE: safety check on bytes 2-3 + if ((to_send->RIR>>21) == 0x2E4) { + int16_t desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF); + int16_t violation = 0; + + uint32_t ts = TIM2->CNT; + + // only check if controls are allowed and actuation_limits are imposed + if (controls_allowed && actuation_limits) { + + // *** global torque limit check *** + if (desired_torque < -MAX_TORQUE) violation = 1; + if (desired_torque > MAX_TORQUE) violation = 1; + + + // *** 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; + } + + // 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; + } + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); + if (ts_elapsed > RT_INTERVAL) { + rt_torque_last = desired_torque; + ts_last = ts; + } + } + + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = 1; + } + + // reset to 0 if either controls is not allowed or there's a violation + if (violation || !controls_allowed) { + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; + } + + if (violation) { + return false; + } + } + } + + // 1 allows the message through + return true; +} + +static int toyota_ipas_tx_lin_hook(int lin_num, uint8_t *data, int len) { + // TODO: add safety if using LIN + return true; +} + +static void toyota_ipas_init(int16_t param) { + controls_allowed = 0; + actuation_limits = 1; + dbc_eps_torque_factor = param; +} + +static int toyota_ipas_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + return -1; +} + +const safety_hooks toyota_ipas_hooks = { + .init = toyota_ipas_init, + .rx = toyota_ipas_rx_hook, + .tx = toyota_ipas_tx_hook, + .tx_lin = toyota_ipas_tx_lin_hook, + .fwd = toyota_ipas_fwd_hook, +}; +