Subaru brake check (#463)
* WIP: subaru brake check * FInalized logic and added tests to Subaru brake checkmaster
parent
4ecc6b3581
commit
74c8ee0a7a
|
@ -7,6 +7,7 @@ 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 = 10;
|
||||
const int SUBARU_STANDSTILL_THRSLD = 20; // about 1kph
|
||||
|
||||
const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x221, 0}, {0x322, 0}};
|
||||
const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}};
|
||||
|
@ -16,6 +17,8 @@ const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_M
|
|||
AddrCheckStruct subaru_rx_checks[] = {
|
||||
{.addr = { 0x40}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U},
|
||||
{.addr = {0x119}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {0x139}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {0x13a}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
{.addr = {0x240}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U},
|
||||
};
|
||||
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
|
||||
|
@ -30,8 +33,10 @@ const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_r
|
|||
int subaru_cruise_engaged_last = 0;
|
||||
int subaru_rt_torque_last = 0;
|
||||
int subaru_desired_torque_last = 0;
|
||||
int subaru_speed = 0;
|
||||
uint32_t subaru_ts_last = 0;
|
||||
bool subaru_gas_last = false;
|
||||
bool subaru_brake_last = false;
|
||||
bool subaru_global = false;
|
||||
struct sample_t subaru_torque_driver; // last few driver torques measured
|
||||
|
||||
|
@ -92,6 +97,22 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
subaru_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// sample subaru wheel speed, averaging opposite corners
|
||||
if ((addr == 0x13a) && subaru_global) {
|
||||
subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR
|
||||
subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF; // RL
|
||||
subaru_speed /= 2;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models)
|
||||
if ((addr == 0x139) && subaru_global) {
|
||||
bool brake = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
|
||||
if (brake && (!subaru_brake_last || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
subaru_brake_last = brake;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if (((addr == 0x40) && subaru_global) ||
|
||||
((addr == 0x140) && !subaru_global)) {
|
||||
|
|
|
@ -15,6 +15,8 @@ RT_INTERVAL = 250000
|
|||
DRIVER_TORQUE_ALLOWANCE = 60;
|
||||
DRIVER_TORQUE_FACTOR = 10;
|
||||
|
||||
SPEED_THRESHOLD = 20 # 1kph (see dbc file)
|
||||
|
||||
TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]]
|
||||
TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]]
|
||||
|
||||
|
@ -44,6 +46,8 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
cnt_gas = 0
|
||||
cnt_torque_driver = 0
|
||||
cnt_cruise = 0
|
||||
cnt_speed = 0
|
||||
cnt_brake = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
|
@ -69,6 +73,24 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
to_send[0].RDHR = (t >> 3) & 0xFF
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
speed &= 0x1FFF
|
||||
to_send = make_msg(0, 0x13a)
|
||||
to_send[0].RDLR = speed << 12
|
||||
to_send[0].RDHR = speed << 6
|
||||
to_send[0].RDLR |= (self.cnt_speed & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x13a, 8)
|
||||
self.__class__.cnt_speed += 1
|
||||
return to_send
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x139)
|
||||
to_send[0].RDHR = (brake << 4) & 0xFFF
|
||||
to_send[0].RDLR |= (self.cnt_brake & 0xF) << 8
|
||||
to_send[0].RDLR |= subaru_checksum(to_send, 0x139, 8)
|
||||
self.__class__.cnt_brake += 1
|
||||
return to_send
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
t = twos_comp(torque, 13)
|
||||
if self.safety.get_subaru_global():
|
||||
|
@ -133,6 +155,33 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_allow_brake_at_zero_speed(self):
|
||||
# Brake was already pressed
|
||||
if (self.safety.get_subaru_global()):
|
||||
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
|
||||
if (self.safety.get_subaru_global()):
|
||||
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_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-3000, 3000):
|
||||
|
|
Loading…
Reference in New Issue