diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index e58b717302..321c171bef 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -90,7 +90,7 @@ from openpilot.starpilot.feature_runtime import (requested as feature_requested, from openpilot.starpilot.saved_source import read_saved from openpilot.starpilot.vehicle_selection import startup_candidate from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences -from openpilot.starpilot.controller_extensions import configure_controller +from openpilot.starpilot.controller_extensions import configure_controller, observe_controller_can from openpilot.starpilot.vehicle_startup import VehicleStartupOwner from openpilot.starpilot.controllers.wheel_actions import WheelPublisher, key_for from openpilot.starpilot.controllers.cruise_action import CruiseActionConsumer, eligible as cruise_action_eligible @@ -513,6 +513,7 @@ class Car: can_list = can_capnp_to_list(can_strs) # Update carState from CAN + observe_controller_can(self.CI, can_list) CS = self.CI.update(can_list) from openpilot.starpilot.car.gm.aol import f1_conventional_lateral_owner cancel_boot_ns = (time.clock_gettime_ns(time.CLOCK_BOOTTIME) diff --git a/openpilot/starpilot/car/ford/bluepilot.py b/openpilot/starpilot/car/ford/bluepilot.py index 71d45d0645..cd22d2ffcb 100644 --- a/openpilot/starpilot/car/ford/bluepilot.py +++ b/openpilot/starpilot/car/ford/bluepilot.py @@ -1,11 +1,20 @@ """BluePilot lateral: StarPilot's startup settings and the per-frame inputs its Ford CarController reads (opendbc bluepilot_lateral/hosts/starpilot).""" +from collections import deque +import math +import numpy as np + import openpilot.cereal.messaging as messaging from openpilot.common.swaglog import cloudlog from openpilot.starpilot.saved_source import read_saved from opendbc.bluepilot_lateral.core.values_ext import BP_LATERAL_PARAMS -from opendbc.bluepilot_lateral.hosts.starpilot.lateral import FrameInputs +from opendbc.bluepilot_lateral.hosts.starpilot.lateral import BluePilotLateral, FrameInputs +from opendbc.bluepilot_lateral.core.lateral_curv_ext import PrimaryLateralControl, MAX_LATERAL_ACCEL +from opendbc.bluepilot_lateral.core.values_ext import BP_ANGLE_LIMITS +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 import structs STARTUP_KEYS = ("FordPrefSteerAngleCurvature",) # Global StarPilot lane settings are the only lane-positioning owner. @@ -93,3 +102,79 @@ class BluePilotInputs: li.angleOffsetDeg = vp.angleOffsetDeg li.lateralDelay = self.sm["lateralDelay"].lateralDelay return frame + + +class FordBluePilotController(BluePilotLateral): + """Keep the library's curvature handoff inside its unchanged native convergence envelope.""" + + def __init__(self, CP, packer, CAN): + super().__init__(CP, packer, CAN) + self.measured_history = deque([0] * 6, maxlen=6) + self.measured_speed = 0 + self.measured_seen = False + + def observe_can(self, can_list): + # Match Panda's receive order: yaw can precede the new speed in the same batch. + if not self.CP.flags & FordFlags.CANFD or self.inputs.bp_pinion_curvature_enabled: + return + for _, batch in can_list: + for address, data, bus in batch: + if bus != self.CAN.main or len(data) != 8: + continue + if address == 0x415 and data[2] >> 6 == 3: + speed = np.float32(((data[0] << 8) | data[1]) * .01 * (1. / 3.6)) + self.measured_speed = math.floor(float(np.float32(speed * 1000.)) + .5) + elif address == 0x91 and (data[6] >> 4) & 3 == 3: + yaw = np.float32(((data[2] << 8) | data[3]) * .0002 - 6.5) + speed = np.float32(self.measured_speed / 1000.) + curvature = np.float32(yaw / max(speed, np.float32(.1))) + raw = float(np.float32(curvature * 50000.)) + self.measured_history.append(math.floor(raw + .5) if raw >= 0 else math.ceil(raw - .5)) + self.measured_seen = True + + def step_lateral(self, CC, CS, actuators, apply_curvature_last): + packets, applied = super().step_lateral(CC, CS, actuators, apply_curvature_last) + state = CS.out + if (not CC.latActive or not self.CP.flags & FordFlags.CANFD or + self.curv.primary_lateral_control == PrimaryLateralControl.angle or applied == 0. or + not state.canValid or state.canTimeout or state.steerFaultTemporary or state.steerFaultPermanent or + not math.isfinite(state.vEgoRaw) or state.vEgoRaw <= 10.): + return packets, applied + measured = self.inputs.get_current_curvature(CS) + if not math.isfinite(measured): + return packets, applied + error = CarControllerParams.CURVATURE_ERROR + low, high = measured - error, measured + error + if self.measured_seen and not self.inputs.bp_pinion_curvature_enabled: + low = max(low, -max(self.measured_history) * .00002 - error) + high = min(high, -min(self.measured_history) * .00002 + error) + if low > high: + return packets, applied + limit = min(self.curv.curvature_max, MAX_LATERAL_ACCEL / state.vEgoRaw ** 2) + if state.steeringPressed and (low > limit or high < -limit): + # Use the curvature controller's neutral reset while the driver exceeds its acceleration cap. + inactive = structs.CarControl.new_message(latActive=False) + super().step_lateral(inactive, CS, actuators, apply_curvature_last) + for index, (address, data, bus) in enumerate(packets): + if address == 0x3D6 and bus == self.CAN.main: + packets[index] = create_lat_ctl2_msg(self.packer, self.CAN, 1, 3, 1, 0., 0., 0., 0., (data[7] >> 1) & 0xF) + return packets, 0. + return packets, applied + delta = float(np.interp(state.vEgoRaw, *BP_ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN)) + # Rejoin the measured band with the library's existing unwind rate. + target = float(np.clip(applied, low, high)) + corrected = float(np.clip(target, apply_curvature_last - delta, apply_curvature_last + delta)) + corrected = float(np.clip(corrected, -limit, limit)) + if corrected == applied: + return packets, applied + for index, (address, data, bus) in enumerate(packets): + if address != 0x3D6 or bus != self.CAN.main or (data[0] >> 4) & 7 != 1: + continue + raw = round(-corrected / 0.00002) + 1000 + payload = bytearray(data) + payload[2] = raw >> 3 + payload[3] = (payload[3] & 0x1F) | ((raw & 7) << 5) + payload[1] = lat_ctl2_checksum(1, (payload[7] >> 1) & 0xF, bytes(payload)) + packets[index] = (address, bytes(payload), bus) + return packets, float((1000 - raw) * 0.00002) + return packets, applied diff --git a/openpilot/starpilot/controller_extensions.py b/openpilot/starpilot/controller_extensions.py index f6091cf257..aaafe8fcd1 100644 --- a/openpilot/starpilot/controller_extensions.py +++ b/openpilot/starpilot/controller_extensions.py @@ -29,8 +29,10 @@ def configure_controller(CI, params): controller.hands_free_cluster_input = FordClusterPreference(params) from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import lateral_inputs_required if controller is not None and lateral_inputs_required(cp) and hasattr(controller, "bluepilot_inputs"): - from openpilot.starpilot.car.ford.bluepilot import BluePilotInputs + from openpilot.starpilot.car.ford.bluepilot import BluePilotInputs, FordBluePilotController controller.bluepilot_inputs = BluePilotInputs(params) + if hasattr(controller, "bp") and controller.frame == 0: + controller.bp = FordBluePilotController(cp, controller.packer, controller.CAN) if controller is not None and cp.brand == 'gm': from opendbc.car.gm.values import (is_bolt_euv_longitudinal, is_volt_longitudinal, camera_acc_pedal_profile, volt_cc_pedal_profile) @@ -170,3 +172,11 @@ def configure_controller(CI, params): except OSError: controller.torque_ev_lead_inputs = None cloudlog.exception('Optional torque EV lead input transport unavailable') + + +def observe_controller_can(CI, can_list): + controller = CI.CC + if controller is not None and CI.CP.brand == 'ford': + from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController + if isinstance(getattr(controller, 'bp', None), FordBluePilotController): + controller.bp.observe_can(can_list) diff --git a/openpilot/starpilot/tests/ford_lifecycle_helpers.py b/openpilot/starpilot/tests/ford_lifecycle_helpers.py index 68fe83b9cb..e28f599535 100644 --- a/openpilot/starpilot/tests/ford_lifecycle_helpers.py +++ b/openpilot/starpilot/tests/ford_lifecycle_helpers.py @@ -333,7 +333,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag fields['CcAslButtnCnclResPress'] = int(cancel) if scenario in ('route49', 'cancel-native-cadence', 'neutral-main-cadence', 'bluepilot-gas', 'bluepilot-cancel-gap') and name == 'Steering_Data_FD1': fields['CcAslButtnCnclResPress'] = int(cancel) - if scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert') and name == 'EPAS_INFO': + if scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert', 'curvature-handoff') and name == 'EPAS_INFO': fields['SteeringColumnTorque'] = 2 if steering else 0 if scenario == 'route45' and name == 'Lane_Assist_Data3_FD1': fields['LatCtlSte_D_Stat'] = 0 if fault in ('temporary-lkas', 'temporary-lkas-gas') else 1 @@ -372,6 +372,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag heartbeat=heartbeat and fault != 'heartbeat', speed=(3.1 + .12 * ((steps // 2) % 12) / 11 if getattr(monkeypatch, "ford_speed_window", False) else getattr(joined, 'speed', 15.0)), eps=1 if fault in ('temporary-eps', 'temporary-eps-gas') else 0, + curvature=getattr(joined, 'measured_curvature', 0.), ) assert native.tick == steps safety.safety_tick() @@ -487,7 +488,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag getattr(monkeypatch, 'ford_main_off_warmup', False) and steps <= start_tick + 130 or availability_transition and not state.cruiseState.enabled or scenario == 'route45' and getattr(monkeypatch, 'ford_route45_refusal', None) == 'main-off') - assert scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert') or not state.steeringPressed + assert scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert', 'curvature-handoff') or not state.steeringPressed assert bool(state.gasPressed) == (fault in ('gas', 'temporary-eps-gas', 'temporary-lkas-gas') and not any(frame[0] == 0x204 for frame in delayed_pedal_frames)) if not source_gap: @@ -685,6 +686,31 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag assert not command.enabled and not command.latActive and not command.longActive assert sd_state.alertType != 'steerTempUnavailableSilent/warning', 'Unarmed parked startup shows steering warning' return + if scenario == 'curvature-handoff': + from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController + assert isinstance(host.CI.CC.bp, FordBluePilotController) + for _ in range(41): + state, axis, sd_state, command = step() + joined.speed = monkeypatch.ford_curve_speed + joined.measured_curvature = monkeypatch.ford_curve_sign * min(.018, (3.8 if monkeypatch.ford_curve_manual else 2.0) / joined.speed ** 2) + for _ in range(40): + state, axis, sd_state, command = step() + assert abs(state.yawRate / state.vEgoRaw - joined.measured_curvature) < .0001 + for tick in range(70): + state, axis, sd_state, command = step(engaged=True, steering=monkeypatch.ford_curve_manual, fault='gas' if monkeypatch.ford_curve_gas else None) + if tick >= 40: + assert command.latActive and host.aol_card_intent.allowed_latch + assert not state.steerFaultTemporary and not state.steerFaultPermanent + assert sd_state.alertType not in ('controlsMismatch/immediateDisable', 'commIssue/softDisable') + if monkeypatch.ford_curve_manual: + assert state.steeringPressed and host.CI.CC.apply_curvature_last == 0. + joined.measured_curvature = monkeypatch.ford_curve_sign * (1.6 / joined.speed ** 2) + for _ in range(70): + state, axis, sd_state, command = step(engaged=True) + assert not state.steerFaultTemporary and not state.steerFaultPermanent + assert command.latActive and host.aol_card_intent.allowed_latch + assert abs(host.CI.CC.apply_curvature_last) > 0. + return if scenario == 'bluepilot-gas': for _ in range(50): state, axis, sd_state, command = step(engaged=True, fault='gas') diff --git a/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py b/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py index a152ed74a3..eb56fee213 100644 --- a/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py +++ b/openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py @@ -100,3 +100,18 @@ def test_manual_steering_retires_held_turn_limit_alert(ford_joined, monkeypatch) def test_fresh_cruise_withdrawal_after_previous_output_expires(ford_joined, monkeypatch, brake): monkeypatch.ford_gap_brake = brake run_actual_producer(ford_joined, monkeypatch, scenario='bluepilot-cancel-gap') + + +@pytest.mark.parametrize('identity,alpha,hands_free,topology,mode,pinion', [ + ((CAR.FORD_MUSTANG_MACH_E_MK1, 2), True, True, 'automatic', 'curvature', False), +]) +@pytest.mark.parametrize('speed', [12.4, 13.6, 16., 25.]) +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('gas', [False, True]) +@pytest.mark.parametrize('manual', [False, True]) +def test_reengaging_in_a_manual_curve_respects_native_convergence(ford_joined, monkeypatch, speed, sign, gas, manual): + monkeypatch.ford_curve_speed = speed + monkeypatch.ford_curve_sign = sign + monkeypatch.ford_curve_gas = gas + monkeypatch.ford_curve_manual = manual + run_actual_producer(ford_joined, monkeypatch, scenario='curvature-handoff') diff --git a/openpilot/starpilot/tests/test_ford_ci_registration.py b/openpilot/starpilot/tests/test_ford_ci_registration.py index cfdecda284..67a6e58603 100644 --- a/openpilot/starpilot/tests/test_ford_ci_registration.py +++ b/openpilot/starpilot/tests/test_ford_ci_registration.py @@ -43,3 +43,16 @@ def test_actual_host_collector_discovers_release_ui_guards(tmp_path): target.parent.mkdir(parents=True) target.write_bytes((runner.ROOT / name).read_bytes()) assert name in pytest_files(tmp_path) + + +def test_imported_native_cases_retain_complete_release_coverage(): + native = {str(path.relative_to(runner.ROOT)) for path in + (runner.ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests').glob('test_*.py')} + assert native + assert native <= set(runner.targets_for('safety-release')) + assert native <= set(runner.targets_for('safety-debug')) + assert native.isdisjoint(runner.targets_for('aol-lifecycle')) + # Actual host/native AOL lifecycle contracts still run in both required lanes. + for path in FAMILY_TARGETS: + assert path in runner.targets_for('aol-lifecycle') + assert path in runner.targets_for('safety-release') diff --git a/prebuilt.json b/prebuilt.json index fd6ba599ab..b71a28d9a6 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "ffc9729968978ec4246b4ae402960624a53120f1dc0ae93bb2133678f20d63bb", "schema_version": 1} +{"protected_inventory_sha256": "723acc8c1cdc255733f98c7c5d8d5ad375f8ae21683d07f75f0b0b5f6e2b4c77", "schema_version": 1} diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index 1a38d3973e..9047dd9873 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -29,10 +29,13 @@ FORD_BLUEPILOT_TARGETS = ( 'openpilot/starpilot/tests/test_bluepilot_vendor_pin.py', 'opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_hooks.py', *(str(path.relative_to(ROOT)) - for folder in ('opendbc_repo/opendbc/bluepilot_lateral/tests', 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests') - for path in sorted((ROOT / folder).glob('test_*.py'))), + for path in sorted((ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/tests').glob('test_*.py'))), ) +# Ordinary native suites include inherited, intentionally unsupported cases; RELEASE preserves those skips. +FORD_BLUEPILOT_NATIVE_TARGETS = tuple(str(path.relative_to(ROOT)) for path in sorted( + (ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests').glob('test_*.py'))) + # Representative production callers plus their source/native refusal cases. # The emitted selected_test_ids record exact parametrized configurations. AOL_LIFECYCLE_TARGETS = ( @@ -577,9 +580,9 @@ def targets_for(suite): paths = [str(path.relative_to(ROOT)) for path in sorted((ROOT / "opendbc_repo/opendbc/safety/tests").glob("test_*.py"))] return [selected for path in paths for selected in DEBUG_SAFETY_CLASS_TARGETS.get(path, (path,))] + [ "openpilot/starpilot/aol/tests/test_classic_startup_neutral_replacement.py", - ] + ] + list(FORD_BLUEPILOT_NATIVE_TARGETS) if suite == "safety-release": - return list(RELEASE_TARGETS + FORD_BLUEPILOT_TARGETS) + return list(RELEASE_TARGETS + FORD_BLUEPILOT_TARGETS + FORD_BLUEPILOT_NATIVE_TARGETS) raise ValueError(f"Unknown vehicle test suite: {suite}")