diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 7008bf8419..c4ee826e41 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -12,7 +12,9 @@ #define TOYOTA_COMMON_SECOC_TX_MSGS \ TOYOTA_BASE_TX_MSGS \ - {0x2E4, 0, 8}, {0x131, 0, 8}, \ + {0x2E4, 0, 8}, {0x131, 0, 8}, /* STEERING_LKA (longer message for SecOC), STEERING_LTA_2 */ \ + {0x183, 0, 8}, {0x411, 0, 8}, /* ACC_CONTROL_2, PCS_HUD */ \ + {0x750, 0, 8}, /* radar diagnostic address */ \ #define TOYOTA_COMMON_LONG_TX_MSGS \ TOYOTA_COMMON_TX_MSGS \ @@ -140,8 +142,8 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { } bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA - if (!toyota_stock_longitudinal && (addr == 0x343)) { - stock_ecu_detected = true; // ACC_CONTROL + if (!toyota_stock_longitudinal && ((addr == 0x343) || (toyota_secoc && (addr == 0x183)))) { + stock_ecu_detected = true; // ACC_CONTROL or ACC_CONTROL_2 } generic_rx_checks(stock_ecu_detected); } @@ -198,6 +200,7 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { desired_accel = to_signed(desired_accel, 16); bool violation = false; + violation |= toyota_secoc && (desired_accel != TOYOTA_LONG_LIMITS.inactive_accel); // SecOC cars move this signal to 0x183 violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS); // only ACC messages that cancel are allowed when openpilot is not controlling longitudinal @@ -216,6 +219,18 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { } } + if (addr == 0x183) { + int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1); + desired_accel = to_signed(desired_accel, 16); + + bool violation = !toyota_secoc; // Only SecOC cars may transmit this message + violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS); + + if (violation) { + tx = false; + } + } + // AEB: block all actuation. only used when DSU is unplugged if (addr == 0x283) { // only allow the checksum, which is the last byte @@ -349,12 +364,10 @@ static safety_config toyota_init(uint16_t param) { toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR; safety_config ret; - if (toyota_stock_longitudinal) { - if (toyota_secoc) { - SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret); - } else { - SET_TX_MSGS(TOYOTA_TX_MSGS, ret); - } + if (toyota_secoc) { + SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret); + } else if (toyota_stock_longitudinal) { + SET_TX_MSGS(TOYOTA_TX_MSGS, ret); } else { SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); } @@ -389,10 +402,11 @@ static int toyota_fwd_hook(int bus_num, int addr) { // block stock lkas messages and stock acc messages (if OP is doing ACC) // in TSS2, 0x191 is LTA which we need to block to avoid controls collision bool is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191)); - // on SecOC cars 0x131 is also LTA - is_lkas_msg |= toyota_secoc && (addr == 0x131); // in TSS2 the camera does ACC as well, so filter 0x343 bool is_acc_msg = (addr == 0x343); + // SecOC cars use additional (not alternate) messages for lateral and longitudinal actuation + is_lkas_msg |= toyota_secoc && (addr == 0x131); + is_acc_msg |= toyota_secoc && (addr == 0x183); bool block_msg = is_lkas_msg || (is_acc_msg && !toyota_stock_longitudinal); if (!block_msg) { bus_fwd = 0; diff --git a/tests/safety/common.py b/tests/safety/common.py index 884f21a360..e817708823 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -134,6 +134,20 @@ def setUpClass(cls): def _accel_msg(self, accel: float): pass + def _accel_msg_2(self, accel: float): + return None + + def _should_tx_1(self, controls_allowed: bool, stock_longitudinal: bool, accel: float, min_accel: float, max_accel: float): + if stock_longitudinal: + should_tx = False + else: + should_tx = controls_allowed and min_accel <= accel <= max_accel + should_tx = should_tx or accel == self.INACTIVE_ACCEL + return should_tx + + def _should_tx_2(self, controls_allowed: bool, stock_longitudinal: bool, accel: float, min_accel: float, max_accel: float): + return False + def test_accel_limits_correct(self): self.assertGreater(self.MAX_ACCEL, 0) self.assertLess(self.MIN_ACCEL, 0) @@ -149,12 +163,12 @@ def test_accel_actuation_limits(self, stock_longitudinal=False): for controls_allowed in [True, False]: self.safety.set_controls_allowed(controls_allowed) self.safety.set_alternative_experience(alternative_experience) - if stock_longitudinal: - should_tx = False - else: - should_tx = controls_allowed and min_accel <= accel <= max_accel - should_tx = should_tx or accel == self.INACTIVE_ACCEL + should_tx = self._should_tx_1(controls_allowed, stock_longitudinal, accel, min_accel, max_accel) self.assertEqual(should_tx, self._tx(self._accel_msg(accel))) + accel_msg_2 = self._accel_msg_2(accel) + if accel_msg_2 is not None: + should_tx_2 = self._should_tx_2(controls_allowed, stock_longitudinal, accel, min_accel, max_accel) + self.assertEqual(should_tx_2, self._tx(self._accel_msg_2(accel))) class LongitudinalGasBrakeSafetyTest(PandaSafetyTestBase, abc.ABC): diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index e60b29c5c2..010aff36f3 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -10,7 +10,7 @@ from panda.tests.safety.common import CANPackerPanda TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]] # LKAS + LTA + ACC & PCM cancel cmds -TOYOTA_SECOC_TX_MSGS = [[0x131, 0]] + TOYOTA_COMMON_TX_MSGS +TOYOTA_SECOC_TX_MSGS = [[0x131, 0], [0x183, 0], [0x411, 0], [0x750, 0]] + TOYOTA_COMMON_TX_MSGS TOYOTA_COMMON_LONG_TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 [0x411, 0], # PCS_HUD @@ -326,18 +326,22 @@ def setUp(self): self.safety.init_tests() -class TestToyotaSecOcSafety(TestToyotaStockLongitudinalBase): +class TestToyotaSecOcSafety(TestToyotaSafetyBase): TX_MSGS = TOYOTA_SECOC_TX_MSGS - RELAY_MALFUNCTION_ADDRS = {0: (0x2E4,)} - FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x131]} + RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x343, 0x183)} + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x131, 0x343, 0x183]} def setUp(self): self.packer = CANPackerPanda("toyota_rav4_prime_generated") self.safety = libpanda_py.libpanda - self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL | Panda.FLAG_TOYOTA_SECOC) + self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_SECOC) self.safety.init_tests() + @unittest.skip("test not applicable for cars without a DSU") + def test_block_aeb(self, stock_longitudinal: bool = False): + pass + # This platform also has alternate brake and PCM messages, but same naming in the DBC, so same packers work def _user_gas_msg(self, gas): @@ -358,6 +362,21 @@ def test_lta_2_steer_cmd(self): should_tx = not req and not req2 and angle == 0 self.assertEqual(should_tx, self._tx(self._lta_2_msg(req, req2, angle)), f"{req=} {req2=} {angle=}") + def _accel_msg_2(self, accel, cancel_req=0): + values = {"ACCEL_CMD": accel} + return self.packer.make_can_msg_panda("ACC_CONTROL_2", 0, values) + + # On a SecOC vehicle, we still transmit ACC_CONTROL but the accel value moves to ACC_CONTROL_2 + # Verify that all non-idle accel values in ACC_CONTROL are rejected, verify ACC_CONTROL_2 accel normally + def _should_tx_1(self, controls_allowed: bool, stock_longitudinal: bool, accel: float, min_accel: float, max_accel: float): + return accel == self.INACTIVE_ACCEL + + def _should_tx_2(self, controls_allowed: bool, stock_longitudinal: bool, accel: float, min_accel: float, max_accel: float): + return (controls_allowed and min_accel <= accel <= max_accel) or accel == self.INACTIVE_ACCEL + + def test_accel_actuation_limits(self, stock_longitudinal=False): + super().test_accel_actuation_limits(stock_longitudinal=stock_longitudinal) + if __name__ == "__main__": unittest.main()