Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Subaru safety param for Impreza/Crosstrek 2020 #650

Closed
wants to merge 1 commit into from
Closed

Subaru safety param for Impreza/Crosstrek 2020 #650

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 23 additions & 8 deletions board/safety/safety_subaru.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
const int SUBARU_MAX_STEER = 2047; // 1s
const int SUBARU_MAX_STEER_2020 = 1439; // lower limit for Impreza/Crosstrek 2020+
// real time torque limit to prevent controls spamming
// the real time limit is 1500/sec
const int SUBARU_MAX_RT_DELTA = 940; // max delta torque allowed for real time checks
Expand Down Expand Up @@ -37,6 +38,9 @@ AddrCheckStruct subaru_l_addr_checks[] = {
#define SUBARU_L_ADDR_CHECK_LEN (sizeof(subaru_l_addr_checks) / sizeof(subaru_l_addr_checks[0]))
addr_checks subaru_l_rx_checks = {subaru_addr_checks, SUBARU_L_ADDR_CHECK_LEN};

const uint16_t SUBARU_PARAM_MAX_STEER_2020 = 1;
bool subaru_max_steer_2020 = false;

static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
}
Expand Down Expand Up @@ -170,13 +174,23 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

if (controls_allowed) {

// *** global torque limit check ***
violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER);

// *** torque rate limit check ***
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN,
SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR);
if (subaru_max_steer_2020) {
// *** global torque limit check ***
violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER_2020, -SUBARU_MAX_STEER_2020);
// *** torque rate limit check ***
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
SUBARU_MAX_STEER_2020, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN,
SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR);
}
else {
// *** global torque limit check ***
violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER);

// *** torque rate limit check ***
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN,
SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR);
}

// used next time
desired_torque_last = desired_torque;
Expand Down Expand Up @@ -322,9 +336,10 @@ static int subaru_legacy_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd)
}

static const addr_checks* subaru_init(int16_t param) {
UNUSED(param);
controls_allowed = false;
relay_malfunction_reset();
// Checking for lower max steer from safety parameter
subaru_max_steer_2020 = GET_FLAG(param, SUBARU_PARAM_MAX_STEER_2020);
return &subaru_rx_checks;
}

Expand Down
35 changes: 23 additions & 12 deletions tests/safety/test_subaru.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@

MAX_RATE_UP = 50
MAX_RATE_DOWN = 70
MAX_STEER = 2047

MAX_RT_DELTA = 940
RT_INTERVAL = 250000
Expand All @@ -24,6 +23,8 @@ class TestSubaruSafety(common.PandaSafetyTest):
cnt_speed = 0
cnt_brake = 0

MAX_STEER = 2047

TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]]
STANDSTILL_THRESHOLD = 20 # 1kph (see dbc file)
RELAY_MALFUNCTION_ADDR = 0x122
Expand Down Expand Up @@ -82,7 +83,7 @@ def test_steer_safety_check(self):
for t in range(-3000, 3000):
self.safety.set_controls_allowed(enabled)
self._set_prev_torque(t)
block = abs(t) > MAX_STEER or (not enabled and abs(t) > 0)
block = abs(t) > self.MAX_STEER or (not enabled and abs(t) > 0)
self.assertEqual(not block, self._tx(self._torque_msg(t)))

def test_non_realtime_limit_up(self):
Expand Down Expand Up @@ -111,19 +112,19 @@ def test_against_torque_driver(self):
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
t *= -sign
self._set_torque_driver(t, t)
self._set_prev_torque(MAX_STEER * sign)
self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign)))
self._set_prev_torque(self.MAX_STEER * sign)
self.assertTrue(self._tx(self._torque_msg(self.MAX_STEER * sign)))

self._set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
self.assertFalse(self._tx(self._torque_msg(-MAX_STEER)))
self.assertFalse(self._tx(self._torque_msg(-self.MAX_STEER)))

# arbitrary high driver torque to ensure max steer torque is allowed
max_driver_torque = int(MAX_STEER / DRIVER_TORQUE_FACTOR + DRIVER_TORQUE_ALLOWANCE + 1)
max_driver_torque = int(self.MAX_STEER / DRIVER_TORQUE_FACTOR + DRIVER_TORQUE_ALLOWANCE + 1)

# spot check some individual cases
for sign in [-1, 1]:
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
torque_desired = (self.MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
delta = 1 * sign
self._set_prev_torque(torque_desired)
self._set_torque_driver(-driver_torque, -driver_torque)
Expand All @@ -132,15 +133,15 @@ def test_against_torque_driver(self):
self._set_torque_driver(-driver_torque, -driver_torque)
self.assertFalse(self._tx(self._torque_msg(torque_desired + delta)))

self._set_prev_torque(MAX_STEER * sign)
self._set_prev_torque(self.MAX_STEER * sign)
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
self.assertTrue(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
self._set_prev_torque(MAX_STEER * sign)
self.assertTrue(self._tx(self._torque_msg((self.MAX_STEER - MAX_RATE_DOWN) * sign)))
self._set_prev_torque(self.MAX_STEER * sign)
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
self.assertTrue(self._tx(self._torque_msg(0)))
self._set_prev_torque(MAX_STEER * sign)
self._set_prev_torque(self.MAX_STEER * sign)
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
self.assertFalse(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
self.assertFalse(self._tx(self._torque_msg((self.MAX_STEER - MAX_RATE_DOWN + 1) * sign)))

def test_realtime_limits(self):
self.safety.set_controls_allowed(True)
Expand All @@ -165,5 +166,15 @@ def test_realtime_limits(self):
self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA + 1))))


class TestSubaru2020Safety(TestSubaruSafety):
MAX_STEER = 1439

def setUp(self):
self.packer = CANPackerPanda("subaru_global_2017_generated")
self.safety = libpandasafety_py.libpandasafety
self.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 1)
self.safety.init_tests()


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