separating subary legacy safety mode from global (#452)
* separating subary legacy safety mode from global * added explicit tests for subaru legacy, which were absent beforemaster
parent
dad5858b8c
commit
dfa6b079de
|
@ -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;
|
||||
|
@ -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},
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
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;
|
||||
// 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_gas_last = gas;
|
||||
}
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) {
|
||||
relay_malfunction = true;
|
||||
// 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;
|
||||
}
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
|
||||
(((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return valid;
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
}
|
||||
|
@ -161,8 +185,22 @@ 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,
|
||||
|
@ -170,3 +208,13 @@ const safety_hooks subaru_hooks = {
|
|||
.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]),
|
||||
};
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -88,6 +88,7 @@ void set_chrysler_torque_meas(int min, int max);
|
|||
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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -15,7 +15,8 @@ RT_INTERVAL = 250000
|
|||
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:
|
||||
|
@ -42,41 +43,58 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
|
||||
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):
|
||||
|
@ -180,7 +198,7 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
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:
|
||||
|
@ -193,6 +211,12 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
# 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()
|
||||
|
|
Loading…
Reference in New Issue