Skip to content

Commit

Permalink
Subaru brake check (commaai#463)
Browse files Browse the repository at this point in the history
* WIP: subaru brake check

* FInalized logic and added tests to Subaru brake check
  • Loading branch information
rbiasini authored Mar 7, 2020
1 parent 4ecc6b3 commit 74c8ee0
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 0 deletions.
21 changes: 21 additions & 0 deletions board/safety/safety_subaru.h
Original file line number Diff line number Diff line change
Expand Up @@ -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}};
Expand All @@ -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
Expand All @@ -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

Expand Down Expand Up @@ -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)) {
Expand Down
49 changes: 49 additions & 0 deletions tests/safety/test_subaru.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
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]]

Expand Down Expand Up @@ -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):
Expand All @@ -69,6 +73,24 @@ def _torque_driver_msg(self, torque):
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():
Expand Down Expand Up @@ -133,6 +155,33 @@ def test_disengage_on_gas(self):
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):
Expand Down

0 comments on commit 74c8ee0

Please sign in to comment.