diff --git a/openpilot/starpilot/car/ford/bluepilot.py b/openpilot/starpilot/car/ford/bluepilot.py index 2bea906a27..d65b2afe5d 100644 --- a/openpilot/starpilot/car/ford/bluepilot.py +++ b/openpilot/starpilot/car/ford/bluepilot.py @@ -18,6 +18,7 @@ from opendbc.bluepilot_lateral.core.lateral_angle_ext import LateralAngleExt from opendbc.bluepilot_lateral.core.lateral_inputs import lateral_inputs_complete from opendbc.bluepilot_lateral.can.fordcan_ext import lat_ctl2_checksum, create_lat_ctl2_msg from opendbc.car.ford.values import CarControllerParams, FordFlags +from opendbc.car.ford.aol import qualified as qualified_aol from opendbc.car import structs from opendbc.car.vehicle_model import calc_slip_factor from opendbc.bluepilot_lateral.core.carcontroller_ext import pinion_vehicle_model @@ -43,6 +44,17 @@ HOST_SETTINGS = { SETTINGS_REFRESH_FRAMES = 300 # 3 s of card's 100 Hz loop +def quiet_cluster_display(address, data): + if len(data) != 8: + return False + if address == 0x18A: + return ((data[5] >> 2) & 7) <= 1 and not ( + data[0] & 0xE0 or data[1] & 1 or data[3] & 0xF0 or data[4] & 0xC0 or data[5] & 0x80 or data[6] & 0x3F) + if address == 0x3D8: + return data[6] >> 3 in (0, 30) and not (data[4] & 0x0D or data[5] & 0x18 or data[6] & 6) + return False + + def startup_params(params, *, enabled: bool) -> dict[str, str]: """The startup settings init_ford_safety_param_starpilot reads. BluePilot lateral stays off without the openpilot toggle, in Safe Mode, or when a startup setting can't be read. Legacy stock selection is retired.""" @@ -234,6 +246,9 @@ class FordBluePilotController(BluePilotLateral): return self._constrain_curvature(CC, CS, actuators, apply_curvature_last, packets, applied) def order_can_sends(self, frames): + if not self.CP.flags & (FordFlags.CANFD | FordFlags.NEW_PORT) and qualified_aol(self.CP, marked_only=True) and not self.lateral_active: + # Inactive classic steering returns display ownership to the camera. + frames = [(a, d, b) for a, d, b in frames if b != self.CAN.main or not quiet_cluster_display(a, d)] if (not self.starpilot_selected or self.curv.primary_lateral_control != PrimaryLateralControl.angle or self.frame % CarControllerParams.STEER_STEP or self.frame % CarControllerParams.LKA_STEP): return frames diff --git a/openpilot/starpilot/tests/test_ford_starpilot_ordering.py b/openpilot/starpilot/tests/test_ford_starpilot_ordering.py index 4b3070d789..eab3577f6b 100644 --- a/openpilot/starpilot/tests/test_ford_starpilot_ordering.py +++ b/openpilot/starpilot/tests/test_ford_starpilot_ordering.py @@ -17,6 +17,7 @@ from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_saf from opendbc.bluepilot_lateral.core.lateral_curv_ext import PrimaryLateralControl from opendbc.safety.tests.libsafety import libsafety_py from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController +from openpilot.starpilot.car.ford.controller_selection import FordController from openpilot.starpilot.controller_extensions import order_controller_can from openpilot.starpilot.tests.test_ford_native_pinion_guard import LearnedGeometryInputs @@ -89,6 +90,27 @@ def native_run(identity, direction, *, ordered, invalid_shadow=False, withdraw=F safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) +@pytest.mark.parametrize('message,signal,value', [ + ('ACCDATA_3', 'FcwVisblWarn_B_Rq', 1), ('ACCDATA_3', 'FcwAudioWarn_B_Rq', 1), + ('ACCDATA_3', 'TjaWarn_D_Rq', 3), ('ACCDATA_3', 'TjaMsgTxt_D_Dsply', 1), + ('ACCDATA_3', 'AccMsgTxt_D2_Rq', 10), ('ACCDATA_3', 'AccWarn_D_Dsply', 1), + ('ACCDATA_3', 'FcwMsgTxt_D_Rq', 2), ('ACCDATA_3', 'Tja_D_Stat', 5), + ('IPMA_Data', 'LaHandsOff_D_Dsply', 1), ('IPMA_Data', 'LaDenyStats_B_Dsply', 1), + ('IPMA_Data', 'CamraStats_D_Dsply', 3), ('IPMA_Data', 'DasWarn_D_Dsply', 2), + ('IPMA_Data', 'LaActvStats_D_Dsply', 3), +]) +def test_classic_pause_preserves_dashboard_warnings(message, signal, value): + fp = gen_empty_fingerprint() + fp[0][0x5A] = 8 + cp = CarInterface.get_params(CAR.FORD_EXPLORER_MK6, fp, [], True, False, False) + cp.alternativeExperience = 32 + controller = cast(Any, FordBluePilotController.__new__(FordBluePilotController)) + controller.CP, controller.CAN = cp, NS(main=0) + controller.lateral_active = controller.starpilot_selected = False + packet = CANPacker('ford_lincoln_base_pt').make_can_msg(message, 0, {signal: value}) + assert controller.order_can_sends([packet]) == [packet] + + @pytest.mark.parametrize('identity', [CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_EXPLORER_MK6, CAR.FORD_F_150_MK14]) @pytest.mark.parametrize('direction', [-1., 1.]) def test_current_shadow_prevents_coincident_native_refusal_without_extra_packets(identity, direction): @@ -115,7 +137,7 @@ def test_ordering_preserves_buses_payloads_and_noncoincident_cadences(main, canf controller.starpilot_selected = True controller.curv = NS(primary_lateral_control=PrimaryLateralControl.angle) controller.CAN = NS(main=main) - controller.CP = NS(flags=FordFlags.CANFD if canfd else 0) + controller.CP = structs.CarParams(flags=int(FordFlags.CANFD if canfd else 0)).as_reader() lmc = (0x3D6 if canfd else 0x3D3, bytes(8), main) lka = (0x3CA, bytes(8), main) unrelated = (0x165, b'12345678', main + 2) @@ -148,3 +170,58 @@ def test_steering_ordering_is_required_in_both_native_ci_builds(): path = 'openpilot/starpilot/tests/test_ford_starpilot_ordering.py' assert targets_for('safety-debug').count(path) == 1 assert targets_for('safety-release').count(path) == 1 + + +@pytest.mark.parametrize('identity', [CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_ESCAPE_MK4, CAR.FORD_EXPLORER_MK6, + CAR.FORD_FOCUS_MK4, CAR.FORD_MAVERICK_MK1, + CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_F_150_MK14]) +@pytest.mark.parametrize('alpha', [False, True]) +@pytest.mark.parametrize('choice', list(FordController)) +@pytest.mark.parametrize('aol', [False, True]) +def test_classic_aol_pause_returns_dashboard_to_camera_without_changing_actuators(identity, alpha, choice, aol): + fp = gen_empty_fingerprint() + fp[0][0x5A] = 8 + fp[2].update({0x3D3: 8, 0x186: 8}) + cp = CarInterface.get_params(identity, fp, [], alpha, False, False) + cp.alternativeExperience = 32 if aol else 0 + init_ford_safety_param_starpilot(cp, STARTUP['on+pinion']) + controller = CarController(DBC[cp.carFingerprint], cp) + inputs = LearnedGeometryInputs(cp, 1., 0.) + inputs.controller_choice = choice + inputs.settings['FordPrefLateralControl'] = b'0' if choice == FordController.BLUEPILOT_CURVATURE else b'1' + controller.bluepilot_inputs = inputs + controller.bp = FordBluePilotController(cp, controller.packer, controller.CAN, inputs) + ci = NS(CP=cp, CC=controller) + state = CarState(cp) + state.update(state.get_can_parsers(cp)) + safety = libsafety_py.libsafety + safety.init_tests() + safety.set_alternative_experience(cp.alternativeExperience) + assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam) == 0 + try: + for frame in range(11): + active = frame < 5 or frame >= 10 + cs = structs.CarState(vEgo=10. if active else 0., vEgoRaw=10. if active else 0., standstill=not active, + canValid=True, gearShifter=structs.CarState.GearShifter.drive) + cs.cruiseState.available = cs.cruiseState.enabled = True + cs.cruiseState.standstill = not active + state.out = cs.as_reader() + cc = structs.CarControl(enabled=True, latActive=active, longActive=alpha) + cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.stopping + _, original = controller.update(cc.as_reader(), state, (1_000_000 + frame * 10_000) * 1000) + actual = order_controller_can(ci, original) + ui = [(a, d, b) for a, d, b in original if a in (0x18A, 0x3D8) and b == controller.CAN.main] + if active or not aol or cp.flags & FordFlags.CANFD: + assert Counter(actual) == Counter(original) + else: + assert Counter(actual) == Counter(original) - Counter(ui) + if frame == 5: + assert {a for a, _, _ in ui} == {0x18A, 0x3D8} + assert any(a == 0x3D3 for a, _, _ in actual) + # The native camera path stays available; no authority is seeded by this routing test. + for address in (0x18A, 0x3D8): + assert safety.safety_fwd_hook(2, address) == 0 + if frame == 6: + assert any(a == 0x186 for a, _, _ in actual) == alpha + finally: + safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) diff --git a/prebuilt.json b/prebuilt.json index ceec9ededd..03fb6f180b 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "b93d12cdd85e78c16f978b68bd56777fab78fa2a3a94beb99a7ed0a2a635466f", "schema_version": 1} +{"protected_inventory_sha256": "11d2c1f5dc49e617d15a743babaf1659da1179e9771734e45ed74491c64a03f0", "schema_version": 1}