Toyota: Add SECOC long support

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;

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):

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):
+    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()