diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index b2d84922aaa..132917a1ea0 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -20,7 +20,8 @@ AddrCheckStruct subaru_addr_checks[] = { {.msg = {{0x119, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, {.msg = {{0x13a, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, {.msg = {{0x13c, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, - {.msg = {{0x240, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}}, + {.msg = {{0x240, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, + {0x321, 2, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 100000U}, { 0 }}}, }; #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}; @@ -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_l_addr_checks, SUBARU_L_ADDR_CHECK_LEN}; +const int SUBARU_PARAM_FORESTER_HYBRID = 1; +bool subaru_forester_hybrid = false; + static uint32_t subaru_get_checksum(CANPacket_t *to_push) { return (uint8_t)GET_BYTE(to_push, 0); } @@ -60,8 +64,22 @@ 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); + int addr = GET_ADDR(to_push); + if (valid && (GET_BUS(to_push) == 2U) && subaru_forester_hybrid) { + // enter controls on rising edge of ACC, exit controls on ACC off (ES_DashStatus) + if (addr == 0x321) { + int cruise_engaged = GET_BIT(to_push, 36U) != 0; + if (cruise_engaged && !cruise_engaged_prev) { + controls_allowed = 1; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + cruise_engaged_prev = cruise_engaged; + } + } + if (valid && (GET_BUS(to_push) == 0U)) { - int addr = GET_ADDR(to_push); if (addr == 0x119) { int torque_driver_new; torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FFU); @@ -70,8 +88,8 @@ static int subaru_rx_hook(CANPacket_t *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x240) { - int cruise_engaged = ((GET_BYTES_48(to_push) >> 9) & 1U); + if (addr == 0x240 && !subaru_forester_hybrid) { + int cruise_engaged = GET_BIT(to_push, 41U) != 0U; if (cruise_engaged && !cruise_engaged_prev) { controls_allowed = 1; } @@ -314,7 +332,8 @@ static int subaru_legacy_fwd_hook(int bus_num, CANPacket_t *to_fwd) { } static const addr_checks* subaru_init(uint16_t param) { - UNUSED(param); + subaru_forester_hybrid = GET_FLAG(param, SUBARU_PARAM_FORESTER_HYBRID); + return &subaru_rx_checks; } diff --git a/python/__init__.py b/python/__init__.py index c20da46a3a2..190999ba53c 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -197,6 +197,8 @@ class Panda: FLAG_TESLA_POWERTRAIN = 1 FLAG_TESLA_LONG_CONTROL = 2 + FLAG_SUBARU_FORESTER_HYBRID = 1 + def __init__(self, serial: Optional[str] = None, claim: bool = True): self._serial = serial self._handle = None diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index 040d58c2691..30d6a3dae9d 100755 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -73,5 +73,19 @@ def _pcm_status_msg(self, enable): return self.packer.make_can_msg_panda("CruiseControl", 0, values) +class TestSubaruForesterHybridSafety(TestSubaruSafety): + + 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_FORESTER_HYBRID) + self.safety.init_tests() + + def _pcm_status_msg(self, enable): + values = {"Cruise_Activated": enable, "Counter": self.cnt_cruise % 4} + self.__class__.cnt_cruise += 1 + return self.packer.make_can_msg_panda("ES_DashStatus", 2, values) + + if __name__ == "__main__": unittest.main()