diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index b9ee6b5cde01fc..8c87af0b13f858 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -1,13 +1,96 @@ -void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {} +const int SUBARU_MAX_STEER = 2047; // 1s +// real time torque limit to prevent controls spamming +// the real time limit is 1500/sec +const int SUBARU_MAX_RT_DELTA = 940; // max delta torque allowed for real time checks +const int32_t SUBARU_RT_INTERVAL = 250000; // 250ms between real time checks +const int SUBARU_MAX_RATE_UP = 50; +const int SUBARU_MAX_RATE_DOWN = 70; +const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60; +const int SUBARU_DRIVER_TORQUE_FACTOR = 5; -// FIXME -// *** all output safety mode *** +int subaru_cruise_engaged_last = 0; +int subaru_rt_torque_last = 0; +int subaru_desired_torque_last = 0; +uint32_t subaru_ts_last = 0; +int subaru_supercruise_on = 0; +struct sample_t subaru_torque_driver; // last few driver torques measured -static void subaru_init(int16_t param) { - controls_allowed = 1; + +static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + int bus_number = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr = to_push->RIR >> 21; + + if ((addr == 0x119) && (bus_number == 0)){ + int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF); + torque_driver_new = to_signed(torque_driver_new, 11); + // update array of samples + update_sample(&subaru_torque_driver, torque_driver_new); + } + + // enter controls on rising edge of ACC, exit controls on ACC off + if ((addr == 0x240) && (bus_number == 0)) { + int cruise_engaged = (to_push->RDHR >> 9) & 1; + if (cruise_engaged && !subaru_cruise_engaged_last) { + controls_allowed = 1; + } else if (!cruise_engaged) { + controls_allowed = 0; + } + subaru_cruise_engaged_last = cruise_engaged; + } } static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + uint32_t addr = to_send->RIR >> 21; + + // steer cmd checks + if (addr == 0x122) { + int desired_torque = ((to_send->RDLR >> 16) && 0x1FFF); + int violation = 0; + uint32_t ts = TIM2->CNT; + desired_torque = to_signed(desired_torque, 13); + + if (controls_allowed) { + + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER); + + // *** torque rate limit check *** + int desired_torque_last = subaru_desired_torque_last; + violation |= driver_limit_check(desired_torque, desired_torque_last, &subaru_torque_driver, + SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN, + SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR); + + // used next time + subaru_desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + violation |= rt_rate_limit_check(desired_torque, subaru_rt_torque_last, SUBARU_MAX_RT_DELTA); + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, subaru_ts_last); + if (ts_elapsed > SUBARU_RT_INTERVAL) { + subaru_rt_torque_last = desired_torque; + subaru_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) { + subaru_desired_torque_last = 0; + subaru_rt_torque_last = 0; + subaru_ts_last = ts; + } + + if (violation || subaru_supercruise_on) { + return false; + } + + } return true; } @@ -40,7 +123,7 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { } const safety_hooks subaru_hooks = { - .init = subaru_init, + .init = nooutput_init, .rx = subaru_rx_hook, .tx = subaru_tx_hook, .tx_lin = nooutput_tx_lin_hook,