diff --git a/openpilot/starpilot/car/ford/starpilot_controller.py b/openpilot/starpilot/car/ford/starpilot_controller.py index 8eb4b63951..cb15c5ecd9 100644 --- a/openpilot/starpilot/car/ford/starpilot_controller.py +++ b/openpilot/starpilot/car/ford/starpilot_controller.py @@ -9,7 +9,8 @@ StarPilot owns the bounded demand conditioning; BluePilot does not maintain this Ford's path-angle signal (radians), not a steering-wheel angle, remains the sole actuator: kappa_cmd * speed * the BluePilot platform gain. Planner/model prediction, -driver release, PSCM recovery, saturation and wire limits retain the pinned strategy. +human-turn handling, reactive recovery, saturation and wire limits retain the pinned strategy. +Ordinary steering-pressure release does not trigger a reset; measured-deviation recovery remains. StarPilot conditions small blended-curvature corrections before those limiters; it adds no integral, lane-position controller, torque field or extra wire authority. @@ -88,13 +89,6 @@ _STALL_HOLD_S = 0.5 # accumulated clip-binding time before a pulse fire _STALL_BLIP_FRAMES = 6 # mode-0 pulse length (6 frames @ 20 Hz = 300 ms; PSCM acked mode 0 in ~150 ms on-road) _STALL_COOLDOWN_S = 2.0 # re-arm delay after a pulse (release ramp + PSCM response time) _STALL_MAX_BLIPS = 3 # give up on a stuck episode; devLim telemetry keeps recording the stall -# Proactive hand-off blip: any sustained driver press attenuates the PSCM (route 000000be seg 4: -# 3 s of sub-45-deg circle-exit steering left it at ~0x delivery, and the reactive detector's -# fire-after-the-stall-develops timing meant 2.4 s of dead-straight running into the next curve -# before the pulse landed). Firing the same pulse on the falling edge of a sustained press resets -# the PSCM while the car is straight and the command is small -- a 300 ms lateral gap right at -# hand-off, imperceptible, instead of a missed curve. The reactive detector above stays as backstop. -_PRESS_BLIP_MIN_S = 0.5 # press must last this long before its release earns a pulse # The pulse releases steering for 300 ms; never fire it in a curve. _BLIP_MAX_PATH_ANGLE = 0.10 # rad @@ -187,7 +181,6 @@ class StarPilotFordController: self.stall_blip_cooldown_s = 0.0 # re-arm delay after a pulse self.stall_blip_count = 0 # pulses fired this stall episode self.angle_stall_blip_active = False - self.press_timer_s = 0.0 # continuous steeringPressed time, for the hand-off blip def update_angle_params(self, params): """Keep platform gains; the separate BluePilot options do not retune StarPilot.""" @@ -255,7 +248,6 @@ class StarPilotFordController: self.stall_blip_cooldown_s = 0.0 self.stall_blip_count = 0 self.angle_stall_blip_active = False - self.press_timer_s = 0.0 self.shared.precision_type = 1 return LateralResult( apply_curvature=0.0, @@ -291,15 +283,12 @@ class StarPilotFordController: # Keep exit detection current so resume doesn't compare against a stale pre-turn value. self._desired_curvature_last = float(actuators.curvature) self.lane_center_trim.reset() - # A human turn ends any stall episode -- its own mode 0 does the PSCM reset job. That also - # covers the press so far: only press time accumulated AFTER the latch releases should earn - # a hand-off pulse. + # A human turn ends any prior recovery episode; its existing mode 0 supplies the reset. self.stall_blip_hold_s = 0.0 self.stall_blip_frames_left = 0 self.stall_blip_cooldown_s = 0.0 self.stall_blip_count = 0 self.angle_stall_blip_active = False - self.press_timer_s = 0.0 self.shared.precision_type = 1 return LateralResult( apply_curvature=0.0, @@ -311,19 +300,6 @@ class StarPilotFordController: lateralUncertainty=0.0, ) - # Proactive hand-off blip: the falling edge of a sustained press earns an immediate mode-0 - # pulse (see _PRESS_BLIP_MIN_S) -- resets the PSCM's press-induced attenuation right at - # hand-off, while the car is straight and the command small, instead of waiting for the - # reactive stall detector below to watch the car miss the next curve first. - if CS.out.steeringPressed: - self.press_timer_s += _STEER_DT - else: - if (self.press_timer_s >= _PRESS_BLIP_MIN_S and self.stall_blip_cooldown_s <= 0.0 - and self.stall_blip_frames_left <= 0 - and abs(self.shared.path_angle_last) < _BLIP_MAX_PATH_ANGLE): - self.stall_blip_frames_left = _STALL_BLIP_FRAMES - self.press_timer_s = 0.0 - # Stall-blip pulse in progress: hold lateral inactive (mode 0, all-zero signals -- the same # wire pattern as the human-turn override, no ford.h involvement) for _STALL_BLIP_FRAMES so the # PSCM drops its post-override attenuation, then release; path_angle ramps back in from zero diff --git a/openpilot/starpilot/tests/test_ford_starpilot_angle.py b/openpilot/starpilot/tests/test_ford_starpilot_angle.py index 6567c6a2ae..9c2a2c9957 100644 --- a/openpilot/starpilot/tests/test_ford_starpilot_angle.py +++ b/openpilot/starpilot/tests/test_ford_starpilot_angle.py @@ -301,3 +301,80 @@ def test_unselected_bluepilot_angle_packets_are_unchanged(): assert baseline.step_lateral(cc, cs, cc.actuators, 0.) == host.step_lateral(cc, cs, cc.actuators, 0.) if frame % 3 == 0: assert baseline.step_lka(cc, cc.hudControl) == host.step_lka(cc, cc.hudControl) + + +@pytest.mark.parametrize('speed', [3., 12., 30.]) +@pytest.mark.parametrize('pinion', [False, True]) +@pytest.mark.parametrize('curvature', [0., .0003]) +def test_ordinary_override_release_does_not_reset_steering(speed, pinion, curvature): + angle, cc, cs = controller(speed=speed, pinion=pinion) + set_curve(angle, cc, cs, curvature) + for tick in range(50): + cs.out.steeringPressed = 10 <= tick < 30 + result = angle.update_angle_strategy(cc, cs, cc.actuators) + assert not angle.angle_human_turn_active + assert not angle.angle_stall_blip_active + assert angle.stall_blip_frames_left == angle.stall_blip_count == 0 + assert math.isfinite(result.path_angle) + + +def test_natural_deviation_recovery_remains_bounded_and_native_admitted(): + from opendbc.can import CANPacker + from opendbc.car.ford.fordcan import CanBus + 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 + angle, cc, cs = controller(speed=12.) + cp, speed = angle.CP, 12. + packer = CANPacker('ford_lincoln_base_pt') + source = SimpleNamespace(controller_choice=FordController.STARPILOT, current_inputs_valid=True) + host = FordBluePilotController(cp, packer, CanBus(cp), source) + road = StraightRoadInputs(settings(STARTUP['on'], True)) + road.curvature = cc.actuators.curvature = .01 + safety = libsafety_py.libsafety + safety.init_tests() + safety.set_alternative_experience(0) + assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, int(cp.safetyConfigs[-1].safetyParam)) == 0 + safety.set_controls_allowed(True) + applied, modes, deviation_seen = 0., [], False + try: + for frame in range(200): + safety.set_timer(1_000_000 + frame * 10_000) + messages: list[tuple[str, dict[str, float]]] = [ + ('BrakeSysFeatures', {'Veh_V_ActlBrk': speed * 3.6, 'VehVActlBrk_D_Qf': 3, 'VehVActlBrk_No_Cnt': (frame // 2) % 16}), + ('EngVehicleSpThrottle2', {'Veh_V_ActlEng': speed * 3.6, 'VehVActlEng_D_Qf': 3}), + ('Yaw_Data_FD1', {'VehYaw_W_Actl': 0, 'VehYawWActl_D_Qf': 3, 'VehRollYaw_No_Cnt': frame % 256}), + ('DesiredTorqBrk', {'VehStop_D_Stat': 0}), + ('EngVehicleSpThrottle', {'ApedPos_Pc_ActlArb': 0}), + ('EngBrakeData', {'BpedDrvAppl_D_Actl': 1, 'CcStat_D_Actl': 5}), + ] + for name, values in messages: + if name in ('BrakeSysFeatures', 'EngVehicleSpThrottle2') and frame % 2: + continue + address, data, bus = packer.make_can_msg(name, 0, values) + assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)) + host.update(road.update(), frame) + cc.latActive = frame >= 15 + packets = [] + if frame % 5 == 0: + sends, applied = host.step_lateral(cc, cs, cc.actuators, applied) + packets.extend(sends) + deviation_seen |= host.shared.bp_curvature_deviation_limited + for address, data, _ in sends: + if address == 0x3D6 and cc.latActive: + mode = (data[0] >> 4) & 7 + modes.append(mode) + if mode == 0: + assert deviation_seen and host.angle.angle_stall_blip_active + assert host.angle.bp_path_angle_final == 0. + if frame % 3 == 0: + packets.append(host.step_lka(cc, cc.hudControl)) + for address, data, bus in packets: + assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (frame, hex(address), data.hex()) + zero_indices = [i for i, mode in enumerate(modes) if mode == 0] + assert len(zero_indices) == 6 + assert zero_indices == list(range(zero_indices[0], zero_indices[0] + 6)) + assert modes[-1] == 1 and host.angle.bp_path_angle_final > 0. + assert host.angle.stall_blip_count == 1 and host.angle.stall_blip_cooldown_s > 0. + finally: + safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) diff --git a/prebuilt.json b/prebuilt.json index 28958db121..78e1a1676b 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "e488c9771fc26f8554ae0c55d1e7e36258d411b2ac7dee375a9c5af942f7e532", "schema_version": 1} +{"protected_inventory_sha256": "b77124d8cb762c0142394084c7a9683df83856033a3d5890f4b26579e6103197", "schema_version": 1}