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;
|
2018-08-17 22:31:00 -06:00
|
|
|
|
2019-06-12 21:12:48 -06:00
|
|
|
bool hyundai_camera_detected = 0;
|
|
|
|
bool hyundai_giraffe_switch_2 = 0; // is giraffe switch 2 high?
|
2018-09-05 16:39:37 -06:00
|
|
|
int hyundai_camera_bus = 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;
|
|
|
|
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
|
|
|
|
|
|
|
static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
2019-06-14 20:13:05 -06:00
|
|
|
int bus = GET_BUS(to_push);
|
|
|
|
int addr = GET_ADDR(to_push);
|
2018-08-17 22:31:00 -06:00
|
|
|
|
2019-06-14 20:13:05 -06:00
|
|
|
if (addr == 897) {
|
2018-08-29 20:23:49 -06:00
|
|
|
int torque_driver_new = ((to_push->RDLR >> 11) & 0xfff) - 2048;
|
2018-08-28 17:01:40 -06:00
|
|
|
// update array of samples
|
|
|
|
update_sample(&hyundai_torque_driver, torque_driver_new);
|
2018-08-17 22:31:00 -06:00
|
|
|
}
|
|
|
|
|
2018-08-28 17:01:40 -06:00
|
|
|
// check if stock camera ECU is still online
|
2019-06-14 20:13:05 -06:00
|
|
|
if ((bus == 0) && (addr == 832)) {
|
2018-08-28 17:01:40 -06:00
|
|
|
hyundai_camera_detected = 1;
|
|
|
|
controls_allowed = 0;
|
|
|
|
}
|
|
|
|
|
2018-09-05 16:39:37 -06:00
|
|
|
// Find out which bus the camera is on
|
2019-06-14 20:13:05 -06:00
|
|
|
if (addr == 832) {
|
2018-09-05 17:08:42 -06:00
|
|
|
hyundai_camera_bus = bus;
|
2018-09-05 16:39:37 -06:00
|
|
|
}
|
|
|
|
|
2018-08-28 17:01:40 -06:00
|
|
|
// enter controls on rising edge of ACC, exit controls on ACC off
|
2019-06-14 20:13:05 -06:00
|
|
|
if (addr == 1057) {
|
2018-08-28 17:01:40 -06:00
|
|
|
// 2 bits: 13-14
|
|
|
|
int cruise_engaged = (to_push->RDLR >> 13) & 0x3;
|
|
|
|
if (cruise_engaged && !hyundai_cruise_engaged_last) {
|
|
|
|
controls_allowed = 1;
|
|
|
|
} else if (!cruise_engaged) {
|
|
|
|
controls_allowed = 0;
|
|
|
|
}
|
|
|
|
hyundai_cruise_engaged_last = cruise_engaged;
|
|
|
|
}
|
2018-08-30 12:53:52 -06:00
|
|
|
|
2018-09-05 16:39:37 -06:00
|
|
|
// 832 is lkas cmd. If it is on camera bus, then giraffe switch 2 is high
|
2019-06-14 20:13:05 -06:00
|
|
|
if ((addr == 832) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
|
2018-08-30 12:53:52 -06:00
|
|
|
hyundai_giraffe_switch_2 = 1;
|
|
|
|
}
|
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-06-12 19:18:07 -06:00
|
|
|
|
2018-08-28 17:01:40 -06:00
|
|
|
// There can be only one! (camera)
|
|
|
|
if (hyundai_camera_detected) {
|
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) {
|
2018-08-29 20:23:49 -06:00
|
|
|
int desired_torque = ((to_send->RDLR >> 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
|
2018-09-07 21:43:13 -06:00
|
|
|
// TODO: fix bug preventing the button msg to be fwd'd on bus 2
|
2019-06-14 20:13:05 -06:00
|
|
|
//if ((addr == 1265) && !controls_allowed && (bus == 0) {
|
2019-06-12 19:18:07 -06:00
|
|
|
// if ((to_send->RDLR & 0x7) != 4) {
|
|
|
|
// tx = 0;
|
|
|
|
// }
|
2018-09-07 21:43:13 -06:00
|
|
|
//}
|
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;
|
2018-08-30 12:53:52 -06:00
|
|
|
// forward cam to ccan and viceversa, except lkas cmd
|
2019-06-12 19:18:07 -06:00
|
|
|
if (hyundai_giraffe_switch_2) {
|
|
|
|
if (bus_num == 0) {
|
|
|
|
bus_fwd = hyundai_camera_bus;
|
|
|
|
} else if (bus_num == hyundai_camera_bus) {
|
2019-06-14 20:13:05 -06:00
|
|
|
int addr = GET_ADDR(to_fwd);
|
|
|
|
if (addr != 832) {
|
2019-06-12 19:18:07 -06:00
|
|
|
bus_fwd = 0;
|
|
|
|
}
|
|
|
|
}
|
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
|
|
|
}
|
|
|
|
|
|
|
|
static void hyundai_init(int16_t param) {
|
|
|
|
controls_allowed = 0;
|
|
|
|
hyundai_giraffe_switch_2 = 0;
|
|
|
|
}
|
|
|
|
|
2018-08-17 22:31:00 -06:00
|
|
|
const safety_hooks hyundai_hooks = {
|
2018-08-30 12:53:52 -06:00
|
|
|
.init = hyundai_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,
|
|
|
|
.ignition = default_ign_hook,
|
2018-08-30 12:53:52 -06:00
|
|
|
.fwd = hyundai_fwd_hook,
|
2018-08-17 22:31:00 -06:00
|
|
|
};
|