Toyota: Add SECOC longitudinal control

opendbc/car/tests/routes.py

@@ -223,6 +223,7 @@ class CarTestRoute(NamedTuple):
   CarTestRoute("7e34a988419b5307/2019-12-18--19-13-30", TOYOTA.TOYOTA_RAV4_TSS2),  # hybrid
   CarTestRoute("2475fb3eb2ffcc2e/2022-04-29--12-46-23", TOYOTA.TOYOTA_RAV4_TSS2_2022),  # hybrid
   CarTestRoute("20ba9ade056a8c7b/2021-02-08--21-57-35", TOYOTA.TOYOTA_RAV4_PRIME),  # SecOC
+  CarTestRoute("41ba5b181f29435d/00000001--e3ae76382f", TOYOTA.TOYOTA_RAV4_PRIME),  # SecOC longitudinal
   CarTestRoute("8bfb000e03b2a257/00000004--f9eee5f52e", TOYOTA.TOYOTA_SIENNA_4TH_GEN),  # SecOC
   CarTestRoute("0b54d0594d924cd9/00000057--b6206a3205", TOYOTA.TOYOTA_YARIS),  # SecOC
   CarTestRoute("7a31f030957b9c85/2023-04-01--14-12-51", TOYOTA.LEXUS_ES),

opendbc/car/toyota/carcontroller.py

@@ -76,6 +76,7 @@ def __init__(self, dbc_names, CP):
 
     self.secoc_lka_message_counter = 0
     self.secoc_lta_message_counter = 0
+    self.secoc_acc_message_counter = 0
     self.secoc_prev_reset_counter = 0
 
   def update(self, CC, CS, now_nanos):
@@ -97,6 +98,7 @@ def update(self, CC, CS, now_nanos):
       if CS.secoc_synchronization['RESET_CNT'] != self.secoc_prev_reset_counter:
         self.secoc_lka_message_counter = 0
         self.secoc_lta_message_counter = 0
+        self.secoc_acc_message_counter = 0
         self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT']
 
         expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT']))
@@ -250,8 +252,19 @@ def update(self, CC, CS, now_nanos):
 
         pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
 
-        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
+        main_accel_cmd = 0. if self.CP.flags & ToyotaFlags.SECOC.value else pcm_accel_cmd
+        can_sends.append(toyotacan.create_accel_command(self.packer, main_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
                                                         CS.acc_type, fcw_alert, self.distance_button))
+        if self.CP.flags & ToyotaFlags.SECOC.value:
+          acc_cmd_2 = toyotacan.create_accel_command_2(self.packer, pcm_accel_cmd)
+          acc_cmd_2 = add_mac(self.secoc_key,
+                              int(CS.secoc_synchronization['TRIP_CNT']),
+                              int(CS.secoc_synchronization['RESET_CNT']),
+                              self.secoc_acc_message_counter,
+                              acc_cmd_2)
+          self.secoc_acc_message_counter += 1
+          can_sends.append(acc_cmd_2)
+
         self.accel = pcm_accel_cmd
 
     else:

opendbc/car/toyota/carstate.py

@@ -6,7 +6,8 @@
 from opendbc.car.common.filter_simple import FirstOrderFilter
 from opendbc.car.interfaces import CarStateBase
 from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
-                                                  TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
+                                                  TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \
+                                                  SECOC_CAR
 
 ButtonType = structs.CarState.ButtonEvent.Type
 SteerControlType = structs.CarParams.SteerControlType
@@ -190,14 +191,14 @@ def update(self, can_parsers) -> structs.CarState:
         buttonEvents.extend(create_button_events(1, 0, {1: ButtonType.lkas}) +
                             create_button_events(0, 1, {1: ButtonType.lkas}))
 
-      if self.CP.carFingerprint not in RADAR_ACC_CAR:
+      if self.CP.carFingerprint not in (RADAR_ACC_CAR | SECOC_CAR):
         # distance button is wired to the ACC module (camera or radar)
         prev_distance_button = self.distance_button
         self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"]
 
         buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
-    ret.buttonEvents = buttonEvents
 
+    ret.buttonEvents = buttonEvents
     return ret
 
   @staticmethod

opendbc/car/toyota/interface.py

@@ -124,12 +124,9 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_lo
     # openpilot longitudinal behind experimental long toggle:
     #  - TSS2 radar ACC cars (disables radar)
 
-    if ret.flags & ToyotaFlags.SECOC.value:
-      ret.openpilotLongitudinalControl = False
-    else:
-      ret.openpilotLongitudinalControl = ret.enableDsu or \
-        candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
-        bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
+    ret.openpilotLongitudinalControl = ret.enableDsu or \
+      candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
+      bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
 
     ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
 

opendbc/car/toyota/toyotacan.py

@@ -56,6 +56,13 @@ def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_r
   return packer.make_can_msg("ACC_CONTROL", 0, values)
 
 
+def create_accel_command_2(packer, accel):
+  values = {
+    "ACCEL_CMD": accel,
+  }
+  return packer.make_can_msg("ACC_CONTROL_2", 0, values)
+
+
 def create_pcs_commands(packer, accel, active, mass):
   values1 = {
     "COUNTER": 0,

opendbc/safety/modes/toyota.h

@@ -14,7 +14,7 @@
 #define TOYOTA_COMMON_SECOC_TX_MSGS \
   TOYOTA_BASE_TX_MSGS \
   {0x2E4, 0, 8, .check_relay = true}, {0x131, 0, 8, .check_relay = true}, \
-  {0x343, 0, 8, .check_relay = false},  /* ACC cancel cmd */  \
+  {0x343, 0, 8, .check_relay = false},  /* ACC cancel cmd */ \
 
 #define TOYOTA_COMMON_LONG_TX_MSGS \
   TOYOTA_COMMON_TX_MSGS \
@@ -31,6 +31,11 @@
   /* ACC */                            \
   {0x343, 0, 8, .check_relay = true},  \
 
+#define TOYOTA_COMMON_SECOC_LONG_TX_MSGS \
+  TOYOTA_COMMON_SECOC_TX_MSGS \
+  {0x343, 0, 8, .check_relay = true}, \
+  {0x183, 0, 8, .check_relay = true},  /* ACC_CONTROL_2 */ \
+
 #define TOYOTA_COMMON_RX_CHECKS(lta)                                                                                                       \
   {.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},  \
   {.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}},                           \
@@ -208,6 +213,10 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
       desired_accel = to_signed(desired_accel, 16);
 
       bool violation = false;
+      if (toyota_secoc) {
+        // SecOC cars move accel to 0x183. Only allow inactive accel on 0x343 to match stock behavior
+        violation = desired_accel != TOYOTA_LONG_LIMITS.inactive_accel;
+      }
       violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
 
       // only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
@@ -226,6 +235,13 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
       }
     }
 
+    if (msg->addr == 0x183U) {
+      int desired_accel = (msg->data[0] << 8) | msg->data[1];
+      desired_accel = to_signed(desired_accel, 16);
+
+      tx = !longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
+    }
+
     // AEB: block all actuation. only used when DSU is unplugged
     if (msg->addr == 0x283U) {
       // only allow the checksum, which is the last byte
@@ -340,6 +356,10 @@ static safety_config toyota_init(uint16_t param) {
     TOYOTA_COMMON_LONG_TX_MSGS
   };
 
+  static const CanMsg TOYOTA_SECOC_LONG_TX_MSGS[] = {
+    TOYOTA_COMMON_SECOC_LONG_TX_MSGS
+  };
+
   // safety param flags
   // first byte is for EPS factor, second is for flags
   const uint32_t TOYOTA_PARAM_OFFSET = 8U;
@@ -359,14 +379,18 @@ 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) {
+  if (toyota_secoc) {
+    if (toyota_stock_longitudinal) {
       SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
     } else {
-      SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
+      SET_TX_MSGS(TOYOTA_SECOC_LONG_TX_MSGS, ret);
     }
   } else {
-    SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
+    if (toyota_stock_longitudinal) {
+      SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
+    } else {
+      SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
+    }
   }
 
   if (toyota_secoc) {

opendbc/safety/tests/test_toyota.py

@@ -11,7 +11,7 @@
 from opendbc.safety.tests.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]] + 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
@@ -52,10 +52,13 @@ def _lta_msg(self, req, req2, angle_cmd, torque_wind_down=100):
     values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd, "TORQUE_WIND_DOWN": torque_wind_down}
     return self.packer.make_can_msg_panda("STEERING_LTA", 0, values)
 
-  def _accel_msg(self, accel, cancel_req=0):
+  def _accel_msg_343(self, accel, cancel_req=0):
     values = {"ACCEL_CMD": accel, "CANCEL_REQ": cancel_req}
     return self.packer.make_can_msg_panda("ACC_CONTROL", 0, values)
 
+  def _accel_msg(self, accel, cancel_req=0):
+    return self._accel_msg_343(accel, cancel_req)
+
   def _speed_msg(self, speed):
     values = {("WHEEL_SPEED_%s" % n): speed * 3.6 for n in ["FR", "FL", "RR", "RL"]}
     return self.packer.make_can_msg_panda("WHEEL_SPEEDS", 0, values)
@@ -73,12 +76,12 @@ def _pcm_status_msg(self, enable):
     values = {"CRUISE_ACTIVE": enable}
     return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values)
 
-  def test_diagnostics(self, stock_longitudinal: bool = False):
+  def test_diagnostics(self, stock_longitudinal: bool = False, ecu_disabled: bool = True):
     for should_tx, msg in ((False, b"\x6D\x02\x3E\x00\x00\x00\x00\x00"),  # fwdCamera tester present
                            (False, b"\x0F\x03\xAA\xAA\x00\x00\x00\x00"),  # non-tester present
                            (True, b"\x0F\x02\x3E\x00\x00\x00\x00\x00")):
       tester_present = libsafety_py.make_CANPacket(0x750, 0, msg)
-      self.assertEqual(should_tx and not stock_longitudinal, self._tx(tester_present))
+      self.assertEqual(should_tx and ecu_disabled and not stock_longitudinal, self._tx(tester_present))
 
   def test_block_aeb(self, stock_longitudinal: bool = False):
     for controls_allowed in (True, False):
@@ -272,8 +275,8 @@ class TestToyotaStockLongitudinalBase(TestToyotaSafetyBase):
 
   LONGITUDINAL = False
 
-  def test_diagnostics(self, stock_longitudinal: bool = True):
-    super().test_diagnostics(stock_longitudinal=stock_longitudinal)
+  def test_diagnostics(self, stock_longitudinal: bool = True, ecu_disabled: bool = True):
+    super().test_diagnostics(stock_longitudinal=stock_longitudinal, ecu_disabled=ecu_disabled)
 
   def test_block_aeb(self, stock_longitudinal: bool = True):
     super().test_block_aeb(stock_longitudinal=stock_longitudinal)
@@ -285,9 +288,9 @@ def test_acc_cancel(self):
     for controls_allowed in [True, False]:
       self.safety.set_controls_allowed(controls_allowed)
       for accel in np.arange(self.MIN_ACCEL - 1, self.MAX_ACCEL + 1, 0.1):
-        self.assertFalse(self._tx(self._accel_msg(accel)))
-        should_tx = np.isclose(accel, 0, atol=0.0001)
-        self.assertEqual(should_tx, self._tx(self._accel_msg(accel, cancel_req=1)))
+        self.assertFalse(self._tx(self._accel_msg_343(accel)))
+        should_tx = np.isclose(accel, self.INACTIVE_ACCEL, atol=0.0001)
+        self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel, cancel_req=1)))
 
 
 class TestToyotaStockLongitudinalTorque(TestToyotaStockLongitudinalBase, TestToyotaSafetyTorque):
@@ -309,7 +312,7 @@ def setUp(self):
     self.safety.init_tests()
 
 
-class TestToyotaSecOcSafety(TestToyotaStockLongitudinalBase):
+class TestToyotaSecOcSafetyBase(TestToyotaSafetyBase):
 
   TX_MSGS = TOYOTA_SECOC_TX_MSGS
   RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x191, 0x412, 0x131)}
@@ -319,9 +322,12 @@ def setUp(self):
     self.packer = CANPackerPanda("toyota_secoc_pt_generated")
     self.safety = libsafety_py.libsafety
     self.safety.set_safety_hooks(CarParams.SafetyModel.toyota,
-                                 self.EPS_SCALE | ToyotaSafetyFlags.STOCK_LONGITUDINAL | ToyotaSafetyFlags.SECOC)
+                                 self.EPS_SCALE | ToyotaSafetyFlags.SECOC)
     self.safety.init_tests()
 
+  def test_diagnostics(self, ecu_disabled: bool = False):
+    super().test_diagnostics(ecu_disabled=ecu_disabled)
+
   # 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):
@@ -342,6 +348,51 @@ 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_183(self, accel):
+    values = {"ACCEL_CMD": accel}
+    return self.packer.make_can_msg_panda("ACC_CONTROL_2", 0, values)
+
+  def _accel_msg(self, accel, cancel_req=0):
+    return self._accel_msg_183(accel)
+
+
+class TestToyotaSecOcSafetyStockLongitudinal(TestToyotaSecOcSafetyBase, TestToyotaStockLongitudinalBase):
+
+  def setUp(self):
+    self.packer = CANPackerPanda("toyota_secoc_pt_generated")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.toyota,
+                                 self.EPS_SCALE | ToyotaSafetyFlags.STOCK_LONGITUDINAL | ToyotaSafetyFlags.SECOC)
+    self.safety.init_tests()
+
+
+class TestToyotaSecOcSafety(TestToyotaSecOcSafetyBase):
+
+  RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x191, 0x412, 0x131, 0x343, 0x183)}
+  FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x191, 0x412, 0x131, 0x343, 0x183]}
+
+  def setUp(self):
+    self.packer = CANPackerPanda("toyota_secoc_pt_generated")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.EPS_SCALE | ToyotaSafetyFlags.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
+
+  def test_343_actuation_blocked(self):
+    """
+    For SecOC cars, longitudinal acceleration must be sent in ACC_CONTROL_2, but all other ACC
+    data remains in ACC_CONTROL. Verify no actuation is sent via ACC_CONTROL.
+    """
+    for controls_allowed in [True, False]:
+      self.safety.set_controls_allowed(controls_allowed)
+      for accel in np.arange(self.MIN_ACCEL - 1, self.MAX_ACCEL + 1, 0.1):
+        should_tx = np.isclose(accel, self.INACTIVE_ACCEL, atol=0.0001)
+        self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel)))
+        self.assertEqual(should_tx, self._tx(self._accel_msg_343(accel, cancel_req=1)))
+
 
 if __name__ == "__main__":
   unittest.main()