diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 7527992c..1373180e 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -1,4 +1,4 @@ -const int SUBARU_MAX_STEER = 2047; // 1s +const int SUBARU_MAX_STEER = 2047; // 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 @@ -6,15 +6,29 @@ const uint32_t SUBARU_RT_INTERVAL = 250000; // 250ms between real time checks 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_DRIVER_TORQUE_FACTOR = 50; const int SUBARU_STANDSTILL_THRSLD = 20; // about 1kph -const int SUBARU_L_DRIVER_TORQUE_ALLOWANCE = 75; -const int SUBARU_L_DRIVER_TORQUE_FACTOR = 10; +const int SUBARU_GEN2_MAX_STEER = 1000; +const int SUBARU_GEN2_MAX_RATE_UP = 40; +const int SUBARU_GEN2_MAX_RATE_DOWN = 40; -const CanMsg SUBARU_TX_MSGS[] = {{0x122, 0, 8}, {0x221, 0, 8}, {0x322, 0, 8}}; +const CanMsg SUBARU_TX_MSGS[] = { + {0x122, 0, 8}, + {0x221, 0, 8}, + {0x321, 0, 8}, + {0x322, 0, 8} +}; #define SUBARU_TX_MSGS_LEN (sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0])) +const CanMsg SUBARU_GEN2_TX_MSGS[] = { + {0x122, 0, 8}, + {0x221, 1, 8}, + {0x321, 0, 8}, + {0x322, 0, 8} +}; +#define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0])) + AddrCheckStruct subaru_addr_checks[] = { {.msg = {{ 0x40, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, {.msg = {{0x119, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, @@ -25,6 +39,21 @@ AddrCheckStruct subaru_addr_checks[] = { #define SUBARU_ADDR_CHECK_LEN (sizeof(subaru_addr_checks) / sizeof(subaru_addr_checks[0])) addr_checks subaru_rx_checks = {subaru_addr_checks, SUBARU_ADDR_CHECK_LEN}; +AddrCheckStruct subaru_gen2_addr_checks[] = { + {.msg = {{ 0x40, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, + {.msg = {{0x119, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{0x13a, 1, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{0x13c, 1, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, + {.msg = {{0x240, 1, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}}, +}; +#define SUBARU_GEN2_ADDR_CHECK_LEN (sizeof(subaru_gen2_addr_checks) / sizeof(subaru_gen2_addr_checks[0])) +addr_checks subaru_gen2_rx_checks = {subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_CHECK_LEN}; + + +const uint16_t SUBARU_PARAM_GEN2 = 1; +bool subaru_gen2 = false; + + static uint32_t subaru_get_checksum(CANPacket_t *to_push) { return (uint8_t)GET_BYTE(to_push, 0); } @@ -46,11 +75,14 @@ static uint32_t subaru_compute_checksum(CANPacket_t *to_push) { static int subaru_rx_hook(CANPacket_t *to_push) { bool valid = addr_safety_check(to_push, &subaru_rx_checks, - subaru_get_checksum, subaru_compute_checksum, subaru_get_counter); + subaru_get_checksum, subaru_compute_checksum, subaru_get_counter); + + if (valid) { + const int bus = GET_BUS(to_push); + const int alt_bus = subaru_gen2 ? 1 : 0; - if (valid && (GET_BUS(to_push) == 0U)) { int addr = GET_ADDR(to_push); - if (addr == 0x119) { + if ((addr == 0x119) && (bus == 0)) { int torque_driver_new; torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FFU); torque_driver_new = -1 * to_signed(torque_driver_new, 11); @@ -58,7 +90,7 @@ static int subaru_rx_hook(CANPacket_t *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x240) { + if ((addr == 0x240) && (bus == alt_bus)) { int cruise_engaged = ((GET_BYTES_48(to_push) >> 9) & 1U); if (cruise_engaged && !cruise_engaged_prev) { controls_allowed = 1; @@ -70,21 +102,21 @@ static int subaru_rx_hook(CANPacket_t *to_push) { } // sample wheel speed, averaging opposite corners - if (addr == 0x13a) { + if ((addr == 0x13a) && (bus == alt_bus)) { int subaru_speed = ((GET_BYTES_04(to_push) >> 12) & 0x1FFFU) + ((GET_BYTES_48(to_push) >> 6) & 0x1FFFU); // FR + RL subaru_speed /= 2; vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD; } - if (addr == 0x13c) { + if ((addr == 0x13c) && (bus == alt_bus)) { brake_pressed = ((GET_BYTE(to_push, 7) >> 6) & 1U); } - if (addr == 0x40) { + if ((addr == 0x40) && (bus == 0)) { gas_pressed = GET_BYTE(to_push, 4) != 0U; } - generic_rx_checks((addr == 0x122)); + generic_rx_checks((addr == 0x122) && (bus == 0)); } return valid; } @@ -95,8 +127,10 @@ static int subaru_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { int tx = 1; int addr = GET_ADDR(to_send); - if (!msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN)) { - tx = 0; + if (subaru_gen2) { + tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN); + } else { + tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN); } // steer cmd checks @@ -108,13 +142,16 @@ static int subaru_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) { desired_torque = -1 * to_signed(desired_torque, 13); if (controls_allowed) { + const int max_steer = subaru_gen2 ? SUBARU_GEN2_MAX_STEER : SUBARU_MAX_STEER; + const int max_rate_up = subaru_gen2 ? SUBARU_GEN2_MAX_RATE_UP : SUBARU_MAX_RATE_UP; + const int max_rate_down = subaru_gen2 ? SUBARU_GEN2_MAX_RATE_DOWN : SUBARU_MAX_RATE_DOWN; // *** global torque limit check *** - violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER); + violation |= max_limit_check(desired_torque, max_steer, -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, + max_steer, max_rate_up, max_rate_down, SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR); // used next time @@ -155,17 +192,17 @@ static int subaru_fwd_hook(int bus_num, CANPacket_t *to_fwd) { int bus_fwd = -1; if (bus_num == 0) { - bus_fwd = 2; // Camera CAN + bus_fwd = 2; // forward to camera } if (bus_num == 2) { // Global platform // 0x122 ES_LKAS - // 0x221 ES_Distance + // 0x321 ES_DashStatus // 0x322 ES_LKAS_State int addr = GET_ADDR(to_fwd); - int block_msg = ((addr == 0x122) || (addr == 0x221) || (addr == 0x322)); - if (!block_msg) { + bool block_lkas = (addr == 0x122) || (addr == 0x321) || (addr == 0x322); + if (!block_lkas) { bus_fwd = 0; // Main CAN } } @@ -174,7 +211,14 @@ static int subaru_fwd_hook(int bus_num, CANPacket_t *to_fwd) { } static const addr_checks* subaru_init(uint16_t param) { - UNUSED(param); + subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2); + + if (subaru_gen2) { + subaru_rx_checks = (addr_checks){subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_CHECK_LEN}; + } else { + subaru_rx_checks = (addr_checks){subaru_addr_checks, SUBARU_ADDR_CHECK_LEN}; + } + return &subaru_rx_checks; } diff --git a/board/safety/safety_subaru_legacy.h b/board/safety/safety_subaru_legacy.h index 8ab746a8..c2640a1a 100644 --- a/board/safety/safety_subaru_legacy.h +++ b/board/safety/safety_subaru_legacy.h @@ -1,6 +1,11 @@ -// constants are in board/safety/safety_subaru.h +// more constants in board/safety/safety_subaru.h +const int SUBARU_L_DRIVER_TORQUE_ALLOWANCE = 75; +const int SUBARU_L_DRIVER_TORQUE_FACTOR = 10; -const CanMsg SUBARU_L_TX_MSGS[] = {{0x161, 0, 8}, {0x164, 0, 8}}; +const CanMsg SUBARU_L_TX_MSGS[] = { + {0x161, 0, 8}, + {0x164, 0, 8} +}; #define 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 diff --git a/python/__init__.py b/python/__init__.py index 3b3ecd80..fdc80e2a 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -200,6 +200,8 @@ class Panda: FLAG_CHRYSLER_RAM_DT = 1 + FLAG_SUBARU_GEN2 = 1 + def __init__(self, serial: Optional[str] = None, claim: bool = True): self._serial = serial self._handle = None diff --git a/tests/safety/common.py b/tests/safety/common.py index a87900dc..389ae8ea 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -564,6 +564,8 @@ class PandaSafetyTest(PandaSafetyTestBase): continue if {attr, current_test}.issubset({'TestToyotaSafety', 'TestToyotaAltBrakeSafety', 'TestToyotaStockLongitudinal'}): continue + if {attr, current_test}.issubset({'TestSubaruSafety', 'TestSubaruGen2Safety'}): + continue # TODO: Temporary, should be fixed in panda firmware, safety_honda.h if attr.startswith('TestHonda'): diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index f1cf4a6e..82e95813 100755 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -7,11 +7,11 @@ from panda.tests.safety.common import CANPackerPanda class TestSubaruSafety(common.PandaSafetyTest, common.DriverTorqueSteeringSafetyTest): - TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]] + TX_MSGS = [[0x122, 0], [0x221, 0], [0x321, 0], [0x322, 0]] STANDSTILL_THRESHOLD = 20 # 1kph (see dbc file) RELAY_MALFUNCTION_ADDR = 0x122 RELAY_MALFUNCTION_BUS = 0 - FWD_BLACKLISTED_ADDRS = {2: [0x122, 0x221, 0x322]} + FWD_BLACKLISTED_ADDRS = {2: [0x122, 0x321, 0x322]} FWD_BUS_LOOKUP = {0: 2, 2: 0} MAX_RATE_UP = 50 @@ -22,7 +22,9 @@ class TestSubaruSafety(common.PandaSafetyTest, common.DriverTorqueSteeringSafety RT_INTERVAL = 250000 DRIVER_TORQUE_ALLOWANCE = 60 - DRIVER_TORQUE_FACTOR = 10 + DRIVER_TORQUE_FACTOR = 50 + + ALT_BUS = 0 def setUp(self): self.packer = CANPackerPanda("subaru_global_2017_generated") @@ -42,11 +44,11 @@ class TestSubaruSafety(common.PandaSafetyTest, common.DriverTorqueSteeringSafety def _speed_msg(self, speed): # subaru safety doesn't use the scaled value, so undo the scaling values = {s: speed * 0.057 for s in ["FR", "FL", "RR", "RL"]} - return self.packer.make_can_msg_panda("Wheel_Speeds", 0, values) + return self.packer.make_can_msg_panda("Wheel_Speeds", self.ALT_BUS, values) def _user_brake_msg(self, brake): values = {"Brake": brake} - return self.packer.make_can_msg_panda("Brake_Status", 0, values) + return self.packer.make_can_msg_panda("Brake_Status", self.ALT_BUS, values) def _torque_cmd_msg(self, torque, steer_req=1): values = {"LKAS_Output": torque} @@ -58,7 +60,22 @@ class TestSubaruSafety(common.PandaSafetyTest, common.DriverTorqueSteeringSafety def _pcm_status_msg(self, enable): values = {"Cruise_Activated": enable} - return self.packer.make_can_msg_panda("CruiseControl", 0, values) + return self.packer.make_can_msg_panda("CruiseControl", self.ALT_BUS, values) + + +class TestSubaruGen2Safety(TestSubaruSafety): + TX_MSGS = [[0x122, 0], [0x221, 1], [0x321, 0], [0x322, 0]] + ALT_BUS = 1 + + MAX_RATE_UP = 40 + MAX_RATE_DOWN = 40 + MAX_TORQUE = 1000 + + def setUp(self): + self.packer = CANPackerPanda("subaru_global_2017_generated") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_SUBARU, Panda.FLAG_SUBARU_GEN2) + self.safety.init_tests() if __name__ == "__main__": diff --git a/tests/safety_replay/test_safety_replay.py b/tests/safety_replay/test_safety_replay.py index 99bdcdf2..07209db7 100755 --- a/tests/safety_replay/test_safety_replay.py +++ b/tests/safety_replay/test_safety_replay.py @@ -13,21 +13,22 @@ BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" ReplayRoute = namedtuple("ReplayRoute", ("route", "safety_mode", "param", "alternative_experience"), defaults=(0, 0)) logs = [ - ReplayRoute("2425568437959f9d|2019-12-22--16-24-37.bz2", Panda.SAFETY_HONDA_NIDEC), # HONDA.CIVIC (fcw presents: 0x1FA blocked as expected) - ReplayRoute("38bfd238edecbcd7|2019-06-07--10-15-25.bz2", Panda.SAFETY_TOYOTA, 66), # TOYOTA.PRIUS - ReplayRoute("f89c604cf653e2bf|2018-09-29--13-46-50.bz2", Panda.SAFETY_GM), # GM.VOLT - ReplayRoute("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER), # CHRYSLER.PACIFICA_2018_HYBRID - ReplayRoute("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU), # SUBARU.IMPREZA - ReplayRoute("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB), # VOLKSWAGEN.GOLF (MK7) - ReplayRoute("d12cd943127f267b|2020-03-27--15-57-18.bz2", Panda.SAFETY_VOLKSWAGEN_PQ), # 2009 VW Passat R36 (B6), supporting OP port not yet upstreamed - ReplayRoute("fbbfa6af821552b9|2020-03-03--08-09-43.bz2", Panda.SAFETY_NISSAN), # NISSAN.XTRAIL - ReplayRoute("5b7c365c50084530_2020-04-15--16-13-24--3--rlog.bz2", Panda.SAFETY_HYUNDAI), # HYUNDAI.SONATA - ReplayRoute("610ebb9faaad6b43|2020-06-13--15-28-36.bz2", Panda.SAFETY_HYUNDAI_LEGACY), # HYUNDAI.IONIQ_EV_LTD - ReplayRoute("5ab784f361e19b78_2020-06-08--16-30-41.bz2", Panda.SAFETY_SUBARU_LEGACY), # SUBARU.OUTBACK - ReplayRoute("bb50caf5f0945ab1|2021-06-19--17-20-18.bz2", Panda.SAFETY_TESLA), # TESLA.AP2_MODELS - ReplayRoute("bd6a637565e91581_2021-10-29--22-18-31--1--rlog.bz2", Panda.SAFETY_MAZDA), # MAZDA.CX9_2021 - # HONDA.CIVIC_2022 - ReplayRoute("1a5d045d2c531a6d_2022-06-07--22-03-00--1--rlog.bz2", Panda.SAFETY_HONDA_BOSCH, Panda.FLAG_HONDA_RADARLESS, ALT_EXP.DISABLE_DISENGAGE_ON_GAS), + ReplayRoute("2425568437959f9d|2019-12-22--16-24-37.bz2", Panda.SAFETY_HONDA_NIDEC), # HONDA.CIVIC (fcw presents: 0x1FA blocked as expected) + ReplayRoute("38bfd238edecbcd7|2019-06-07--10-15-25.bz2", Panda.SAFETY_TOYOTA, 66), # TOYOTA.PRIUS + ReplayRoute("f89c604cf653e2bf|2018-09-29--13-46-50.bz2", Panda.SAFETY_GM), # GM.VOLT + ReplayRoute("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER), # CHRYSLER.PACIFICA_2018_HYBRID + ReplayRoute("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU, # SUBARU.OUTBACK + Panda.FLAG_SUBARU_GEN2), + ReplayRoute("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB), # VOLKSWAGEN.GOLF (MK7) + ReplayRoute("d12cd943127f267b|2020-03-27--15-57-18.bz2", Panda.SAFETY_VOLKSWAGEN_PQ), # 2009 VW Passat R36 (B6), supporting OP port not yet upstreamed + ReplayRoute("fbbfa6af821552b9|2020-03-03--08-09-43.bz2", Panda.SAFETY_NISSAN), # NISSAN.XTRAIL + ReplayRoute("5b7c365c50084530_2020-04-15--16-13-24--3--rlog.bz2", Panda.SAFETY_HYUNDAI), # HYUNDAI.SONATA + ReplayRoute("610ebb9faaad6b43|2020-06-13--15-28-36.bz2", Panda.SAFETY_HYUNDAI_LEGACY), # HYUNDAI.IONIQ_EV_LTD + ReplayRoute("5ab784f361e19b78_2020-06-08--16-30-41.bz2", Panda.SAFETY_SUBARU_LEGACY), # SUBARU.OUTBACK_PREGLOBAL + ReplayRoute("bb50caf5f0945ab1|2021-06-19--17-20-18.bz2", Panda.SAFETY_TESLA), # TESLA.AP2_MODELS + ReplayRoute("bd6a637565e91581_2021-10-29--22-18-31--1--rlog.bz2", Panda.SAFETY_MAZDA), # MAZDA.CX9_2021 + ReplayRoute("1a5d045d2c531a6d_2022-06-07--22-03-00--1--rlog.bz2", Panda.SAFETY_HONDA_BOSCH, # HONDA.CIVIC_2022 + Panda.FLAG_HONDA_RADARLESS, ALT_EXP.DISABLE_DISENGAGE_ON_GAS), ]