diff --git a/opendbc_repo/opendbc/car/toyota/angle_control.py b/opendbc_repo/opendbc/car/toyota/angle_control.py new file mode 100644 index 0000000000..310514a28f --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/safety/modes/toyota_aol.h b/opendbc_repo/opendbc/safety/modes/toyota_aol.h index d96dc14e00..ce1156ec88 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota_aol.h +++ b/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 diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py index 0cc93178c7..2ce72e105a 100755 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/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) diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota_angle_aol.py b/opendbc_repo/opendbc/safety/tests/test_toyota_angle_aol.py new file mode 100644 index 0000000000..192fb00f77 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/toyota/aol.py b/openpilot/starpilot/car/toyota/aol.py index 5c678af46c..ce2ccfdf21 100644 --- a/openpilot/starpilot/car/toyota/aol.py +++ b/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): diff --git a/openpilot/starpilot/car/toyota/aol_policy.h b/openpilot/starpilot/car/toyota/aol_policy.h index 100868bd8c..14d96a94ca 100644 --- a/openpilot/starpilot/car/toyota/aol_policy.h +++ b/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}; diff --git a/openpilot/starpilot/car/toyota/tests/test_angle_aol.py b/openpilot/starpilot/car/toyota/tests/test_angle_aol.py new file mode 100644 index 0000000000..d998bc22f6 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/toyota/tests/test_angle_aol_callers.py b/openpilot/starpilot/car/toyota/tests/test_angle_aol_callers.py new file mode 100644 index 0000000000..58470683aa --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/toyota/tests/test_rav4_angle_filter_callers.py b/openpilot/starpilot/car/toyota/tests/test_rav4_angle_filter_callers.py index 3e31de04b3..1540ea4676 100644 --- a/openpilot/starpilot/car/toyota/tests/test_rav4_angle_filter_callers.py +++ b/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) diff --git a/openpilot/starpilot/car/toyota/tests/test_toyota_cpp_transport.py b/openpilot/starpilot/car/toyota/tests/test_toyota_cpp_transport.py new file mode 100644 index 0000000000..f98acf0853 --- /dev/null +++ b/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 +#include +#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 words(axis_size / 8U); + std::memcpy(words.data(), axis_bytes, axis_size); + capnp::FlatArrayMessageReader reader(kj::ArrayPtr(words.data(), words.size())); + auto event = reader.getRoot(); + 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 intent_words(intent_size / 8U); + std::memcpy(intent_words.data(), intent_bytes, intent_size); + capnp::FlatArrayMessageReader intent_reader(kj::ArrayPtr(intent_words.data(), intent_words.size())); + auto intent = intent_reader.getRoot(); + 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 +template bool read_value(T &value) { return bool(std::cin.read(reinterpret_cast(&value), sizeof(T))); } +static void respond(uint32_t value) { std::cout.write(reinterpret_cast(&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 axis(axis_size), intent(intent_size); + if (!std::cin.read(reinterpret_cast(axis.data()), axis_size) || + !std::cin.read(reinterpret_cast(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('