2019-01-28 22:46:13 -07:00
|
|
|
const int HYUNDAI_MAX_STEER = 255; // like stock
|
2018-08-29 20:30:14 -06:00
|
|
|
const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
|
2019-06-14 11:57:14 -06:00
|
|
|
const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
|
2018-08-28 21:05:58 -06:00
|
|
|
const int HYUNDAI_MAX_RATE_UP = 3;
|
|
|
|
const int HYUNDAI_MAX_RATE_DOWN = 7;
|
2018-08-28 17:01:40 -06:00
|
|
|
const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
|
2018-08-28 21:05:58 -06:00
|
|
|
const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
|
2019-11-17 01:24:19 -07:00
|
|
|
const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}};
|
|
|
|
|
2019-12-21 02:25:54 -07:00
|
|
|
// TODO: do checksum and counter checks
|
|
|
|
AddrCheckStruct hyundai_rx_checks[] = {
|
2020-02-22 11:13:32 -07:00
|
|
|
{.addr = {608}, .bus = 0, .expected_timestep = 10000U},
|
2019-12-21 02:25:54 -07:00
|
|
|
{.addr = {897}, .bus = 0, .expected_timestep = 10000U},
|
|
|
|
{.addr = {1057}, .bus = 0, .expected_timestep = 20000U},
|
|
|
|
};
|
|
|
|
const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]);
|
|
|
|
|
2018-08-28 17:01:40 -06:00
|
|
|
int hyundai_rt_torque_last = 0;
|
|
|
|
int hyundai_desired_torque_last = 0;
|
|
|
|
int hyundai_cruise_engaged_last = 0;
|
2020-02-22 11:13:32 -07:00
|
|
|
bool hyundai_gas_last = false;
|
2018-08-28 17:01:40 -06:00
|
|
|
uint32_t hyundai_ts_last = 0;
|
|
|
|
struct sample_t hyundai_torque_driver; // last few driver torques measured
|
2018-08-17 22:31:00 -06:00
|
|
|
|
2019-12-21 02:25:54 -07:00
|
|
|
static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
2018-08-17 22:31:00 -06:00
|
|
|
|
2020-01-18 14:26:34 -07:00
|
|
|
bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN,
|
|
|
|
NULL, NULL, NULL);
|
|
|
|
|
2020-03-06 18:51:01 -07:00
|
|
|
if (valid && GET_BUS(to_push) == 0) {
|
2020-01-18 14:26:34 -07:00
|
|
|
int addr = GET_ADDR(to_push);
|
2018-08-17 22:31:00 -06:00
|
|
|
|
2020-01-18 14:26:34 -07:00
|
|
|
if (addr == 897) {
|
|
|
|
int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048;
|
|
|
|
// update array of samples
|
|
|
|
update_sample(&hyundai_torque_driver, torque_driver_new);
|
2019-06-17 15:13:50 -06:00
|
|
|
}
|
2020-01-18 14:26:34 -07:00
|
|
|
|
|
|
|
// enter controls on rising edge of ACC, exit controls on ACC off
|
|
|
|
if (addr == 1057) {
|
|
|
|
// 2 bits: 13-14
|
|
|
|
int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
|
|
|
|
if (cruise_engaged && !hyundai_cruise_engaged_last) {
|
|
|
|
controls_allowed = 1;
|
|
|
|
}
|
|
|
|
if (!cruise_engaged) {
|
|
|
|
controls_allowed = 0;
|
|
|
|
}
|
|
|
|
hyundai_cruise_engaged_last = cruise_engaged;
|
2018-08-28 17:01:40 -06:00
|
|
|
}
|
2019-12-21 02:25:54 -07:00
|
|
|
|
2020-02-22 11:13:32 -07:00
|
|
|
// exit controls on rising edge of gas press
|
2020-03-06 18:51:01 -07:00
|
|
|
if (addr == 608) {
|
2020-02-22 11:13:32 -07:00
|
|
|
bool gas = (GET_BYTE(to_push, 7) >> 6) != 0;
|
|
|
|
if (gas && ! hyundai_gas_last) {
|
|
|
|
controls_allowed = 0;
|
|
|
|
}
|
|
|
|
hyundai_gas_last = gas;
|
|
|
|
}
|
2019-12-21 02:25:54 -07:00
|
|
|
|
2020-01-18 14:26:34 -07:00
|
|
|
// check if stock camera ECU is on bus 0
|
2020-03-06 18:51:01 -07:00
|
|
|
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) {
|
2020-01-18 14:26:34 -07:00
|
|
|
relay_malfunction = true;
|
|
|
|
}
|
2019-12-21 02:25:54 -07:00
|
|
|
}
|
2020-01-18 14:26:34 -07:00
|
|
|
return valid;
|
2018-08-17 22:31:00 -06:00
|
|
|
}
|
|
|
|
|
2018-08-28 17:01:40 -06:00
|
|
|
static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|
|
|
|
2019-06-12 19:18:07 -06:00
|
|
|
int tx = 1;
|
2019-06-14 20:13:05 -06:00
|
|
|
int addr = GET_ADDR(to_send);
|
2019-11-17 01:24:19 -07:00
|
|
|
int bus = GET_BUS(to_send);
|
|
|
|
|
2019-12-18 01:17:25 -07:00
|
|
|
if (!msg_allowed(addr, bus, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0]))) {
|
2019-11-17 01:24:19 -07:00
|
|
|
tx = 0;
|
|
|
|
}
|
2019-06-12 19:18:07 -06:00
|
|
|
|
2019-11-15 01:32:45 -07:00
|
|
|
if (relay_malfunction) {
|
2019-06-12 19:18:07 -06:00
|
|
|
tx = 0;
|
2018-08-28 17:01:40 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
// LKA STEER: safety check
|
2019-06-14 20:13:05 -06:00
|
|
|
if (addr == 832) {
|
2019-07-10 21:56:03 -06:00
|
|
|
int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024;
|
2018-08-28 17:01:40 -06:00
|
|
|
uint32_t ts = TIM2->CNT;
|
2019-06-12 21:12:48 -06:00
|
|
|
bool violation = 0;
|
2018-08-28 17:01:40 -06:00
|
|
|
|
2018-08-31 23:47:20 -06:00
|
|
|
if (controls_allowed) {
|
2018-08-28 17:01:40 -06:00
|
|
|
|
|
|
|
// *** global torque limit check ***
|
|
|
|
violation |= max_limit_check(desired_torque, HYUNDAI_MAX_STEER, -HYUNDAI_MAX_STEER);
|
|
|
|
|
|
|
|
// *** torque rate limit check ***
|
|
|
|
violation |= driver_limit_check(desired_torque, hyundai_desired_torque_last, &hyundai_torque_driver,
|
|
|
|
HYUNDAI_MAX_STEER, HYUNDAI_MAX_RATE_UP, HYUNDAI_MAX_RATE_DOWN,
|
|
|
|
HYUNDAI_DRIVER_TORQUE_ALLOWANCE, HYUNDAI_DRIVER_TORQUE_FACTOR);
|
|
|
|
|
|
|
|
// used next time
|
|
|
|
hyundai_desired_torque_last = desired_torque;
|
2018-08-17 22:31:00 -06:00
|
|
|
|
2018-08-28 17:01:40 -06:00
|
|
|
// *** torque real time rate limit check ***
|
|
|
|
violation |= rt_rate_limit_check(desired_torque, hyundai_rt_torque_last, HYUNDAI_MAX_RT_DELTA);
|
|
|
|
|
|
|
|
// every RT_INTERVAL set the new limits
|
|
|
|
uint32_t ts_elapsed = get_ts_elapsed(ts, hyundai_ts_last);
|
|
|
|
if (ts_elapsed > HYUNDAI_RT_INTERVAL) {
|
|
|
|
hyundai_rt_torque_last = desired_torque;
|
|
|
|
hyundai_ts_last = ts;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// no torque if controls is not allowed
|
2018-08-31 23:47:20 -06:00
|
|
|
if (!controls_allowed && (desired_torque != 0)) {
|
2018-08-28 17:01:40 -06:00
|
|
|
violation = 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
// reset to 0 if either controls is not allowed or there's a violation
|
2018-08-31 23:47:20 -06:00
|
|
|
if (violation || !controls_allowed) {
|
2018-08-28 17:01:40 -06:00
|
|
|
hyundai_desired_torque_last = 0;
|
|
|
|
hyundai_rt_torque_last = 0;
|
|
|
|
hyundai_ts_last = ts;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (violation) {
|
2019-06-12 19:18:07 -06:00
|
|
|
tx = 0;
|
2018-08-28 17:01:40 -06:00
|
|
|
}
|
2018-08-17 22:31:00 -06:00
|
|
|
}
|
2018-08-28 17:01:40 -06:00
|
|
|
|
2018-08-31 23:47:20 -06:00
|
|
|
// FORCE CANCEL: safety check only relevant when spamming the cancel button.
|
|
|
|
// ensuring that only the cancel button press is sent (VAL 4) when controls are off.
|
|
|
|
// This avoids unintended engagements while still allowing resume spam
|
2019-11-14 00:57:07 -07:00
|
|
|
if ((addr == 1265) && !controls_allowed) {
|
|
|
|
if ((GET_BYTES_04(to_send) & 0x7) != 4) {
|
|
|
|
tx = 0;
|
|
|
|
}
|
|
|
|
}
|
2018-08-31 23:47:20 -06:00
|
|
|
|
2018-08-28 17:01:40 -06:00
|
|
|
// 1 allows the message through
|
2019-06-12 19:18:07 -06:00
|
|
|
return tx;
|
2018-08-17 22:31:00 -06:00
|
|
|
}
|
|
|
|
|
2018-08-30 12:53:52 -06:00
|
|
|
static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
2019-06-12 19:18:07 -06:00
|
|
|
|
|
|
|
int bus_fwd = -1;
|
2019-11-15 01:32:45 -07:00
|
|
|
int addr = GET_ADDR(to_fwd);
|
2018-08-30 12:53:52 -06:00
|
|
|
// forward cam to ccan and viceversa, except lkas cmd
|
2019-11-15 01:32:45 -07:00
|
|
|
if (!relay_malfunction) {
|
2019-06-12 19:18:07 -06:00
|
|
|
if (bus_num == 0) {
|
2019-11-15 01:32:45 -07:00
|
|
|
bus_fwd = 2;
|
2019-06-17 15:13:50 -06:00
|
|
|
}
|
2019-11-15 01:32:45 -07:00
|
|
|
if ((bus_num == 2) && (addr != 832)) {
|
|
|
|
bus_fwd = 0;
|
2019-06-12 19:18:07 -06:00
|
|
|
}
|
2018-08-30 12:53:52 -06:00
|
|
|
}
|
2019-06-12 19:18:07 -06:00
|
|
|
return bus_fwd;
|
2018-08-30 12:53:52 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2018-08-17 22:31:00 -06:00
|
|
|
const safety_hooks hyundai_hooks = {
|
2019-11-15 01:32:45 -07:00
|
|
|
.init = nooutput_init,
|
2018-08-17 22:31:00 -06:00
|
|
|
.rx = hyundai_rx_hook,
|
2018-08-28 17:01:40 -06:00
|
|
|
.tx = hyundai_tx_hook,
|
2018-08-17 22:31:00 -06:00
|
|
|
.tx_lin = nooutput_tx_lin_hook,
|
2018-08-30 12:53:52 -06:00
|
|
|
.fwd = hyundai_fwd_hook,
|
2019-12-21 02:25:54 -07:00
|
|
|
.addr_check = hyundai_rx_checks,
|
|
|
|
.addr_check_len = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]),
|
2018-08-17 22:31:00 -06:00
|
|
|
};
|