Subaru: added proper safety model
parent
0b10bb70ec
commit
176f1325ba
|
@ -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,
|
||||
|
|
Loading…
Reference in New Issue