Skip to content

Commit

Permalink
separating subary legacy safety mode from global (#452)
Browse files Browse the repository at this point in the history 
* separating subary legacy safety mode from global

* added explicit tests for subaru legacy, which were absent before
  • Loading branch information
@rbiasini
rbiasini authored Feb 27, 2020
1 parent dad5858 commit dfa6b07
Show file tree
Hide file tree
Showing 6 changed files with 136 additions and 56 deletions.
2 changes: 2 additions & 0 deletions board/safety.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#define SAFETY_GM_ASCM 18U
#define SAFETY_NOOUTPUT 19U
#define SAFETY_HONDA_BOSCH_HARNESS 20U
#define SAFETY_SUBARU_LEGACY 22U

uint16_t current_safety_mode = SAFETY_SILENT;
const safety_hooks *current_hooks = &nooutput_hooks;
Expand Down Expand Up @@ -201,6 +202,7 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_HYUNDAI, &hyundai_hooks},
{SAFETY_CHRYSLER, &chrysler_hooks},
{SAFETY_SUBARU, &subaru_hooks},
{SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks},
{SAFETY_MAZDA, &mazda_hooks},
{SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks},
{SAFETY_NOOUTPUT, &nooutput_hooks},
Expand Down
128 changes: 88 additions & 40 deletions board/safety/safety_subaru.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,65 +8,85 @@ const int SUBARU_MAX_RATE_DOWN = 70;
const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60;
const int SUBARU_DRIVER_TORQUE_FACTOR = 10;

const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x164, 0}, {0x221, 0}, {0x322, 0}};
const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x221, 0}, {0x322, 0}};
const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}};
const int SUBARU_TX_MSGS_LEN = sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]);
const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_MSGS[0]);

// TODO: do checksum and counter checks after adding the signals to the outback dbc file
AddrCheckStruct subaru_rx_checks[] = {
{.addr = { 0x40, 0x140}, .bus = 0, .expected_timestep = 10000U},
{.addr = {0x119, 0x371}, .bus = 0, .expected_timestep = 20000U},
{.addr = {0x240, 0x144}, .bus = 0, .expected_timestep = 50000U},
{.addr = { 0x40}, .bus = 0, .expected_timestep = 10000U},
{.addr = {0x119}, .bus = 0, .expected_timestep = 20000U},
{.addr = {0x240}, .bus = 0, .expected_timestep = 50000U},
};
AddrCheckStruct subaru_l_rx_checks[] = {
{.addr = {0x140}, .bus = 0, .expected_timestep = 10000U},
{.addr = {0x371}, .bus = 0, .expected_timestep = 20000U},
{.addr = {0x144}, .bus = 0, .expected_timestep = 50000U},
};
const int SUBARU_RX_CHECK_LEN = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]);
const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]);

int subaru_cruise_engaged_last = 0;
int subaru_rt_torque_last = 0;
int subaru_desired_torque_last = 0;
uint32_t subaru_ts_last = 0;
bool subaru_gas_last = false;
bool subaru_global = false;
struct sample_t subaru_torque_driver; // last few driver torques measured

static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

bool valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN,
NULL, NULL, NULL);
bool valid = false;
if (subaru_global) {
valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN,
NULL, NULL, NULL);
} else {
valid = addr_safety_check(to_push, subaru_l_rx_checks, SUBARU_L_RX_CHECK_LEN,
NULL, NULL, NULL);
}

if (valid) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

if (((addr == 0x119) || (addr == 0x371)) && (bus == 0)){
int bit_shift = (addr == 0x119) ? 16 : 29;
int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&subaru_torque_driver, torque_driver_new);
}

// enter controls on rising edge of ACC, exit controls on ACC off
if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) {
int bit_shift = (addr == 0x240) ? 9 : 17;
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
if (bus == 0) {
if (((addr == 0x119) && subaru_global) ||
((addr == 0x371) && !subaru_global)) {
int bit_shift = subaru_global ? 16 : 29;
int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
update_sample(&subaru_torque_driver, torque_driver_new);
}
if (!cruise_engaged) {
controls_allowed = 0;

// enter controls on rising edge of ACC, exit controls on ACC off
if (((addr == 0x240) && subaru_global) ||
((addr == 0x144) && !subaru_global)) {
int bit_shift = subaru_global ? 9 : 17;
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
}
if (!cruise_engaged) {
controls_allowed = 0;
}
subaru_cruise_engaged_last = cruise_engaged;
}
subaru_cruise_engaged_last = cruise_engaged;
}

// exit controls on rising edge of gas press
if (((addr == 0x40) || (addr == 0x140)) && (bus == 0)) {
int byte = (addr == 0x40) ? 4 : 0;
bool gas = GET_BYTE(to_push, byte) != 0;
if (gas && !subaru_gas_last) {
controls_allowed = 0;
// exit controls on rising edge of gas press
if (((addr == 0x40) && subaru_global) ||
((addr == 0x140) && !subaru_global)) {
int byte = subaru_global ? 4 : 0;
bool gas = GET_BYTE(to_push, byte) != 0;
if (gas && !subaru_gas_last) {
controls_allowed = 0;
}
subaru_gas_last = gas;
}
subaru_gas_last = gas;
}

if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) {
relay_malfunction = true;
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
(((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) {
relay_malfunction = true;
}
}
}
return valid;
Expand All @@ -77,7 +97,8 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);

if (!msg_allowed(addr, bus, SUBARU_TX_MSGS, sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))) {
if ((!msg_allowed(addr, bus, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN) && subaru_global) ||
(!msg_allowed(addr, bus, SUBARU_L_TX_MSGS, SUBARU_L_TX_MSGS_LEN) && !subaru_global)) {
tx = 0;
}

Expand All @@ -86,8 +107,9 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}

// steer cmd checks
if ((addr == 0x122) || (addr == 0x164)) {
int bit_shift = (addr == 0x122) ? 16 : 8;
if (((addr == 0x122) && subaru_global) ||
((addr == 0x164) && !subaru_global)) {
int bit_shift = subaru_global ? 16 : 8;
int desired_torque = ((GET_BYTES_04(to_send) >> bit_shift) & 0x1FFF);
bool violation = 0;
uint32_t ts = TIM2->CNT;
Expand Down Expand Up @@ -151,7 +173,9 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// 545 is ES_Distance
// 802 is ES_LKAS
int addr = GET_ADDR(to_fwd);
int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
int block_msg = ((addr == 0x122) && subaru_global) ||
((addr == 0x164) && !subaru_global) ||
(addr == 0x221) || (addr == 0x322);
if (!block_msg) {
bus_fwd = 0; // Main CAN
}
Expand All @@ -161,12 +185,36 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
return bus_fwd;
}

static void subaru_init(int16_t param) {
UNUSED(param);
controls_allowed = false;
relay_malfunction = false;
subaru_global = true;
}

static void subaru_legacy_init(int16_t param) {
UNUSED(param);
controls_allowed = false;
relay_malfunction = false;
subaru_global = false;
}

const safety_hooks subaru_hooks = {
.init = nooutput_init,
.init = subaru_init,
.rx = subaru_rx_hook,
.tx = subaru_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.fwd = subaru_fwd_hook,
.addr_check = subaru_rx_checks,
.addr_check_len = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]),
};

const safety_hooks subaru_legacy_hooks = {
.init = subaru_legacy_init,
.rx = subaru_rx_hook,
.tx = subaru_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.fwd = subaru_fwd_hook,
.addr_check = subaru_l_rx_checks,
.addr_check_len = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]),
};
1 change: 1 addition & 0 deletions python/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,7 @@ class Panda(object):
SAFETY_GM_ASCM = 18
SAFETY_NOOUTPUT = 19
SAFETY_HONDA_BOSCH_HARNESS = 20
SAFETY_SUBARU_LEGACY = 22

SERIAL_DEBUG = 0
SERIAL_ESP = 1
Expand Down
1 change: 1 addition & 0 deletions tests/safety/libpandasafety_py.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,7 @@
void init_tests_subaru(void);
void set_subaru_desired_torque_last(int t);
void set_subaru_rt_torque_last(int t);
bool get_subaru_global(void);
void set_subaru_torque_driver(int min, int max);
void init_tests_volkswagen(void);
Expand Down
4 changes: 4 additions & 0 deletions tests/safety/test.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,10 @@ int get_hw_type(void){
return hw_type;
}

bool get_subaru_global(void){
return subaru_global;
}

void set_timer(uint32_t t){
timer.CNT = t;
}
Expand Down
56 changes: 40 additions & 16 deletions tests/safety/test_subaru.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@
DRIVER_TORQUE_ALLOWANCE = 60;
DRIVER_TORQUE_FACTOR = 10;

TX_MSGS = [[0x122, 0], [0x164, 0], [0x221, 0], [0x322, 0]]
TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]]
TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]]

def twos_comp(val, bits):
if val >= 0:
Expand All @@ -42,41 +43,58 @@ def _set_prev_torque(self, t):

def _torque_driver_msg(self, torque):
t = twos_comp(torque, 11)
to_send = make_msg(0, 0x119)
to_send[0].RDLR = ((t & 0x7FF) << 16)
if self.safety.get_subaru_global():
to_send = make_msg(0, 0x119)
to_send[0].RDLR = ((t & 0x7FF) << 16)
else:
to_send = make_msg(0, 0x371)
to_send[0].RDLR = ((t & 0x7FF) << 29)
return to_send

def _torque_msg(self, torque):
to_send = make_msg(0, 0x122)
t = twos_comp(torque, 13)
to_send[0].RDLR = (t << 16)
if self.safety.get_subaru_global():
to_send = make_msg(0, 0x122)
to_send[0].RDLR = (t << 16)
else:
to_send = make_msg(0, 0x164)
to_send[0].RDLR = (t << 8)
return to_send

def _gas_msg(self, gas):
to_send = make_msg(0, 0x40)
to_send[0].RDHR = gas & 0xFF
if self.safety.get_subaru_global():
to_send = make_msg(0, 0x40)
to_send[0].RDHR = gas & 0xFF
else:
to_send = make_msg(0, 0x140)
to_send[0].RDLR = gas & 0xFF
return to_send

def _cruise_msg(self, cruise):
if self.safety.get_subaru_global():
to_send = make_msg(0, 0x240)
to_send[0].RDHR = cruise << 9
else:
to_send = make_msg(0, 0x144)
to_send[0].RDHR = cruise << 17
return to_send

def test_spam_can_buses(self):
test_spam_can_buses(self, TX_MSGS)
test_spam_can_buses(self, TX_MSGS if self.safety.get_subaru_global() else TX_L_MSGS)

def test_relay_malfunction(self):
test_relay_malfunction(self, 0x122)
test_relay_malfunction(self, 0x122 if self.safety.get_subaru_global() else 0x164)

def test_default_controls_not_allowed(self):
self.assertFalse(self.safety.get_controls_allowed())

def test_enable_control_allowed_from_cruise(self):
to_push = make_msg(0, 0x240)
to_push[0].RDHR = 1 << 9
self.safety.safety_rx_hook(to_push)
self.safety.safety_rx_hook(self._cruise_msg(True))
self.assertTrue(self.safety.get_controls_allowed())

def test_disable_control_allowed_from_cruise(self):
to_push = make_msg(0, 0x240)
to_push[0].RDHR = 0
self.safety.set_controls_allowed(1)
self.safety.safety_rx_hook(to_push)
self.safety.safety_rx_hook(self._cruise_msg(False))
self.assertFalse(self.safety.get_controls_allowed())

def test_disengage_on_gas(self):
Expand Down Expand Up @@ -180,7 +198,7 @@ def test_realtime_limits(self):
def test_fwd_hook(self):
buss = list(range(0x0, 0x3))
msgs = list(range(0x1, 0x800))
blocked_msgs = [290, 356, 545, 802]
blocked_msgs = [290, 545, 802] if self.safety.get_subaru_global() else [356, 545, 802]
for b in buss:
for m in msgs:
if b == 0:
Expand All @@ -193,6 +211,12 @@ def test_fwd_hook(self):
# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))

class TestSubaruLegacySafety(TestSubaruSafety):
@classmethod
def setUp(cls):
cls.safety = libpandasafety_py.libpandasafety
cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0)
cls.safety.init_tests_subaru()

if __name__ == "__main__":
unittest.main()

0 comments on commit dfa6b07

Please sign in to comment.