parent
74c8ee0a7a
commit
04809e1329
|
@ -1,16 +1,19 @@
|
||||||
const int HYUNDAI_MAX_STEER = 255; // like stock
|
const int HYUNDAI_MAX_STEER = 255; // like stock
|
||||||
const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
|
const int HYUNDAI_MAX_RT_DELTA = 112; // max delta torque allowed for real time checks
|
||||||
const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
|
const uint32_t HYUNDAI_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||||
const int HYUNDAI_MAX_RATE_UP = 3;
|
const int HYUNDAI_MAX_RATE_UP = 3;
|
||||||
const int HYUNDAI_MAX_RATE_DOWN = 7;
|
const int HYUNDAI_MAX_RATE_DOWN = 7;
|
||||||
const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
|
const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
|
||||||
const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
|
const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
|
||||||
|
const int HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph
|
||||||
const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}};
|
const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}};
|
||||||
|
|
||||||
// TODO: do checksum and counter checks
|
// TODO: do checksum and counter checks
|
||||||
AddrCheckStruct hyundai_rx_checks[] = {
|
AddrCheckStruct hyundai_rx_checks[] = {
|
||||||
{.addr = {608}, .bus = 0, .expected_timestep = 10000U},
|
{.addr = {608}, .bus = 0, .expected_timestep = 10000U},
|
||||||
{.addr = {897}, .bus = 0, .expected_timestep = 10000U},
|
{.addr = {897}, .bus = 0, .expected_timestep = 10000U},
|
||||||
|
{.addr = {902}, .bus = 0, .expected_timestep = 10000U},
|
||||||
|
{.addr = {916}, .bus = 0, .expected_timestep = 10000U},
|
||||||
{.addr = {1057}, .bus = 0, .expected_timestep = 20000U},
|
{.addr = {1057}, .bus = 0, .expected_timestep = 20000U},
|
||||||
};
|
};
|
||||||
const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]);
|
const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]);
|
||||||
|
@ -18,7 +21,9 @@ const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_c
|
||||||
int hyundai_rt_torque_last = 0;
|
int hyundai_rt_torque_last = 0;
|
||||||
int hyundai_desired_torque_last = 0;
|
int hyundai_desired_torque_last = 0;
|
||||||
int hyundai_cruise_engaged_last = 0;
|
int hyundai_cruise_engaged_last = 0;
|
||||||
|
int hyundai_speed = 0;
|
||||||
bool hyundai_gas_last = false;
|
bool hyundai_gas_last = false;
|
||||||
|
bool hyundai_brake_last = false;
|
||||||
uint32_t hyundai_ts_last = 0;
|
uint32_t hyundai_ts_last = 0;
|
||||||
struct sample_t hyundai_torque_driver; // last few driver torques measured
|
struct sample_t hyundai_torque_driver; // last few driver torques measured
|
||||||
|
|
||||||
|
@ -58,6 +63,22 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
hyundai_gas_last = gas;
|
hyundai_gas_last = gas;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// sample subaru wheel speed, averaging opposite corners
|
||||||
|
if (addr == 902) {
|
||||||
|
hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL
|
||||||
|
hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL
|
||||||
|
hyundai_speed /= 2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// exit controls on rising edge of brake press
|
||||||
|
if (addr == 916) {
|
||||||
|
bool brake = (GET_BYTE(to_push, 6) >> 7) != 0;
|
||||||
|
if (brake && (!hyundai_brake_last || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
|
||||||
|
controls_allowed = 0;
|
||||||
|
}
|
||||||
|
hyundai_brake_last = brake;
|
||||||
|
}
|
||||||
|
|
||||||
// check if stock camera ECU is on bus 0
|
// check if stock camera ECU is on bus 0
|
||||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) {
|
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) {
|
||||||
relay_malfunction = true;
|
relay_malfunction = true;
|
||||||
|
|
|
@ -15,6 +15,8 @@ RT_INTERVAL = 250000
|
||||||
DRIVER_TORQUE_ALLOWANCE = 50;
|
DRIVER_TORQUE_ALLOWANCE = 50;
|
||||||
DRIVER_TORQUE_FACTOR = 2;
|
DRIVER_TORQUE_FACTOR = 2;
|
||||||
|
|
||||||
|
SPEED_THRESHOLD = 30 # ~1kph
|
||||||
|
|
||||||
TX_MSGS = [[832, 0], [1265, 0]]
|
TX_MSGS = [[832, 0], [1265, 0]]
|
||||||
|
|
||||||
def twos_comp(val, bits):
|
def twos_comp(val, bits):
|
||||||
|
@ -46,6 +48,17 @@ class TestHyundaiSafety(unittest.TestCase):
|
||||||
to_send[0].RDHR = (val & 0x3) << 30;
|
to_send[0].RDHR = (val & 0x3) << 30;
|
||||||
return to_send
|
return to_send
|
||||||
|
|
||||||
|
def _brake_msg(self, brake):
|
||||||
|
to_send = make_msg(0, 916)
|
||||||
|
to_send[0].RDHR = brake << 23;
|
||||||
|
return to_send
|
||||||
|
|
||||||
|
def _speed_msg(self, speed):
|
||||||
|
to_send = make_msg(0, 902)
|
||||||
|
to_send[0].RDLR = speed & 0x3FFF;
|
||||||
|
to_send[0].RDHR = (speed & 0x3FFF) << 16;
|
||||||
|
return to_send
|
||||||
|
|
||||||
def _set_prev_torque(self, t):
|
def _set_prev_torque(self, t):
|
||||||
self.safety.set_hyundai_desired_torque_last(t)
|
self.safety.set_hyundai_desired_torque_last(t)
|
||||||
self.safety.set_hyundai_rt_torque_last(t)
|
self.safety.set_hyundai_rt_torque_last(t)
|
||||||
|
@ -101,6 +114,31 @@ class TestHyundaiSafety(unittest.TestCase):
|
||||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||||
self.assertFalse(self.safety.get_controls_allowed())
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
|
||||||
|
def test_allow_brake_at_zero_speed(self):
|
||||||
|
# Brake was already pressed
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(1))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(1))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(0))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
# rising edge of brake should disengage
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(1))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(0)) # reset no brakes
|
||||||
|
|
||||||
|
def test_not_allow_brake_when_moving(self):
|
||||||
|
# Brake was already pressed
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(1))
|
||||||
|
self.safety.set_controls_allowed(1)
|
||||||
|
self.safety.safety_rx_hook(self._speed_msg(SPEED_THRESHOLD))
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(1))
|
||||||
|
self.assertTrue(self.safety.get_controls_allowed())
|
||||||
|
self.safety.safety_rx_hook(self._speed_msg(SPEED_THRESHOLD + 1))
|
||||||
|
self.safety.safety_rx_hook(self._brake_msg(1))
|
||||||
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||||
|
|
||||||
def test_non_realtime_limit_up(self):
|
def test_non_realtime_limit_up(self):
|
||||||
self.safety.set_hyundai_torque_driver(0, 0)
|
self.safety.set_hyundai_torque_driver(0, 0)
|
||||||
self.safety.set_controls_allowed(True)
|
self.safety.set_controls_allowed(True)
|
||||||
|
|
Loading…
Reference in New Issue