Keep StarPilot steering active after ordinary driver release

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

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)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "e488c9771fc26f8554ae0c55d1e7e36258d411b2ac7dee375a9c5af942f7e532", "schema_version": 1}
+{"protected_inventory_sha256": "b77124d8cb762c0142394084c7a9683df83856033a3d5890f4b26579e6103197", "schema_version": 1}