From eac8cea6ee905caf0c0760dfa289e9d152ddb801 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 23 Oct 2026 14:40:44 -0500 Subject: [PATCH] Support Toyota Matrix stock-control AOL --- .../opendbc/car/toyota/tests/test_retrofit.py | 47 ++++ opendbc_repo/opendbc/safety/modes/toyota.h | 8 +- .../opendbc/safety/modes/toyota_aol.h | 4 +- .../tests/test_toyota_matrix_stock_aol.py | 203 ++++++++++++++++++ .../pandad/tests/test_aol_protocol.cc | 2 +- openpilot/starpilot/car/toyota/aol.py | 15 +- openpilot/starpilot/car/toyota/aol_policy.h | 2 +- .../tests/test_matrix_stock_caller_native.py | 157 ++++++++++++++ .../toyota/tests/test_rav4h_caller_native.py | 64 +++++- prebuilt.json | 2 +- tools/ci/run_vehicle_tests.py | 4 + tools/ci/tests/test_matrix_registration.py | 27 +++ 12 files changed, 515 insertions(+), 20 deletions(-) create mode 100644 opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py create mode 100644 openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py create mode 100644 tools/ci/tests/test_matrix_registration.py diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py b/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py index c55ae08118..989cb4536e 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_retrofit.py @@ -47,6 +47,53 @@ class TestRetrofit(unittest.TestCase): if identity == CAR.TOYOTA_PRIUS_RETROFIT: self.assertTrue(cp.dashcamOnly) + def test_matrix_stock_aol_admission(self): + import struct + from openpilot.starpilot.car.toyota.aol import qualified, native_accepts_cp + from opendbc.car import structs + + for alpha in (False, True): + cp = params(CAR.TOYOTA_MATRIX_RETROFIT, alpha=alpha) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 856) + self.assertEqual(cp.flags, 0) + self.assertTrue(qualified(cp)) + cp.alternativeExperience = 32 + self.assertTrue(qualified(cp, marked_only=True)) + self.assertTrue(native_accepts_cp(cp, int(structs.CarParams.SafetyModel.toyota), 856)) + self.assertFalse(cp.openpilotLongitudinalControl) + self.assertEqual(cp.lateralTuning.torque.latAccelFactor, struct.unpack('addr == 0x1D3U)) || (msg->addr == 0x262U) || (toyota_aol_secoc && (msg->addr == 0x127U)); - const unsigned int aol_source_len = ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert()) && (msg->addr == 0x262U)) ? 5U : 8U; + const unsigned int aol_source_len = ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert() || (current_safety_param == 856U)) && (msg->addr == 0x262U)) ? 5U : 8U; if (!aol_checksum_source || ((GET_LEN(msg) == aol_source_len) && (toyota_get_checksum(msg) == toyota_compute_checksum(msg)))) { toyota_aol_observe(msg); } else if ((toyota_aol_enabled || toyota_legacy_advert() || toyota_camera_2aa() || toyota_prius_filter()) && (msg->bus == 0U)) { @@ -246,7 +246,7 @@ static void toyota_optional_rx_hook(const CANPacket_t *msg) { static void toyota_rx_hook(const CANPacket_t *msg) { // Required sources have already passed RX length/checksum validation here. // Optional ingress only runs for messages outside the required RX catalog. - if ((toyota_legacy_advert() || toyota_camera_2aa() || toyota_prius_filter()) && ((msg->addr == 0x262U) || (!toyota_alt_cruise && (msg->addr == 0x1D3U)))) { + if ((toyota_legacy_advert() || toyota_camera_2aa() || toyota_prius_filter() || (current_safety_param == 856U)) && ((msg->addr == 0x262U) || (!toyota_alt_cruise && (msg->addr == 0x1D3U)))) { toyota_aol_observe(msg); } if (toyota_alt_cruise && msg_matches(msg, 0x365U, 0U) && (GET_LEN(msg) == 7U)) { @@ -692,7 +692,7 @@ static safety_config toyota_init(uint16_t param) { // Restore the sixth row on every warm switch before specializing the new owner. toyota_rx_checks[5].msg[0] = (CanMsgCheck){.addr = 0x365, .bus = 0U, .len = 7, .frequency = 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}; - if (toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param)) { + if (toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param) || (param == 856U)) { toyota_rx_checks[4].msg[0] = (CanMsgCheck){.addr = 0x262, .bus = 0U, .len = (toyota_camera_2aa_param(param) || toyota_prius_advert_param(param)) ? 8 : 5, .frequency = 25U, .ignore_counter = true, .ignore_quality_flag = true}; if (!toyota_alt_cruise) { @@ -710,7 +710,7 @@ static safety_config toyota_init(uint16_t param) { ret.disable_forwarding = false; if (toyota_prius_sdsu_param(param)) { ret.rx_checks_len = 7; - } else if ((toyota_legacy_sdsu && toyota_alt_cruise) || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param)) { + } else if ((toyota_legacy_sdsu && toyota_alt_cruise) || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_advert_param(param) || (param == 856U)) { ret.rx_checks_len = 6; } else if (toyota_pedal || toyota_alt_cruise || toyota_sdsu_status) { ret.rx_checks_len = 5; diff --git a/opendbc_repo/opendbc/safety/modes/toyota_aol.h b/opendbc_repo/opendbc/safety/modes/toyota_aol.h index 77948cb0a0..fe6b1ca360 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota_aol.h +++ b/opendbc_repo/opendbc/safety/modes/toyota_aol.h @@ -115,7 +115,7 @@ static void toyota_aol_observe(const CANPacket_t *msg) { unsigned int expected_len = 8U; if (toyota_alt_cruise && (msg->addr == main_addr)) { expected_len = 7U; - } else if ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert()) && (msg->addr == 0x262U)) { + } else if ((toyota_aol_rav4h_filter || toyota_alt_cruise || toyota_legacy_sdsu || toyota_legacy_advert() || (current_safety_param == 856U)) && (msg->addr == 0x262U)) { expected_len = 5U; } else { } @@ -183,7 +183,7 @@ static void toyota_aol_configure(uint16_t param) { toyota_pedal = (param == 0x2049U) || (param == 0x2249U); toyota_aol_rav4h_filter = (param == 0x9049U) || (param == 0x9249U); toyota_sdsu_status = toyota_aol_rav4h_filter || toyota_eps8_sdsu || toyota_legacy_sdsu || toyota_prius_sdsu_param(param); - toyota_aol_stock = (param == 585U) || (param == 4681U) || (param == 0x9249U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x649U) || (param == 0x1649U) || (param == 0x8249U) || (param == 0x8649U) || ((toyota_legacy_sdsu || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_filter_param(param)) && ((param & 0x200U) != 0U)); + toyota_aol_stock = (param == 585U) || (param == 856U) || (param == 4681U) || (param == 0x9249U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x649U) || (param == 0x1649U) || (param == 0x8249U) || (param == 0x8649U) || ((toyota_legacy_sdsu || toyota_legacy_advert_param(param) || toyota_camera_2aa_param(param) || toyota_prius_filter_param(param)) && ((param & 0x200U) != 0U)); #ifdef ALLOW_DEBUG toyota_aol_secoc = (param == 2121U); #else diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py b/opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py new file mode 100644 index 0000000000..774d8127e6 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py @@ -0,0 +1,203 @@ +"""Exact manual Matrix stock PCM native observer and authority regressions.""" +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_rav4h_caller_native as helpers + +# Read actual native required-RX timestamps; never modify them. +libsafety_py.ffi.cdef('extern safety_config current_safety_config;', override=True) + +source_matched_native = helpers.source_matched_native + + +class Physical: + def __init__(self, *, word=856, experience=32): + self.safety = libsafety_py.libsafety + self.now = 1_000_000 + self.packer = CANPacker('toyota_new_mc_pt_generated') + self.safety.set_alternative_experience(experience) + assert self.safety.set_safety_hooks(structs.CarParams.SafetyModel.toyota, word) == 0 + self.safety.init_tests() + self.safety.set_alternative_experience(experience) + self.safety.set_aol_test_heartbeat(True) + + def tick(self, *, fault=None, omit=None, **fields): + self.now += 10_000 + self.safety.set_timer(self.now) + frames = helpers.physical(self.packer, cruise=fields.pop('cruise', False), sdsu=None, **fields) + for address, data, bus in frames: + if address == omit: + continue + required = address in (0xAA, 0x260, 0x1D2, 0x224, 0x262, 0x1D3) + corrupted = fault == ('crc', address) + if corrupted: + data = data[:-1] + bytes([data[-1] ^ 1]) + if fault == ('length', address): + data = data + bytes(3) if address == 0x262 else data[:-1] + if fault == ('bus', address): + bus = 2 + accepted = bool(self.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))) + if required and corrupted: + assert not accepted, (hex(address), 'corrupt required source was accepted') + elif required and fault != ('length', address): + assert accepted, (hex(address), data.hex()) + self.safety.safety_tick() + self.safety.aol_set_host_request(3) + + def warm(self): + for _ in range(25): + self.tick(cruise=False) + for _ in range(25): + self.tick(cruise=True) + + def tx(self, name, values): + address, data, bus = self.packer.make_can_msg(name, 0, values) + return bool(self.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))) + + def close(self): + self.safety.set_alternative_experience(0) + self.safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) + + +@pytest.fixture +def matrix(): + case = Physical() + try: + yield case + finally: + case.close() + + +def test_actual_dlc5_stock_lateral_only(matrix): + matrix.warm() + assert matrix.safety.safety_config_valid() + assert matrix.safety.aol_get_permission_mask() == 1 + assert matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1}) + assert not matrix.tx('ACC_CONTROL', {'ACCEL_CMD': .1}) + assert not matrix.tx('ACC_CONTROL', {'ACCEL_CMD': -.1}) + assert matrix.tx('ACC_CONTROL', {'ACCEL_CMD': 0, 'CANCEL_REQ': 1}) + + +@pytest.mark.parametrize('fields', ({'main': False}, {'gear': 8}, {'gear': 16}, + {'eps': 0}, {'eps': 3}, {'eps': 9}, {'eps': 17})) +def test_physical_refusals_and_recovery(matrix, fields): + matrix.warm() + assert matrix.safety.aol_get_permission_mask() == 1 + matrix.tick(**fields) + assert matrix.safety.aol_get_permission_mask() == 0 + assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1}) + matrix.tick() + assert matrix.safety.aol_get_permission_mask() == 1 + + +@pytest.mark.parametrize('address', (0x262, 0x1D3, 0x260)) +@pytest.mark.parametrize('kind', ('crc', 'length')) +def test_actual_required_rx_refusal(matrix, address, kind): + matrix.warm() + assert matrix.safety.aol_get_permission_mask() == 1 + config = matrix.safety.current_safety_config + check = next(config.rx_checks[i] for i in range(config.rx_checks_len) if config.rx_checks[i].msg[0].addr == address) + original_timestamp = check.status.last_timestamp + matrix.tick(fault=(kind, address)) + if address == 0x260 and kind == 'length': + # Generic required RX matches address, bus AND DLC. A wrong-DLC frame must + # never renew the last good source; the unchanged native tick expires it. + assert check.msg[0].frequency == 50 + assert check.status.last_timestamp == original_timestamp + for _ in range(19): + matrix.tick(fault=(kind, address)) + assert check.status.last_timestamp == original_timestamp + # AOL's unchanged ten-period freshness bound is tighter than generic lag. + assert matrix.now - original_timestamp == 200_000 + assert not check.status.lagging and matrix.safety.aol_get_permission_mask() == 1 + matrix.tick(fault=(kind, address)) + assert matrix.now - original_timestamp == 210_000 + assert check.status.last_timestamp == original_timestamp and not check.status.lagging + assert matrix.safety.aol_get_permission_mask() == 0 + assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1}) + for _ in range(79): + matrix.tick(fault=(kind, address)) + assert check.status.last_timestamp == original_timestamp + assert matrix.now - original_timestamp == 1_000_000 + assert not check.status.lagging and matrix.safety.aol_get_permission_mask() == 0 + matrix.tick(fault=(kind, address)) + assert check.status.last_timestamp == original_timestamp and check.status.lagging + assert matrix.now - original_timestamp == 1_010_000 + # EPS/MAIN observer DLC and required-source checksum refusals are immediate. + assert matrix.safety.aol_get_permission_mask() == 0 + assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1}) + + +@pytest.mark.parametrize('address', (0x262, 0x1D3, 0x620, 0x3BC)) +def test_wrong_bus_cannot_establish_source(matrix, address): + for _ in range(50): + matrix.tick(fault=('bus', address)) + assert matrix.safety.aol_get_permission_mask() == 0 + + +@pytest.mark.parametrize('address,elapsed', ((0x262, 400001), (0x1D3, 303032))) +def test_original_source_expiry(matrix, address, elapsed): + matrix.warm() + assert matrix.safety.aol_get_permission_mask() == 1 + matrix.now += elapsed + matrix.tick(omit=address) + assert matrix.safety.aol_get_permission_mask() == 0 + + +def test_original_host_request_expiry(matrix): + matrix.warm() + assert matrix.safety.aol_get_permission_mask() == 1 + matrix.safety.set_timer(matrix.now + 300001) + assert matrix.safety.aol_get_permission_mask() == 0 + + +def test_injected_relay_fault_refusal(matrix): + matrix.warm() + matrix.safety.set_relay_malfunction(True) + assert matrix.safety.aol_get_permission_mask() == 0 + assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1}) + + +@pytest.mark.parametrize('experience', (0, 1, 33, 128, 160, 256, 288)) +def test_other_alternative_experiences_remain_unadmitted(experience): + case = Physical(experience=experience) + try: + case.warm() + assert case.safety.aol_get_permission_mask() == 0 + finally: + case.close() + + +def test_brake_and_unbelted_preserve_only_lateral(matrix): + matrix.warm() + matrix.tick(brake=True, cruise=False) + assert matrix.safety.aol_get_permission_mask() == 1 + address, data, bus = matrix.packer.make_can_msg('BODY_CONTROL_STATE', 0, {'SEATBELT_DRIVER_UNLATCHED': 1}) + assert matrix.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)) + assert matrix.safety.aol_get_permission_mask() == 1 + + +def test_heartbeat_loss(matrix): + matrix.warm() + matrix.safety.set_aol_test_heartbeat(False) + assert matrix.safety.aol_get_permission_mask() == 0 + assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1}) + + +def test_open_door_denies_lateral(matrix): + matrix.warm() + address, data, bus = matrix.packer.make_can_msg('BODY_CONTROL_STATE', 0, {'DOOR_OPEN_FL': 1}) + assert matrix.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)) + assert matrix.safety.aol_get_permission_mask() == 0 + + +def test_real_stock_steering_rx_triggers_relay_refusal(matrix): + matrix.warm() + assert not matrix.safety.get_relay_malfunction() + address, data, bus = matrix.packer.make_can_msg('STEERING_LKA', 0, {}) + matrix.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)) + assert matrix.safety.get_relay_malfunction() + assert matrix.safety.aol_get_permission_mask() == 0 + assert not matrix.tx('STEERING_LKA', {'STEER_TORQUE_CMD': 1, 'STEER_REQUEST': 1}) diff --git a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc index c312ce0620..d3612bc77b 100644 --- a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc +++ b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc @@ -179,7 +179,7 @@ int main() { } for (uint32_t word = 0U; word <= 65535U; ++word) { - const bool expected = (word == 0xC042U) || (word == 0xC242U) || (word == 0x8042U) || (word == 0x8242U) || (word == 0xC049U) || (word == 0xC249U) || (word == 73U) || (word == 585U) || (word == 4169U) || (word == 4681U) || (word == 2121U) || (word == 0x9049U) || (word == 0x9249U) || (word == 0x2049U) || (word == 0x2249U) || (word == 0x424DU) || (word == 0x449U) || (word == 0x649U) || (word == 0x1449U) || (word == 0x1649U) || (word == 0x8049U) || (word == 0x8249U) || (word == 0x8449U) || (word == 0x8649U) || (word == 0x9158U) || (word == 0x9358U) || (word == 0x9149U) || (word == 0x9349U) || (word == 0x9164U) || (word == 0x9364U) || (word == 0xd149U) || (word == 0xd349U) || (word == 0xd04dU) || (word == 0xd24dU) || (word == 0x9449U) || (word == 0x9549U) || (word == 0x9558U) || (word == 0x9564U) || (word == 0x9649U) || (word == 0x9749U) || (word == 0x9758U) || (word == 0x9764U) || (word == 0xD44DU) || (word == 0xD549U) || (word == 0xD64DU) || (word == 0xD749U); + const bool expected = (word == 0xC042U) || (word == 0xC242U) || (word == 0x8042U) || (word == 0x8242U) || (word == 0xC049U) || (word == 0xC249U) || (word == 73U) || (word == 585U) || (word == 856U) || (word == 4169U) || (word == 4681U) || (word == 2121U) || (word == 0x9049U) || (word == 0x9249U) || (word == 0x2049U) || (word == 0x2249U) || (word == 0x424DU) || (word == 0x449U) || (word == 0x649U) || (word == 0x1449U) || (word == 0x1649U) || (word == 0x8049U) || (word == 0x8249U) || (word == 0x8449U) || (word == 0x8649U) || (word == 0x9158U) || (word == 0x9358U) || (word == 0x9149U) || (word == 0x9349U) || (word == 0x9164U) || (word == 0x9364U) || (word == 0xd149U) || (word == 0xd349U) || (word == 0xd04dU) || (word == 0xd24dU) || (word == 0x9449U) || (word == 0x9549U) || (word == 0x9558U) || (word == 0x9564U) || (word == 0x9649U) || (word == 0x9749U) || (word == 0x9758U) || (word == 0x9764U) || (word == 0xD44DU) || (word == 0xD549U) || (word == 0xD64DU) || (word == 0xD749U); auto toyota = status(); toyota.safety_mode = 2U; toyota.safety_param = static_cast(word); diff --git a/openpilot/starpilot/car/toyota/aol.py b/openpilot/starpilot/car/toyota/aol.py index 4c9228bddd..7696be40c5 100644 --- a/openpilot/starpilot/car/toyota/aol.py +++ b/openpilot/starpilot/car/toyota/aol.py @@ -9,7 +9,7 @@ AOL_WORDS = frozenset(( 0x8042, 0x8242, 0xC042, 0xC242, 0xC049, 0xC249, 0x9449, 0x9549, 0x9558, 0x9564, 0x9649, 0x9749, 0x9758, 0x9764, 0xD44D, 0xD549, 0xD64D, 0xD749, - 73, 585, 4169, 4681, 2121, 0x9049, 0x9249, + 73, 585, 856, 4169, 4681, 2121, 0x9049, 0x9249, 0x2049, 0x2249, 0x424D, 0x449, 0x649, 0x1449, 0x1649, 0x8049, 0x8249, 0x8449, 0x8649, 0x9158, 0x9358, 0x9149, 0x9349, 0x9164, 0x9364, 0xd149, 0xd349, 0xd04d, 0xd24d, @@ -74,7 +74,20 @@ def angle_qualified(cp, *, marked_only=False): return configured(cp) and cp.alternativeExperience in ((32,) if marked_only else (0, 32)) +def matrix_stock_qualified(cp, *, marked_only=False): + if (cp is None or cp.brand != 'toyota' or cp.carFingerprint != CAR.TOYOTA_MATRIX_RETROFIT or + cp.notCar or cp.passive or cp.dashcamOnly or not cp.pcmCruise or cp.openpilotLongitudinalControl or + cp.secOcRequired or cp.flags != 0 or + cp.steerControlType != structs.CarParams.SteerControlType.torque or len(cp.safetyConfigs) != 1 or + cp.alternativeExperience not in ((32,) if marked_only else (0, 32))): + return False + config = cp.safetyConfigs[0] + return config.safetyModel == structs.CarParams.SafetyModel.toyota and config.safetyParam == 856 + + def qualified(cp, *, marked_only=False): + if matrix_stock_qualified(cp, marked_only=marked_only): + return True if angle_qualified(cp, marked_only=marked_only): return True from opendbc.car.toyota.advert_filter import configured as advert_filter_configured diff --git a/openpilot/starpilot/car/toyota/aol_policy.h b/openpilot/starpilot/car/toyota/aol_policy.h index 9278ac114a..3e03d24a33 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 == 0xC042U || param == 0xC242U || param == 0x8042U || param == 0x8242U || param == 0xC049U) || (param == 0xC249U) || (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) || (param == 0x8049U) || (param == 0x8249U) || (param == 0x8449U) || (param == 0x8649U) || (param == 0x9158U) || (param == 0x9358U) || (param == 0x9149U) || (param == 0x9349U) || (param == 0x9164U) || (param == 0x9364U) || (param == 0xd149U) || (param == 0xd349U) || (param == 0xd04dU) || (param == 0xd24dU) || (param == 0x9449U) || (param == 0x9549U) || (param == 0x9558U) || (param == 0x9564U) || (param == 0x9649U) || (param == 0x9749U) || (param == 0x9758U) || (param == 0x9764U) || (param == 0xD44DU) || (param == 0xD549U) || (param == 0xD64DU) || (param == 0xD749U); + return (param == 0xC042U || param == 0xC242U || param == 0x8042U || param == 0x8242U || param == 0xC049U) || (param == 0xC249U) || (param == 73U) || (param == 585U) || (param == 856U) || (param == 4169U) || (param == 4681U) || (param == 2121U) || (param == 0x9049U) || (param == 0x9249U) || (param == 0x2049U) || (param == 0x2249U) || (param == 0x424DU) || (param == 0x449U) || (param == 0x649U) || (param == 0x1449U) || (param == 0x1649U) || (param == 0x8049U) || (param == 0x8249U) || (param == 0x8449U) || (param == 0x8649U) || (param == 0x9158U) || (param == 0x9358U) || (param == 0x9149U) || (param == 0x9349U) || (param == 0x9164U) || (param == 0x9364U) || (param == 0xd149U) || (param == 0xd349U) || (param == 0xd04dU) || (param == 0xd24dU) || (param == 0x9449U) || (param == 0x9549U) || (param == 0x9558U) || (param == 0x9564U) || (param == 0x9649U) || (param == 0x9749U) || (param == 0x9758U) || (param == 0x9764U) || (param == 0xD44DU) || (param == 0xD549U) || (param == 0xD64DU) || (param == 0xD749U); } inline constexpr AolSafetyProfile TOYOTA_AOL_PROFILE{2U, toyota_aol_param, true}; diff --git a/openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py b/openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py new file mode 100644 index 0000000000..79bc467dda --- /dev/null +++ b/openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py @@ -0,0 +1,157 @@ +"""Manual Matrix stock PCM through real Card, SelfdriveD, Controls and native TX. + +Synthetic boundaries are manual vehicle selection, CAN/perception producers, +input time and USB transport. Native permission, process health and actuator +commands are never forced. This does not establish hardware timing acceptance. +""" +from dataclasses import replace +import time + +import pytest +from opendbc.car.toyota.values import CAR +from openpilot.starpilot.aol.wire import decode_safety, encode_safety +from openpilot.starpilot.car.toyota.tests import test_rav4h_caller_native 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 + + +def run(scenario, bridge): + tick = 0 + actual_vetoes = 0 + fault_ns = 0 + + def physical(packer, **fields): + nonlocal tick, fault_ns + tick += 1 + fields['sdsu'] = None + if scenario in ('startup-main', 'standstill'): + fields['cruise'] = False + fields['stopped'] = scenario == 'standstill' + if scenario == 'pcm-cancellation' and tick >= 260: + fields.update(cruise=False, main=True) + if scenario == 'settings-off' and 260 <= tick < 470: + fields['cruise'] = False + if scenario in ('eps-temporary', 'eps-permanent') and 260 <= tick < 315: + fields['eps'] = 9 if scenario == 'eps-temporary' else 3 + if scenario == 'main-off' and tick >= 260: + fields.update(main=False, cruise=False) + frames = helpers.physical(packer, **fields) + assert len(next(data for address, data, bus in frames if address == 0x262 and bus == 0)) == 5 + if scenario in ('main-expiry', 'eps-expiry') and 260 <= tick < 315: + missing = 0x1D3 if scenario == 'main-expiry' else 0x262 + frames = [frame for frame in frames if not (frame[0] == missing and frame[2] == 0)] + if scenario == 'required-rx' and 260 <= tick < 315: + if tick == 260: + fault_ns = time.monotonic_ns() + frames = [(address, data[:-1] + bytes([data[-1] ^ 1]) if address == 0x260 and bus == 0 else data, bus) + for address, data, bus in frames] + return frames + + def on_state(current, host, cs): + assert host.CP.carFingerprint == CAR.TOYOTA_MATRIX_RETROFIT + assert host.CP.safetyConfigs[0].safetyParam == 856 and host.CP.flags == 0 + assert host.CP.pcmCruise and not host.CP.openpilotLongitudinalControl + assert abs(host.CP.lateralTuning.torque.latAccelFactor - 4.05) < 1e-6 + assert abs(host.CP.lateralTuning.torque.friction - .10) < 1e-6 + if scenario == 'eps-temporary' and 270 <= current < 310: + assert cs.steerFaultTemporary + assert host.aol_card_intent.allowed_latch + if scenario == 'eps-permanent' and 270 <= current < 310: + assert cs.steerFaultPermanent + # Toyota derives the request from physical MAIN. Faults veto commands; + # this is not another family's explicit button latch. + assert not host.aol_card_intent.explicit_latch + assert host.aol_card_intent.allowed_latch == cs.cruiseState.available + + def observe(current, host, cs, cc, safety, native): + assert not cc.longActive and not safety.aol_get_permission_mask() & 2 + if scenario == 'standstill' and 220 <= current < 250: + assert host.CP.minSteerSpeed <= 0 and host.CP.steerAtStandstill + assert cs.vEgo < .01 and cc.latActive and not cc.longActive + if scenario == 'startup-main' and 220 <= current < 250: + assert cs.cruiseState.available and not cs.cruiseState.enabled + assert host.aol_card_intent is not None and host.aol_card_intent.allowed_latch + assert cc.latActive and safety.aol_get_permission_mask() == 1 + if scenario == 'pcm-cancellation' and current >= 300: + assert cs.cruiseState.available and not cs.cruiseState.enabled + assert host.aol_card_intent.allowed_latch and cc.latActive + if scenario in ('main-expiry', 'eps-expiry', 'required-rx', 'native-identity', 'eps-temporary', 'eps-permanent') and 306 <= current < 310: + assert not cc.latActive and not cc.longActive + if scenario == 'eps-permanent' and 270 <= current < 310: + assert cs.steerFaultPermanent and not cc.latActive and not cc.longActive + assert safety.aol_get_permission_mask() == 0 + if scenario == 'required-rx' and 270 <= current < 315: + assert not cc.latActive and not cc.longActive + assert safety.aol_get_permission_mask() == 0 and actual_vetoes > 0 + if current == 270: + wire = next(event for event in native if event.which() == 'aolSafetyWire') + observed = decode_safety(bytes(wire.aolSafetyWire)) + assert observed is not None and not observed.lateralAllowed and not observed.longitudinalAllowed + assert fault_ns <= observed.observedMonoTime <= fault_ns + 100_000_000 + assert observed.validUntilMonoTime - observed.observedMonoTime == 200_000_000 + if scenario == 'main-off' and current >= 290: + assert not cc.latActive and not cc.longActive + if scenario == 'settings-off' and current == 470: + assert not cc.latActive and not cc.longActive + if scenario == 'settings-off' and current >= 480: + assert cc.enabled and cc.latActive and not cc.longActive + assert not host.aol_card_intent.settings.enabled + if scenario == 'settings-off' and 300 <= current < 315: + assert not host.aol_card_intent.settings.enabled + assert not host.aol_card_intent.allowed_latch and not cc.latActive and not cc.longActive + + def rx_expected(address, data, bus, default, cp): + del data, cp + if scenario == 'required-rx' and address == 0x260 and bus == 0 and 260 <= tick < 315: + return False + return default + + def tx_expected(current, frame, cp, safety): + nonlocal actual_vetoes + if scenario == 'required-rx' and 260 <= current < 315 and frame.address == 0x2E4: + assert not safety.safety_config_valid() + assert safety.aol_get_permission_mask() == safety.aol_get_request_mask() == 0 + decoded = helpers.decode(cp, (frame.address, bytes(frame.dat), frame.src), 'STEERING_LKA') + active = bool(decoded['STEER_REQUEST'] or decoded['STEER_TORQUE_CMD']) + # A fresh 10Hz Panda report has not yet reached the host on the first + # corrupt RX. The independent native hook must veto every active packet. + actual_vetoes += int(active) + return not active + return True + + original_native = helpers.native_events + + def native_events(*args, **kwargs): + messages = original_native(*args, **kwargs) + if scenario == 'native-identity' and 260 <= tick < 315: + result = [] + for event in messages: + if event.which() == 'aolSafetyWire': + wire = event.as_reader().as_builder() + state = decode_safety(bytes(wire.aolSafetyWire)) + assert state is not None + wire.aolSafetyWire = encode_safety(replace(state, safetyParam=state.safetyParam ^ 1)) + event = wire.as_reader() + result.append(event) + return result + return messages + + permanent = scenario == 'main-off' + actual_scenario = ('identity_refusal' if permanent else 'physical' if scenario == 'physical' else + 'cold_low' if scenario == 'standstill' else 'nominal' if scenario == 'startup-main' else 'main_lease') + with pytest.MonkeyPatch.context() as monkeypatch: + if scenario == 'native-identity': + monkeypatch.setattr(helpers, 'native_events', native_events) + helpers.run_case(True, actual_scenario, candidate=CAR.TOYOTA_MATRIX_RETROFIT, + bridge=bridge, input_clock=True, aol=scenario != 'settings-off', sdsu_words=(856, 856), + firmware_override=[], fingerprint_frames=[], manual_selection=True, + physical_frames=physical, on_state=on_state, on_sample=observe, + native_rx_expected=rx_expected, native_tx_expected=tx_expected) + + +@pytest.mark.parametrize('scenario', ('startup-main', 'standstill', 'physical', 'pcm-cancellation', 'main-expiry', 'eps-expiry', 'required-rx', + 'native-identity', 'eps-temporary', 'eps-permanent', 'main-off', 'settings-off')) +def test_actual_matrix_stock_lifecycle(scenario, host_heartbeat_bridge): + run(scenario, host_heartbeat_bridge) diff --git a/openpilot/starpilot/car/toyota/tests/test_rav4h_caller_native.py b/openpilot/starpilot/car/toyota/tests/test_rav4h_caller_native.py index 91cc34c620..416f09ddde 100644 --- a/openpilot/starpilot/car/toyota/tests/test_rav4h_caller_native.py +++ b/openpilot/starpilot/car/toyota/tests/test_rav4h_caller_native.py @@ -9,6 +9,7 @@ installed-hardware scheduling or timing acceptance. """ import json +import hashlib import ctypes import gc import time @@ -227,7 +228,7 @@ def control_inputs(controls, published, state, sd_events, native, now): def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H, bridge=None, input_clock=False, brake_stop_check_start=350, sdsu_words=(0x9049, 0x9249), alpha_long=False, disable_setting=None, physical_frames=None, angle=False, firmware_override=None, assert_hybrid_release=False, sdsu_mirror=False, on_sample=None, camera_messages=False, filter_advert=False, - brake_pause_mps=None, native_rx_expected=None, on_state=None, fingerprint_frames=None): + brake_pause_mps=None, native_rx_expected=None, on_state=None, fingerprint_frames=None, manual_selection=False, native_tx_expected=None): SYNTHETIC_LAST.clear() NATIVE_LAST.clear() safety = libsafety_py.libsafety @@ -246,8 +247,12 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H, if brake_pause_mps is not None: params.put('AolBrakePauseSpeedMps', float(brake_pause_mps), block=True) fw = firmware(candidate) if firmware_override is None else firmware_override - exact, found = match_fw_to_car(fw, '', log=False) - assert exact and found == {candidate} + if manual_selection: + assert candidate == CAR.TOYOTA_MATRIX_RETROFIT and stock and not fw + assert firmware_override == [] and fingerprint_frames == [] + else: + exact, found = match_fw_to_car(fw, '', log=False) + assert exact and found == {candidate} fp = gen_empty_fingerprint() if fingerprint_frames is None: fp[0][0x2FF] = 4 if filter_advert else 8 @@ -258,7 +263,8 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H, fp[2][0x2FF] = 8 if camera_messages: fp[2].update({0x343: 8, 0x4CB: 8}) - observed = (candidate, fp, '0' * 17, fw, structs.CarParams.FingerprintSource.fw, True) + source = structs.CarParams.FingerprintSource.fixed if manual_selection else structs.CarParams.FingerprintSource.fw + observed = (candidate, fp, '0' * 17, fw, source, True) with ( patch.object(car_helpers, 'fingerprint', return_value=observed), patch.object(card_module, 'can_comm_callbacks', return_value=(lambda *a: [], lambda x: None)), @@ -426,13 +432,45 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H, cc_event = published['carControl'] cc = cc_event.carControl deliver(host.sm, [cc_event, *native, *sd_events.values()]) + # A valid previous publication is not a new producer event. Card may + # deliberately emit nothing when the current control message is invalid. + previous_sendcan = published.pop('sendcan', None) host.controls_update(cs, cc) outgoing = published.get('sendcan') - assert outgoing is not None - if not outgoing.valid: - assert not outgoing.sendcan and not cs.canValid, ('startup must stay neutral until real CAN health', tick, cs.canValid) - for f in outgoing.sendcan: - assert safety.safety_tx_hook(libsafety_py.make_CANPacket(f.address, f.src, bytes(f.dat))), (tick, hex(f.address), bytes(f.dat).hex()) + if outgoing is None: + assert not cs.canValid or cs.canTimeout + assert not host.sm.valid['carControl'] + assert not cc.latActive and not cc.longActive + assert safety.aol_get_request_mask() == safety.aol_get_permission_mask() == 0 + outgoing_frames = () + else: + outgoing_frames = outgoing.sendcan + if not outgoing.valid: + assert not outgoing_frames and not cs.canValid, ('startup must stay neutral until real CAN health', tick, cs.canValid) + native_tx_accepted = native_tx_rejected = 0 + for f in outgoing_frames: + before_tx = { + 'nativePermission': int(safety.aol_get_permission_mask()), 'nativeRequest': int(safety.aol_get_request_mask()), + 'controlsAllowed': bool(safety.get_controls_allowed()), 'configValid': bool(safety.safety_config_valid()), + 'heartbeatEngaged': ctypes.c_bool.in_dll(native_globals, 'heartbeat_engaged').value, + 'globals': {name: ctypes.c_int.in_dll(native_globals, name).value for name in + ('desired_torque_last', 'rt_torque_last', 'valid_steer_req_count', 'invalid_steer_req_count')}, + } + expected_tx = True if native_tx_expected is None else native_tx_expected(tick, f, host.CP, safety) + assert type(expected_tx) is bool + accepted_tx = bool(safety.safety_tx_hook(libsafety_py.make_CANPacket(f.address, f.src, bytes(f.dat)))) + native_tx_accepted += int(accepted_tx) + native_tx_rejected += int(not accepted_tx) + if accepted_tx != expected_tx: + refusal = {'tick': tick, 'candidate': str(candidate), 'scenario': scenario, 'address': f.address, + 'data': bytes(f.dat).hex(), 'bus': f.src, 'outgoingValid': bool(outgoing.valid), + 'CP': host.CP.to_dict(), 'CS': cs.to_dict(), 'CC': cc.to_dict(), 'beforeNativeTX': before_tx, + 'controllerLastTorque': host.CI.CC.last_torque, + 'nativeAfterPermission': int(safety.aol_get_permission_mask()), + 'axis': {'lateral': wanted.desired_lateral, 'longitudinal': wanted.desired_longitudinal}, + 'state': str(sd.state_machine.state), 'fault': sd.aol_fault_reason} + (OUT / 'matrix-native-tx-refusal.json').write_text(json.dumps(refusal, indent=2, default=str) + '\n') + assert accepted_tx == expected_tx, (tick, hex(f.address), bytes(f.dat).hex(), accepted_tx, expected_tx) if f.address == 0x2E4: decoded = decode(host.CP, (f.address, bytes(f.dat), f.src), 'STEERING_LKA') positive_steering |= bool(decoded['STEER_REQUEST'] and decoded['STEER_TORQUE_CMD']) @@ -449,6 +487,12 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H, { 'tick': tick, 'canValid': cs.canValid, + 'freshSendcan': outgoing is not None, + 'actualSendcanFrames': len(outgoing_frames), + 'nativeAcceptedFrames': native_tx_accepted, + 'nativeRejectedFrames': native_tx_rejected, + 'previousSendcanMonoTime': int(previous_sendcan.logMonoTime) if previous_sendcan is not None else 0, + 'previousSendcanSha256': hashlib.sha256(previous_sendcan.as_builder().to_bytes()).hexdigest() if previous_sendcan is not None else '', 'lat': cc.latActive, 'long': cc.longActive, 'sd': str(sd.state_machine.state), @@ -514,7 +558,7 @@ def run_case(stock, scenario='nominal', *, aol=True, candidate=CAR.TOYOTA_RAV4H, host.CI.CC.advert_filter_input if filter_advert or fingerprint_frames is not None and any( address == 0x2AA for address, _, _ in fingerprint_frames) else host.CI.CC.rav4h_filter_input) assert preference is not None and not preference.update(time.monotonic_ns()) - for f in outgoing.sendcan: + for f in outgoing_frames: if f.address == 0x343: acc = decode(host.CP, (f.address, bytes(f.dat), f.src), 'ACC_CONTROL') assert acc['ACCEL_CMD'] == 0 and acc['CANCEL_REQ'] and not acc['RELEASE_STANDSTILL'] diff --git a/prebuilt.json b/prebuilt.json index f1ee17a3f8..72d8a64b2a 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "10bc0a35396529a54c069434d8d2cc9f0b63f814338ab004b3b783bcc4eda793", "schema_version": 1} +{"protected_inventory_sha256": "6f7a43aaa35dc95e301b5f12a1ca0bad066c62c21c3542bde54ea96478ffa111", "schema_version": 1} diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index 975b86c0c0..45adfb41b7 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -159,6 +159,8 @@ AOL_LIFECYCLE_TARGETS = ( "openpilot/starpilot/car/subaru/tests/test_smoothing_settings.py", "openpilot/starpilot/car/subaru/tests/test_smoothing_model_join.py", "opendbc_repo/opendbc/safety/tests/test_subaru_outback_angle.py", + 'openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py', + 'opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py', ) AOL_LIFECYCLE_SCOPE = { @@ -454,6 +456,8 @@ RELEASE_TARGETS = ( "opendbc_repo/opendbc/safety/tests/test_subaru_forester_angle.py", "opendbc_repo/opendbc/safety/tests/test_subaru_receive_guards.py", "opendbc_repo/opendbc/safety/tests/test_subaru_outback_angle.py", + 'openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py', + 'opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py', ) # Vehicle tests import both the vendored car code and these host control owners. diff --git a/tools/ci/tests/test_matrix_registration.py b/tools/ci/tests/test_matrix_registration.py new file mode 100644 index 0000000000..ab9b6bb2b3 --- /dev/null +++ b/tools/ci/tests/test_matrix_registration.py @@ -0,0 +1,27 @@ +"""Matrix qualification must remain mandatory in lifecycle and RELEASE CI.""" +import ast +from pathlib import Path +import unittest + + +ROOT = Path(__file__).resolve().parents[3] +CALLER = 'openpilot/starpilot/car/toyota/tests/test_matrix_stock_caller_native.py' +NATIVE = 'opendbc_repo/opendbc/safety/tests/test_toyota_matrix_stock_aol.py' + + +class TestMatrixRegistration(unittest.TestCase): + def test_matrix_actual_callers_and_native_are_registered(self): + module = ast.parse((ROOT / 'tools/ci/run_vehicle_tests.py').read_text()) + values = {node.targets[0].id: ast.literal_eval(node.value) for node in module.body + if isinstance(node, ast.Assign) and len(node.targets) == 1 + and isinstance(node.targets[0], ast.Name) + and node.targets[0].id in ('AOL_LIFECYCLE_TARGETS', 'RELEASE_TARGETS')} + for collection in ('AOL_LIFECYCLE_TARGETS', 'RELEASE_TARGETS'): + for target in (CALLER, NATIVE): + assert values[collection].count(target) == 1 + assert (ROOT / target).is_file() + native = ast.parse((ROOT / NATIVE).read_text()) + caller = ast.parse((ROOT / CALLER).read_text()) + for module in (native, caller): + assert any(isinstance(node, ast.Assign) and any(isinstance(target, ast.Name) + and target.id == 'source_matched_native' for target in node.targets) for node in module.body)