Restore independent RAV4 angle lateral control

opendbc_repo/opendbc/car/toyota/angle_control.py

@@ -0,0 +1,32 @@
+"""Exact RAV4 2023–25 LTA ownership; stock/radar-disable/filter remain distinct."""
+from opendbc.car import structs
+from opendbc.car.toyota.values import CAR, ToyotaFlags
+
+
+def configured(cp):
+  if (cp is None or cp.brand != 'toyota' or cp.carFingerprint != CAR.TOYOTA_RAV4_TSS2_2023 or
+      cp.notCar or cp.passive or cp.dashcamOnly or cp.secOcRequired or not cp.pcmCruise or len(cp.safetyConfigs) != 1 or
+      cp.steerControlType != structs.CarParams.SteerControlType.angle):
+    return False
+  required = ToyotaFlags.TSS2 | ToyotaFlags.NO_DSU | ToyotaFlags.RADAR_ACC | ToyotaFlags.ANGLE_CONTROL | ToyotaFlags.RAISED_ACCEL_LIMIT
+  filtered = bool(cp.flags & ToyotaFlags.RADAR_CAN_FILTER)
+  if filtered:
+    required |= ToyotaFlags.RADAR_CAN_FILTER
+  elif cp.openpilotLongitudinalControl:
+    required |= ToyotaFlags.DISABLE_RADAR
+  allowed = required | ToyotaFlags.HYBRID | ToyotaFlags.HAS_BSM | ToyotaFlags.ARS510_RADAR | ToyotaFlags.ARS510_FUSED
+  expected = 0x449 | (0x1000 if filtered else 0) | (0 if cp.openpilotLongitudinalControl else 0x200)
+  config = cp.safetyConfigs[0]
+  return bool(cp.flags & required == required and not int(cp.flags) & ~int(allowed) and
+              (not cp.flags & ToyotaFlags.ARS510_FUSED or cp.flags & ToyotaFlags.ARS510_RADAR) and
+              config.safetyModel == structs.CarParams.SafetyModel.toyota and config.safetyParam == expected)
+
+
+def prepare_stock(cp):
+  if not configured(cp):
+    return False
+  cp.openpilotLongitudinalControl = False
+  cp.flags &= ~int(ToyotaFlags.DISABLE_RADAR)
+  cp.safetyConfigs[0].safetyParam |= 0x200
+  cp.autoResumeSng = False
+  return True

opendbc_repo/opendbc/safety/modes/toyota_aol.h

@@ -6,6 +6,7 @@ static bool toyota_alt_cruise = false;
 
 static bool toyota_aol_secoc = false;
 static bool toyota_aol_stock = false;
+static bool toyota_aol_lta = false;
 static bool toyota_aol_rav4h_filter = false;
 static bool toyota_sdsu_control = false;
 static bool toyota_aol_main = false;
@@ -105,6 +106,11 @@ static void toyota_aol_observe(const CANPacket_t *msg) {
       const unsigned int state = (msg->data[3] >> 1U) & 0x7FU;
       toyota_aol_eps = (state != 0U) && (state != 9U) && (state != 11U) && (state != 21U) &&
                        (state != 25U) && (state != 3U) && (state != 17U);
+      if (toyota_aol_lta) {
+        const unsigned int lta_state = (msg->data[1] >> 3U) & 0x1FU;
+        toyota_aol_eps = toyota_aol_eps && (lta_state != 0U) && (lta_state != 9U) && (lta_state != 11U) && (lta_state != 21U) &&
+                          (lta_state != 25U) && (lta_state != 3U) && (lta_state != 17U);
+      }
     } else {
     }
     if (index < 4U) {
@@ -118,15 +124,16 @@ static void toyota_aol_observe(const CANPacket_t *msg) {
 static void toyota_aol_configure(uint16_t param) {
   toyota_aol_reset();
   toyota_alt_cruise = param == 0x424DU;
+  toyota_aol_lta = (param == 0x449U) || (param == 0x649U) || (param == 0x1449U) || (param == 0x1649U);
   toyota_pedal = (param == 0x2049U) || (param == 0x2249U);
   toyota_aol_rav4h_filter = (param == 0x9049U) || (param == 0x9249U);
-  toyota_aol_stock = (param == 585U) || (param == 4681U) || (param == 0x9249U) || (param == 0x2249U) || toyota_alt_cruise;
+  toyota_aol_stock = (param == 585U) || (param == 4681U) || (param == 0x9249U) || (param == 0x2249U) || toyota_alt_cruise || (param == 0x649U) || (param == 0x1649U);
 #ifdef ALLOW_DEBUG
   toyota_aol_secoc = (param == 2121U);
 #else
   toyota_aol_secoc = false;
 #endif
-  toyota_aol_enabled = (toyota_aol_stock && (alternative_experience == 32)) ||
+  toyota_aol_enabled = (toyota_aol_lta && (alternative_experience == 32)) || (toyota_aol_stock && (alternative_experience == 32)) ||
 #ifdef ALLOW_DEBUG
                        (toyota_aol_secoc && (alternative_experience == 32)) ||
 #endif

opendbc_repo/opendbc/safety/tests/test_toyota.py

@@ -680,7 +680,7 @@ class TestToyotaHighlanderAol(unittest.TestCase):
       self.assertEqual(self.safety.aol_get_permission_mask(), 0)
 
   def test_exact_profile_admission_and_reset(self):
-    for word, ae in ((585, 160), (585, 288), (72, 32), (329, 32), (1097, 32), (73, 33), (73, 289), (73, 416)):
+    for word, ae in ((585, 160), (585, 288), (72, 32), (329, 32), (1096, 32), (73, 33), (73, 289), (73, 416)):
       self.init(word, ae)
       self.physical()
       self.assertEqual(self.safety.aol_get_request_mask(), 0)

opendbc_repo/opendbc/safety/tests/test_toyota_angle_aol.py

@@ -0,0 +1,79 @@
+import pytest
+from opendbc.can import CANPacker
+from opendbc.car import structs
+from opendbc.safety.tests.libsafety import libsafety_py
+from openpilot.starpilot.car.toyota.tests import test_pedal_caller_native as helpers
+
+source_matched_native = helpers.source_matched_native
+
+
+class Physical:
+  def __init__(self, word, experience=32):
+    self.native = libsafety_py.libsafety
+    self.native.init_tests()
+    self.native.set_alternative_experience(experience)
+    assert self.native.set_safety_hooks(structs.CarParams.SafetyModel.toyota, word) == 0
+    self.native.set_aol_test_heartbeat(True)
+    self.packer = CANPacker('toyota_nodsu_pt_generated')
+    self.now = 1_000_000
+    for _ in range(20):
+      self.tick()
+
+  def tick(self, *, lka=1, lta=1, bad=None, skip_main=False):
+    self.now += 10_000
+    self.native.set_timer(self.now)
+    frames = helpers.physical(self.packer, cruise=False)
+    frames = [f for f in frames if f[0] not in (0x260, 0x262) and not (skip_main and f[0] == 0x1D3)]
+    frames += [self.packer.make_can_msg('STEER_TORQUE_SENSOR', 0, {'STEER_ANGLE': .0573}),
+               self.packer.make_can_msg('EPS_STATUS', 0, {'LKA_STATE': lka, 'LTA_STATE': lta})]
+    for address, data, bus in frames:
+      if address == 0x262 and bad == 'checksum':
+        data = data[:-1] + bytes([data[-1] ^ 1])
+      elif address == 0x262 and bad == 'length':
+        data = data[:-1]
+      self.native.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+    self.native.safety_tick()
+    self.native.aol_set_host_request(3)
+
+  def angle(self):
+    address, data, bus = self.packer.make_can_msg('STEERING_LTA', 0,
+      {'STEER_REQUEST': 1, 'STEER_REQUEST_2': 1, 'STEER_ANGLE_CMD': .0573, 'TORQUE_WIND_DOWN': 100})
+    return libsafety_py.make_CANPacket(address, bus, data)
+
+
+@pytest.mark.parametrize('word', [0x449, 0x649, 0x1449, 0x1649])
+@pytest.mark.parametrize('fault', ['lta', 'lta_zero', 'lka', 'checksum', 'length', 'main_expiry'])
+def test_angle_native_lateral_only_and_fault_withdrawal(word, fault):
+  case = Physical(word)
+  try:
+    assert not case.native.get_controls_allowed()
+    assert case.native.aol_get_permission_mask() == 1
+    assert case.native.safety_tx_hook(case.angle())
+    address, data, bus = case.packer.make_can_msg('ACC_CONTROL', 0, {'ACCEL_CMD': .8})
+    assert not case.native.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))
+    if fault == 'main_expiry':
+      for _ in range(34):
+        case.tick(skip_main=True)
+    else:
+      case.tick(lta=0 if fault == 'lta_zero' else 9 if fault == 'lta' else 1, lka=9 if fault == 'lka' else 1,
+                bad=fault if fault in ('checksum', 'length') else None)
+    assert case.native.aol_get_permission_mask() == 0
+    assert not case.native.safety_tx_hook(case.angle())
+    for _ in range(20):
+      case.tick()
+    assert case.native.aol_get_permission_mask() == 1
+    assert not case.native.get_controls_allowed()
+  finally:
+    case.native.set_alternative_experience(0)
+    case.native.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
+
+
+@pytest.mark.parametrize('word', [0x448, 0x44A, 0x648, 0x64A, 0x1448, 0x144A, 0x1648, 0x164A])
+def test_angle_adjacent_profiles_fail_closed(word):
+  case = Physical(word)
+  try:
+    assert case.native.aol_get_permission_mask() == 0
+    assert not case.native.safety_tx_hook(case.angle())
+  finally:
+    case.native.set_alternative_experience(0)
+    case.native.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)

openpilot/starpilot/car/toyota/aol.py

@@ -5,7 +5,7 @@ from opendbc.car.toyota.values import CAR, ToyotaFlags, ToyotaSafetyFlags, EPS_S
 from openpilot.starpilot.aol.policy import AolVehiclePolicy
 from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE
 
-AOL_WORDS = frozenset((73, 585, 4169, 4681, 2121, 0x9049, 0x9249, 0x2049, 0x2249, 0x424D))
+AOL_WORDS = frozenset((73, 585, 4169, 4681, 2121, 0x9049, 0x9249, 0x2049, 0x2249, 0x424D, 0x449, 0x649, 0x1449, 0x1649))
 
 
 SECOC_TORQUE_CARS = frozenset((CAR.TOYOTA_SIENNA_4TH_GEN,))
@@ -61,7 +61,14 @@ def radar_filter_qualified(cp, *, marked_only=False):
               (not cp.flags & ToyotaFlags.ARS510_FUSED or cp.flags & ToyotaFlags.ARS510_RADAR))
 
 
+def angle_qualified(cp, *, marked_only=False):
+  from opendbc.car.toyota.angle_control import configured
+  return configured(cp) and cp.alternativeExperience in ((32,) if marked_only else (0, 32))
+
+
 def qualified(cp, *, marked_only=False):
+  if angle_qualified(cp, marked_only=marked_only):
+    return True
   if radar_filter_qualified(cp, marked_only=marked_only):
     return True
   if lexus_is_qualified(cp, marked_only=marked_only):

openpilot/starpilot/car/toyota/aol_policy.h

@@ -3,7 +3,7 @@
 #include "selfdrive/pandad/aol_protocol.h"
 
 inline bool toyota_aol_param(uint16_t param) {
-  return (param == 73U) || (param == 585U) || (param == 4169U) || (param == 4681U) || (param == 2121U) || (param == 0x9049U) || (param == 0x9249U) || (param == 0x2049U) || (param == 0x2249U) || (param == 0x424DU);
+  return (param == 73U) || (param == 585U) || (param == 4169U) || (param == 4681U) || (param == 2121U) || (param == 0x9049U) || (param == 0x9249U) || (param == 0x2049U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x449U) || (param == 0x649U) || (param == 0x1449U) || (param == 0x1649U);
 }
 
 inline constexpr AolSafetyProfile TOYOTA_AOL_PROFILE{2U, toyota_aol_param, true};

openpilot/starpilot/car/toyota/tests/test_angle_aol.py

@@ -0,0 +1,50 @@
+from pathlib import Path
+import pytest
+from opendbc.car import gen_empty_fingerprint
+from opendbc.car.toyota.interface import CarInterface
+from opendbc.car.toyota.values import CAR, ToyotaFlags
+from opendbc.car.toyota.angle_control import configured
+from openpilot.starpilot.car.toyota.aol import angle_qualified
+from openpilot.starpilot.car.toyota.tests.test_radar_preferences import FilesystemParams
+from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
+
+
+def factory(alpha, filtered):
+  fp = gen_empty_fingerprint()
+  if filtered:
+    fp[0][0x2AA] = 8
+  return CarInterface.get_params(CAR.TOYOTA_RAV4_TSS2_2023, fp, [], alpha, False, False)
+
+
+@pytest.mark.parametrize('alpha', [False, True])
+@pytest.mark.parametrize('filtered', [False, True])
+def test_angle_exact_factory_owner_and_startup_off(alpha, filtered, tmp_path):
+  cp = factory(alpha, filtered)
+  assert configured(cp) and angle_qualified(cp)
+  assert not angle_qualified(cp, marked_only=True)
+  params = FilesystemParams(tmp_path)
+  Path(params.get_param_path('DisableOpenpilotLongitudinal')).write_bytes(b'1')
+  prior_tune = cp.lateralTuning.to_dict()
+  owner = VehicleStartupPreferences.read(params, enabled=True)
+  owner.prepare(cp)
+  owner.finalize(cp)
+  assert configured(cp) and not cp.openpilotLongitudinalControl
+  assert cp.safetyConfigs[0].safetyParam == (0x1649 if filtered else 0x649)
+  assert not cp.flags & ToyotaFlags.DISABLE_RADAR
+  assert cp.lateralTuning.to_dict() == prior_tune
+
+
+@pytest.mark.parametrize('field,value', [('passive', True), ('dashcamOnly', True), ('notCar', True),
+                                        ('secOcRequired', True), ('pcmCruise', False),
+                                        ('flags', 0), ('alternativeExperience', 128)])
+def test_angle_aol_rejects_wrong_configuration(field, value):
+  cp = factory(True, True)
+  setattr(cp, field, value)
+  assert not angle_qualified(cp)
+
+
+@pytest.mark.parametrize('word', [73, 585, 0x1049, 0x1249, 0x1448, 0x144A, 0x2449])
+def test_angle_aol_rejects_unrelated_native_words(word):
+  cp = factory(True, True)
+  cp.safetyConfigs[0].safetyParam = word
+  assert not angle_qualified(cp)

openpilot/starpilot/car/toyota/tests/test_angle_aol_callers.py

@@ -0,0 +1,15 @@
+import pytest
+from opendbc.car.toyota.values import CAR
+from openpilot.starpilot.car.toyota.tests import test_rav4_angle_filter_callers as helpers
+from openpilot.starpilot.car.toyota.tests.test_toyota_cpp_transport import host_heartbeat_bridge as host_heartbeat_bridge
+
+source_matched_native = helpers.source_matched_native
+daemon_gc_scope = helpers.daemon_gc_scope
+
+
+@pytest.mark.parametrize('filtered', [False, True])
+@pytest.mark.parametrize('alpha,disable', [(False, False), (True, False), (True, True)])
+@pytest.mark.parametrize('aol', [False, True])
+def test_angle_real_callers_cpp_native(filtered, alpha, disable, aol, tmp_path, host_heartbeat_bridge):
+  helpers.run_case(CAR.TOYOTA_RAV4_TSS2_2023, alpha, disable, 0, tmp_path,
+                   aol=aol, has_filter=filtered, bridge=host_heartbeat_bridge)

openpilot/starpilot/car/toyota/tests/test_rav4_angle_filter_callers.py

@@ -4,8 +4,10 @@ CAN, firmware acquisition, perception and USB are synthetic. A shared 100 Hz
 clock tests source/native ordering; this is not installed-hardware acceptance.
 """
 import json
+import ctypes
 import time
 from contextlib import ExitStack
+from types import SimpleNamespace
 from unittest.mock import patch
 
 import pytest
@@ -13,6 +15,7 @@ import pytest
 from opendbc.can import CANPacker
 from opendbc.car import Bus, car_helpers, gen_empty_fingerprint, structs
 from opendbc.car.fw_versions import match_fw_to_car
+from opendbc.car.can_definitions import CanData
 from opendbc.car.toyota.values import CAR, DBC, ToyotaFlags
 from opendbc.car.toyota.tests.test_auto_hold import decode
 from opendbc.safety.tests.libsafety import libsafety_py
@@ -33,11 +36,41 @@ def test_rav4_filter_actual_callers(alpha, disable, radar_mode, tmp_path):
   run_case(CAR.TOYOTA_RAV4_TSS2_2023, alpha, disable, radar_mode, tmp_path)
 
 
-def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True):
+def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True, has_filter=True, bridge=None):
   helpers.SYNTHETIC_LAST.clear()
   helpers.NATIVE_LAST.clear()
   safety = libsafety_py.libsafety
   trace = []
+  diag_queue, diag_tx = [], []
+  native_globals = ctypes.CDLL(libsafety_py.libpath)
+
+  def native_view(host):
+    cfg = structs.CarParams.SafetyConfig(
+      safetyModel=ctypes.c_uint16.in_dll(native_globals, 'current_safety_mode').value,
+      safetyParam=ctypes.c_uint16.in_dll(native_globals, 'current_safety_param').value)
+    return SimpleNamespace(CP=SimpleNamespace(safetyConfigs=[cfg],
+      alternativeExperience=ctypes.c_int.in_dll(native_globals, 'alternative_experience').value))
+
+  def diag_recv(wait_for_one=False):
+    del wait_for_one
+    if not diag_queue:
+      return []
+    batch = list(diag_queue)
+    diag_queue.clear()
+    return [batch]
+
+  def diag_send(messages):
+    assert ctypes.c_uint16.in_dll(native_globals, 'current_safety_mode').value == int(structs.CarParams.SafetyModel.elm327)
+    for message in messages:
+      assert safety.safety_tx_hook(libsafety_py.make_CANPacket(message.address, message.src, message.dat))
+      assert message.address == 0x750 and message.src == 0
+      diag_tx.append(bytes(message.dat).hex())
+      if bytes(message.dat)[2:4] == bytes([0x10, 0x03]):
+        diag_queue.append(CanData(0x758, bytes([0x0F, 2, 0x50, 3, 0, 0, 0, 0]), 0))
+      elif bytes(message.dat)[2:5] == bytes([0x28, 1, 1]):
+        diag_queue.append(CanData(0x758, bytes([0x0F, 2, 0x68, 1, 0, 0, 0, 0]), 0))
+      else:
+        raise AssertionError(('Unexpected initialization diagnostic', message))
   with OpenpilotPrefix(), ExitStack() as stack:
     params = Params()
     for key, value in [('OpenpilotEnabledToggle', True), ('AlphaLongitudinalEnabled', alpha),
@@ -48,25 +81,31 @@ def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True):
     exact, found = match_fw_to_car(fw, '', log=False)
     assert exact and found == {candidate}
     fp = gen_empty_fingerprint()
-    fp[0][0x2AA] = 8
+    if has_filter:
+      fp[0][0x2AA] = 8
     observed = (candidate, fp, '0' * 17, fw, structs.CarParams.FingerprintSource.fw, True)
     with (patch.object(car_helpers, 'fingerprint', return_value=observed),
-          patch.object(card_module, 'can_comm_callbacks', return_value=(lambda *a: [], lambda x: None)),
+          patch.object(card_module, 'can_comm_callbacks', return_value=(diag_recv, diag_send)),
           patch.object(card_module.messaging, 'recv_one_retry', return_value=messaging.new_message('can', 1))):
       host = card_module.Car()
     long = alpha and not disable
-    assert not host.CP.flags & ToyotaFlags.DISABLE_RADAR
     angle = candidate == CAR.TOYOTA_RAV4_TSS2_2023
-    enrolled = not angle and aol
-    assert host.CP.safetyConfigs[0].safetyParam == ((0x1449 if long else 0x1649) if angle else (0x1049 if long else 0x1249))
+    enrolled = aol
+    assert bool(host.CP.flags & ToyotaFlags.DISABLE_RADAR) == (not has_filter and long)
+    word = 0x49 | (0x400 if angle else 0) | (0x1000 if has_filter else 0) | (0 if long else 0x200)
+    assert host.CP.safetyConfigs[0].safetyParam == word
     assert host.CP.openpilotLongitudinalControl == long
-    # This port keeps the existing angle-control AOL enrollment boundary.
-    assert host.CP.alternativeExperience == (0 if angle else 32)
+    assert host.CP.alternativeExperience == 32
     controls, sd = Controls(), sd_module.SelfdriveD(CP=host.CP)
     assert controls.CP.to_dict() == host.CP.to_dict()
     safety.init_tests()
     safety.set_alternative_experience(host.CP.alternativeExperience)
-    assert safety.set_safety_hooks(host.CP.safetyConfigs[0].safetyModel.raw, host.CP.safetyConfigs[0].safetyParam) == 0
+    diagnostic = bool(host.CP.flags & ToyotaFlags.DISABLE_RADAR)
+    initial_mode = int(structs.CarParams.SafetyModel.elm327) if diagnostic else host.CP.safetyConfigs[0].safetyModel.raw
+    diagnostic_word = 0 if params.get_bool('ObdMultiplexingEnabled') else 1
+    assert safety.set_safety_hooks(initial_mode, diagnostic_word if diagnostic else word) == 0
+    if bridge is not None:
+      bridge.reset_host()
     safety.set_aol_test_heartbeat(True)
     packer = CANPacker(DBC[candidate][Bus.pt])
     published, previous = {}, {}
@@ -104,14 +143,16 @@ def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True):
                    packer.make_can_msg('EPS_STATUS', 0, {'LKA_STATE': 9 if fault else 1, 'LTA_STATE': 9 if fault else 1}),
                    packer.make_can_msg('PCS_HUD', 0, {}),
                    # The physical filter advertises itself; it does not grant controls.
-                   (0x2AA, bytes(8), 0)]
+                   *([(0x2AA, bytes(8), 0)] if has_filter else [])]
+        if not has_filter and not long:
+          frames.append(packer.make_can_msg('ACC_CONTROL', 0, {'ACC_TYPE': 1}))
         # Stock AEB/PCS traffic must not trip the ACC relay check.
-        if 580 <= tick < 600:
+        if has_filter and 580 <= tick < 600:
           frames.append(packer.make_can_msg('ACC_CONTROL', 0, {}))
         for address, data, bus in frames:
           assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)), (tick, hex(address))
         safety.safety_tick()
-        native = helpers.native_events(host, safety, now, sd.aol_session_id)
+        native = helpers.native_events(native_view(host), safety, now, sd.aol_session_id)
         helpers.deliver(host.sm, [*native, *previous.values()])
         batch = messaging.log_from_bytes(card_module.can_list_to_can_capnp(frames)).as_builder()
         batch.logMonoTime = now
@@ -127,8 +168,24 @@ def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True):
               patch.object(sd.pm, 'send', side_effect=lambda n, m, events=events: events.update({n: m.as_reader()}))):
           sd.step()
         wanted = sd.aol_axis_decision
-        safety.aol_set_host_request(int(wanted.desired_lateral) | int(wanted.desired_longitudinal) << 1)
-        native = helpers.native_events(host, safety, now, sd.aol_session_id)
+        if bridge is None:
+          safety.aol_set_host_request(int(wanted.desired_lateral) | int(wanted.desired_longitudinal) << 1)
+        else:
+          axis = events.get('aolAxisState', previous.get('aolAxisState'))
+          intent = state.get('aolIntentWire')
+          axis_bytes = axis.as_builder().to_bytes() if axis is not None else b''
+          intent_bytes = intent.as_builder().to_bytes() if intent is not None else b''
+          observed = native_view(host).CP.safetyConfigs[0]
+          capabilities = int(bool(ctypes.c_void_p.in_dll(native_globals, 'aol_policy').value))
+          capabilities |= 2 * int(ctypes.c_bool.in_dll(native_globals, 'aol_optional_release_capable').value)
+          engaged = bool(events.get('selfdriveState', previous.get('selfdriveState')).selfdriveState.enabled)
+          packed = bridge.plan_host(observed.safetyModel.raw, observed.safetyParam,
+            safety.aol_get_request_mask(), safety.aol_get_permission_mask(), now,
+            axis_bytes, len(axis_bytes), intent_bytes, len(intent_bytes), engaged, capabilities)
+          safety.aol_set_host_request(packed & 0xFF)
+          safety.set_aol_test_heartbeat(bool(packed & 0x100))
+          bridge.complete_host(safety.aol_get_request_mask(), safety.aol_get_permission_mask())
+        native = helpers.native_events(native_view(host), safety, now, sd.aol_session_id)
         current = {**previous, **events}
         helpers.deliver(controls.sm, helpers.control_inputs(controls, published, state, current, native, now, .00005))
         command, lac_log = controls.state_control()
@@ -137,9 +194,21 @@ def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True):
         cc = cc_event.carControl
         helpers.deliver(host.sm, [cc_event, *native, *events.values()])
         host.controls_update(cs, cc)
+        if diagnostic and host.ci_initialized and ctypes.c_uint16.in_dll(native_globals, 'current_safety_mode').value == initial_mode:
+          # The real pandad waits for Card's asynchronous Params handoff.
+          # Poll its actual write; never set the readiness flag in the fixture.
+          for _ in range(50):
+            if params.get_bool('ControlsReady'):
+              break
+            time.sleep(.005)
+          assert params.get_bool('ControlsReady')
+          safety.set_alternative_experience(host.CP.alternativeExperience)
+          assert safety.set_safety_hooks(host.CP.safetyConfigs[0].safetyModel.raw, word) == 0
+          safety.set_aol_test_heartbeat(True)
         outgoing = published['sendcan']
         for frame in outgoing.sendcan:
-          assert frame.address != 0x750, 'Filter path must not keep radar disabled'
+          if has_filter:
+            assert frame.address != 0x750, 'Filter path must not keep radar disabled'
           packet = libsafety_py.make_CANPacket(frame.address, frame.src, bytes(frame.dat))
           assert safety.safety_tx_hook(packet), (tick, hex(frame.address), bytes(frame.dat).hex())
           if frame.address == 0x191:
@@ -164,10 +233,12 @@ def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True):
         if enrolled and 442 <= tick < 449:
           assert cc.latActive and not cc.longActive, trace[-1]
         previous = current
+      assert bool(diag_tx) == diagnostic
       assert active and steering
       assert acceleration == long
       assert not safety.get_relay_malfunction()
     finally:
+      (tmp_path / 'diagnostics.json').write_text(json.dumps(diag_tx))
       (tmp_path / 'trace.json').write_text(json.dumps(trace, indent=2) + '\n')
       safety.set_alternative_experience(0)
       safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)

openpilot/starpilot/car/toyota/tests/test_toyota_cpp_transport.py

@@ -0,0 +1,158 @@
+import json
+import pytest
+
+@pytest.fixture(scope='module')
+def host_heartbeat_bridge(tmp_path_factory):
+  import hashlib
+  import re
+  import capnproto
+  import subprocess
+  import struct
+  from pathlib import Path
+
+  root = Path(__file__).resolve().parents[5]
+  tmp = tmp_path_factory.mktemp('toyota-angle-host')
+  pandad = (root / 'openpilot/selfdrive/pandad/pandad.cc').read_text()
+  matched = re.search(r'      if \(aol_write_ok\) \{.*?\n      \}', pandad, re.S)
+  assert matched is not None
+  code = r'''
+#include <cstring>
+#include <vector>
+#include "selfdrive/pandad/aol_wire.h"
+#include "openpilot/cereal/gen/cpp/log.capnp.h"
+#include "openpilot/starpilot/car/toyota/aol_policy.h"
+static const AolSafetyProfile profiles[] = {TOYOTA_AOL_PROFILE};
+static const AolProfileRegistry registry{profiles, std::size(profiles)};
+static AolAxisNegotiator negotiator(registry);
+static AolWritePlan pending;
+static aol_safety_health_t snapshot;
+extern "C" void reset_host() { negotiator = AolAxisNegotiator(registry); }
+extern "C" uint32_t plan_host(uint8_t mode, uint16_t word, uint8_t request, uint8_t permission, uint64_t now,
+ const uint8_t *axis_bytes, size_t axis_size, const uint8_t *intent_bytes, size_t intent_size, bool engaged, uint8_t capability) {
+ snapshot = {AOL_SAFETY_PROTOCOL_MAGIC, AOL_SAFETY_PROTOCOL_VERSION, request, permission, mode, word, capability};
+ AolAxisInput input;
+ if (axis_size != 0U) {
+  std::vector<capnp::word> words(axis_size / 8U);
+  std::memcpy(words.data(), axis_bytes, axis_size);
+  capnp::FlatArrayMessageReader reader(kj::ArrayPtr<const capnp::word>(words.data(), words.size()));
+  auto event = reader.getRoot<cereal::Event>();
+  auto axis = event.getAolAxisState();
+  input = {true, axis.getQualified(), axis.getSessionId().cStr(), axis.getObservedMonoTime(),
+           axis.getValidUntilMonoTime(), event.getLogMonoTime(), axis.getDesiredLateral(), axis.getDesiredLongitudinal()};
+  if (intent_size != 0U) {
+   std::vector<capnp::word> intent_words(intent_size / 8U);
+   std::memcpy(intent_words.data(), intent_bytes, intent_size);
+   capnp::FlatArrayMessageReader intent_reader(kj::ArrayPtr<const capnp::word>(intent_words.data(), intent_words.size()));
+   auto intent = intent_reader.getRoot<cereal::Event>();
+   input.retain_lateral_arm = aol_armed_intent(intent.getAolIntentWire(), intent.getLogMonoTime(),
+        axis.getSourceCarStateMonoTime(), now, false, axis.getIntentProducerSessionId().cStr());
+  }
+ }
+ auto plan = negotiator.prepare(snapshot, input, now, mode);
+ pending = plan;
+ const bool aol_write_ok = plan.capable;
+''' + matched[0] + r'''
+ return uint32_t(plan.request_mask) | (uint32_t(engaged) << 8U) | (uint32_t(plan.retain_lateral_arm) << 9U);
+}
+extern "C" void complete_host(uint8_t request, uint8_t permission) {
+ snapshot.request_mask = request; snapshot.permission_mask = permission;
+ negotiator.complete(pending, true, snapshot);
+}
+'''
+  source = tmp / 'host.cc'
+  code += r'''
+#include <iostream>
+template <typename T> bool read_value(T &value) { return bool(std::cin.read(reinterpret_cast<char *>(&value), sizeof(T))); }
+static void respond(uint32_t value) { std::cout.write(reinterpret_cast<const char *>(&value), sizeof(value)); std::cout.flush(); }
+int main() {
+ uint8_t op;
+ while (read_value(op)) {
+  if (op == 0U) { reset_host(); respond(0U); }
+  else if (op == 1U) {
+   uint8_t mode, request, permission, engaged, capability; uint16_t word; uint64_t now; uint32_t axis_size, intent_size;
+   if (!read_value(mode) || !read_value(word) || !read_value(request) || !read_value(permission) || !read_value(now) ||
+       !read_value(axis_size) || !read_value(intent_size) || !read_value(engaged) || !read_value(capability)) return 2;
+   std::vector<uint8_t> axis(axis_size), intent(intent_size);
+   if (!std::cin.read(reinterpret_cast<char *>(axis.data()), axis_size) ||
+       !std::cin.read(reinterpret_cast<char *>(intent.data()), intent_size)) return 3;
+   respond(plan_host(mode, word, request, permission, now, axis.data(), axis_size, intent.data(), intent_size, engaged != 0U, capability));
+  } else if (op == 2U) {
+   uint8_t request, permission; if (!read_value(request) || !read_value(permission)) return 4;
+   complete_host(request, permission); respond(0U);
+  } else if (op == 3U) { respond(0U); return 0; }
+  else return 5;
+ }
+ return 0;
+}
+'''
+  source.write_text(code)
+  library = tmp / 'host-peer'
+  generated = [root / ('openpilot/cereal/gen/cpp/' + name + '.capnp.c++') for name in ('log', 'car', 'custom', 'deprecated')]
+  capnp_flags = ['-I', capnproto.INCLUDE_DIR, '-L', capnproto.LIB_DIR, '-lcapnp', '-lkj']
+  subprocess.run(['c++', '-std=c++17', '-O1', '-Wall', '-Werror',
+                  '-I', str(root), '-I', str(root / 'openpilot'), '-I', str(root / 'opendbc_repo'), '-I', str(root / 'openpilot/cereal/gen/cpp'),
+                  str(source), *map(str, generated), *capnp_flags, '-o', str(library)], check=True, timeout=35)
+
+  class Peer:
+    def __init__(self):
+      self.process = subprocess.Popen([str(library)], stdin=subprocess.PIPE, stdout=subprocess.PIPE)
+
+    def exchange(self, raw):
+      assert self.process.stdin is not None and self.process.stdout is not None
+      assert self.process.stdin.write(raw) == len(raw)
+      self.process.stdin.flush()
+      result = self.process.stdout.read(4)
+      assert len(result) == 4, self.process.poll()
+      return struct.unpack('<I', result)[0]
+
+    def reset_host(self):
+      assert self.exchange(b'\x00') == 0
+
+    def plan_host(self, mode, word, request, permission, now, axis, axis_size, intent, intent_size, engaged, capability):
+      assert axis_size == len(axis) and intent_size == len(intent)
+      return self.exchange(b'\x01' + struct.pack('<BHBBQIIBB', mode, word, request, permission, now,
+                                               axis_size, intent_size, int(engaged), capability) + axis + intent)
+
+    def complete_host(self, request, permission):
+      assert self.exchange(b'\x02' + struct.pack('<BB', request, permission)) == 0
+
+    def close(self):
+      try:
+        if self.process.poll() is None:
+          assert self.exchange(b'\x03') == 0
+        assert self.process.wait(timeout=5) == 0
+      finally:
+        if self.process.poll() is None:
+          self.process.kill()
+          self.process.wait(timeout=5)
+        if self.process.stdin is not None:
+          self.process.stdin.close()
+        if self.process.stdout is not None:
+          self.process.stdout.close()
+
+  paths = ['openpilot/selfdrive/pandad/pandad.cc', 'openpilot/selfdrive/pandad/aol_wire.h',
+           'openpilot/selfdrive/pandad/aol_protocol.h', 'openpilot/starpilot/car/toyota/aol_policy.h',
+           'opendbc_repo/opendbc/safety/modes/toyota.h', 'opendbc_repo/opendbc/safety/modes/toyota_aol.h']
+  paths += [str(f.relative_to(root)) for f in generated]
+  paths += [str(f.relative_to(root)) for f in (root / 'openpilot/cereal/gen/cpp').glob('*.capnp.h')]
+  queued = [root / name for name in paths]
+  closure = {}
+  include_roots = [root, root / 'openpilot', root / 'opendbc_repo', root / 'openpilot/cereal/gen/cpp']
+  while queued:
+    path = queued.pop().resolve()
+    name = str(path.relative_to(root))
+    if name in closure:
+      continue
+    closure[name] = hashlib.sha256(path.read_bytes()).hexdigest()
+    if name == 'openpilot/selfdrive/pandad/pandad.cc':
+      continue
+    for quoted in re.findall(r'^\s*#include\s+"([^"\n]+)"', path.read_text(), re.M):
+      target = next((directory / quoted for directory in [path.parent, *include_roots] if (directory / quoted).is_file()), None)
+      assert target is not None, (name, quoted)
+      queued.append(target)
+  print(json.dumps({'hostHeartbeatSourceClosure': closure, 'artifactSha256': hashlib.sha256(library.read_bytes()).hexdigest()}))
+  peer = Peer()
+  try:
+    yield peer
+  finally:
+    peer.close()

openpilot/starpilot/vehicle_preferences.py

@@ -181,6 +181,8 @@ class VehicleStartupPreferences:
       prepare_pedal_stock(cp)
       from opendbc.car.toyota.radar_filter import prepare_stock as prepare_radar_filter_stock
       prepare_radar_filter_stock(cp)
+      from opendbc.car.toyota.angle_control import prepare_stock as prepare_angle_stock
+      prepare_angle_stock(cp)
     self._prepare_bolt(cp, fingerprints)
     prepare_disable_longitudinal(cp, self.disable_bolt_long)
     self._prepare_camera_pedal(cp)
@@ -216,6 +218,8 @@ class VehicleStartupPreferences:
       prepare_pedal_stock(cp)
       from opendbc.car.toyota.radar_filter import prepare_stock as prepare_radar_filter_stock
       prepare_radar_filter_stock(cp)
+      from opendbc.car.toyota.angle_control import prepare_stock as prepare_angle_stock
+      prepare_angle_stock(cp)
     self._prepare_bolt(cp)
     prepare_disable_longitudinal(cp, self.disable_bolt_long)
     self._prepare_camera_pedal(cp)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "5ff0bb6bd4af3776faedec086cdf0a7ed07e4eb23544132665b85e045ee537ab", "schema_version": 1}
+{"protected_inventory_sha256": "80bd6fce940eaec059a84a0aed1a35088d37f055ce625057c8e40f422867815a", "schema_version": 1}