Retain established Ford model stops through stationary noise

openpilot/starpilot/conditional_mode/projection.py

@@ -11,6 +11,7 @@ import re
 import time
 
 from openpilot.cereal.services import SERVICE_LIST
+from opendbc.car import structs
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
@@ -21,7 +22,7 @@ from openpilot.starpilot.conditional_mode.policy import Authority, LeadEvidence,
 from openpilot.starpilot.conditional_mode.preferences import CEMOptions
 from openpilot.starpilot.conditional_mode.signal_lane import MonoSource, SignalLaneFrame, SignalLaneTracker
 from openpilot.starpilot.conditional_mode.slower_lead import SlowerLeadDetector, SlowerLeadFrame
-from openpilot.starpilot.conditional_mode.stop import StopFrame, StopLead, StopLightDetector, StopObservation
+from openpilot.starpilot.conditional_mode.stop import FORD_STOP_NAMES, StopFrame, StopLead, StopLightDetector, StopObservation
 from openpilot.starpilot.conditional_mode.turn_scene import committed_turn_scene
 from openpilot.starpilot.model_geometry import (
   STANDSTILL_MODEL_PREFIX_NOISE_M as STANDSTILL_MODEL_PREFIX_NOISE_M, STANDSTILL_SPEED_NOISE_MPS, normalized_origin,
@@ -460,7 +461,10 @@ class SceneProjector:
           # projecting forward. Use that endpoint only to clear an existing stop;
           # Curve geometry and cold stop acquisition retain strict validation.
           stop_horizon = _standstill_clear_horizon(model)
-        current_stop = model_should_stop if stop_horizon is not None else None
+        # A fresh discrete stop bit can corroborate an existing Ford stopped
+        # hold even when geometry is unavailable. Geometry admission stays strict.
+        current_stop = model_should_stop if (stop_horizon is not None or
+          _field(cp, 'carFingerprint') in FORD_STOP_NAMES and _field(car, 'standstill') is True) else None
     lead = _lead(radar) if radar is not None else None
     car_valid = speed is not None
     headway_fresh = (
@@ -626,6 +630,8 @@ class SceneProjector:
       model_tick_mono_s=model_stamp / 1e9 if model_stamp is not None else None,
       model_should_stop=current_stop,
       stationary_launch_clear=stationary_launch_clear,
+      longitudinal_stop_active=bool(_field(control, 'longActive') is True and _field(selfdrive, 'enabled') is True and
+        _field(controls, 'longControlState') == structs.CarControl.Actuators.LongControlState.stopping),
       standstill_path_extent_m=(_standstill_path_extent(model) if model_transport_current and car_valid and
                                 _field(car, 'standstill') is True else None),
     )

openpilot/starpilot/conditional_mode/stop.py

@@ -10,12 +10,15 @@ from dataclasses import dataclass
 import math
 
 import numpy as np
+from opendbc.car.ford.values import CAR as FORD_CAR
 
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.conditional_mode.turn_scene import committed_turn_scene
 
 
+FORD_STOP_NAMES = frozenset(str(car) for car in FORD_CAR)
+
 THRESHOLD = 1.0 - 1.0 / math.e
 MAX_SOURCE_AGE_S = 0.25
 MAX_FRAME_GAP_S = MAX_SOURCE_AGE_S
@@ -58,6 +61,7 @@ class StopFrame:
   model_should_stop: bool | None = None
   stationary_launch_clear: bool = False
   standstill_path_extent_m: float | None = None
+  longitudinal_stop_active: bool = False
 
 
 @dataclass(frozen=True)
@@ -152,6 +156,16 @@ class StopLightDetector:
       else:
         self.commit_clear_since_s = None
 
+  def _commit_ford_standstill(self, frame: StopFrame, extent: float) -> None:
+    # Commit only an already established, geometry-qualified stopped hold.
+    # The model action corroborates that hold; it cannot acquire a light alone.
+    if (frame.car_fingerprint in FORD_STOP_NAMES and frame.standstill is True and
+        frame.pedal_override is False and frame.model_should_stop is True and
+        frame.longitudinal_stop_active is True and
+        self.standstill_reason == 'light' and self.standstill_model_stopped and
+        extent < FROZEN_RAW_STOP_DISTANCE_M):
+      self._commit(frame, extent, True, True, 0.0)
+
   def _standstill(self, frame: StopFrame, model_stopped: bool) -> tuple[bool | None, str | None]:
     if not frame.standstill:
       self.standstill_reason = None
@@ -244,6 +258,7 @@ class StopLightDetector:
         return StopObservation(True, hold, reason, None)
       extent = _finite(frame.standstill_path_extent_m, 0.0, 500.0)
       if horizon is None and frame.standstill is True and frame.pedal_override is False and extent is not None:
+        self._commit_ford_standstill(frame, extent)
         release_distance = FROZEN_RAW_STOP_DISTANCE_M + (STOP_MODEL_RELEASE_MARGIN_M if self.standstill_model_stopped else 0.0)
         self.standstill_model_stopped = extent < release_distance
         hold, reason = self._standstill(frame, self.standstill_model_stopped or forcing_stop)
@@ -262,6 +277,7 @@ class StopLightDetector:
       self.standstill_model_stopped = horizon < release_distance
     else:
       self.standstill_model_stopped = False
+    self._commit_ford_standstill(frame, horizon)
     model_stopped = (self.standstill_model_stopped if frame.standstill else horizon < FROZEN_RAW_STOP_DISTANCE_M) or forcing_stop
     if frame.stop_sign_confirmed:
       self.light_filter.x = 1.0

openpilot/starpilot/conditional_mode/tests/test_stop_commit.py

@@ -80,3 +80,40 @@ def test_standstill_commit_survives_unknowns_and_requires_continuous_clear_evide
   for tick in range(79, 86):
     detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=False))
   assert not detector.committed and not detector.standstill_committed
+
+
+def test_ford_stopped_commit_needs_prior_bounded_geometry_and_current_stop_bit():
+  from opendbc.car.ford.values import CAR
+  for name in CAR:
+    detector = StopLightDetector()
+    held = replace(frame(0, speed=0., standstill=True, horizon=None, model_should_stop=True),
+                   car_fingerprint=str(name), standstill_path_extent_m=.1, longitudinal_stop_active=True)
+    detector.step(held)
+    assert not detector.committed
+    detector.step(replace(held, observed_mono_s=100.05, now_mono_s=100.055))
+    assert detector.committed
+    missing = replace(held, observed_mono_s=100.1, now_mono_s=100.105, standstill_path_extent_m=None)
+    assert detector.step(missing).standstill_hold
+    assert not detector.step(replace(missing, observed_mono_s=100.15, now_mono_s=100.155,
+                                     pedal_override=True)).standstill_hold
+    assert not detector.committed
+  for defect in ('unknown', 'invalid', 'no_stop', 'moving', 'stale', 'long_off'):
+    detector = StopLightDetector()
+    current = replace(frame(0, speed=0., standstill=True, horizon=None, model_should_stop=True),
+                      car_fingerprint=str(CAR.FORD_MUSTANG_MACH_E_MK1), standstill_path_extent_m=.1, longitudinal_stop_active=True)
+    if defect == 'unknown':
+      current = replace(current, car_fingerprint='FORD_UNKNOWN')
+    elif defect == 'invalid':
+      current = replace(current, standstill_path_extent_m=None)
+    elif defect == 'no_stop':
+      current = replace(current, model_should_stop=False)
+    elif defect == 'moving':
+      current = replace(current, standstill=False)
+    elif defect == 'stale':
+      current = replace(current, observed_mono_s=99.)
+    elif defect == 'long_off':
+      current = replace(current, longitudinal_stop_active=False)
+    detector.step(current)
+    detector.step(replace(current, now_mono_s=current.now_mono_s+.05,
+                          observed_mono_s=current.observed_mono_s+.05))
+    assert not detector.committed, defect

openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py

@@ -389,3 +389,106 @@ class TestStopCommitTransport(unittest.TestCase):
     self.assertTrue(result.accepted and result.experimental)
     self.sample(traffic_verdict=verdict, traffic_owner_present=True, cp=cp, unavailable='carControl')
     self.assertTrue(self.host.projector.stop_detector.committed)
+
+
+def test_recorded_ford_stopped_geometry_does_not_release_native_stop(request):
+  from pathlib import Path
+  from opendbc.car import gen_empty_fingerprint, structs
+  from opendbc.car.ford.interface import CarInterface
+  from opendbc.car.ford.values import CAR
+  from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+  from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+  from openpilot.starpilot.conditional_mode.policy import ConditionalModePolicy
+  from openpilot.starpilot.conditional_mode.projection import SceneProjector
+  from openpilot.starpilot.conditional_mode.tests.test_projection import owner_context
+  from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import Frame, DRIVE_NS
+
+  recorded = json.loads((Path(__file__).parent / 'testdata/ford_stationary_stop.json').read_text())['frames']
+  fingerprint = gen_empty_fingerprint()
+  fingerprint[0][0x5A] = 8
+  cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint, [], True, False, False)
+  assert cp.openpilotLongitudinalControl
+  from opendbc.car.ford import fordcan
+  from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent
+  from opendbc.safety.tests.libsafety import libsafety_py
+  endpoint = TestFordAolDriverIntent(methodName='runTest')
+  endpoint.setUp()
+  request.addfinalizer(endpoint.tearDown)
+  endpoint.reset(19)
+  endpoint.pump(40, main=0)
+  endpoint.pump(10, main=3)
+  endpoint.pump(10, main=5, mask=3)
+  assert endpoint.safety.aol_get_permission_mask() == 3
+  from opendbc.can import CANParser
+  packet_parser = CANParser('ford_lincoln_base_pt', [('ACCDATA', 50)], 0)
+  bus = fordcan.CanBus(fingerprint=fingerprint)
+  projector, policy = SceneProjector(), ConditionalModePolicy()
+  planner, controller = LongitudinalPlanner(cp), LongControl(cp)
+  checked = 0
+  for index, sample in enumerate(recorded):
+    stamp = MONO + index * 50_000_000
+    boot = BOOT + index * 50_000_000
+    payloads = serialized_scene(stamp=boot, speed=sample['vEgo'], standstill=sample['standstill'],
+                                lead_present=False, experimental_mode=True)
+    car = messaging.new_message('carState')
+    car.carState = payloads['carState']
+    car.carState.vEgoRaw = sample['vEgoRaw']
+    car.carState.cruiseState.available = car.carState.cruiseState.enabled = True
+    payloads['carState'] = messaging.log_from_bytes(car.to_bytes()).carState
+    command = messaging.new_message('carControl')
+    command.carControl = payloads['carControl']
+    command.carControl.longActive = sample['longActive']
+    command.carControl.enabled = sample['enabled']
+    payloads['carControl'] = messaging.log_from_bytes(command.to_bytes()).carControl
+    controls = messaging.new_message('controlsState')
+    controls.controlsState = payloads['controlsState']
+    controls.controlsState.longControlState = sample['longControlState']
+    payloads['controlsState'] = messaging.log_from_bytes(controls.to_bytes()).controlsState
+    model = messaging.new_message('modelV2')
+    model.modelV2 = payloads['modelV2']
+    model.modelV2.position.x = sample['x']
+    model.modelV2.action.shouldStop = sample['shouldStop']
+    model.modelV2.action.desiredAcceleration = sample['aTarget']
+    payloads['modelV2'] = messaging.log_from_bytes(model.to_bytes()).modelV2
+    sm = FakeSubMaster(payloads, stamp)
+    projected = projector.project(sm, cp, now_mono_ns=stamp+1_000_000, now_boot_ns=boot+1_000_000,
+      sample_skew_ns=1000, safe_mode=False, owner_context=owner_context(stamp))
+    if projected.authority is None:
+      continue
+    decision = policy.step((stamp+1_000_000)/1e9, ModeChoice.CEM, ManualIntent.NONE,
+                           projected.authority, projected.scene)
+    native = Frame(index, approach=False)
+    native['carState'].vEgo = max(sample['vEgo'], 0.)
+    native['carState'].standstill = sample['standstill']
+    native['carState'].cruiseState.available = native['carState'].cruiseState.enabled = True
+    native['carControl'].longActive = True
+    native['selfdriveState'].enabled = True
+    native['selfdriveState'].experimentalMode = decision.requested_experimental
+    native['modelV2'].action.shouldStop = sample['shouldStop']
+    native['modelV2'].action.desiredAcceleration = sample['aTarget']
+    for lead in (native['radarState'].leadOne, native['radarState'].leadTwo):
+      lead.present = False
+    if index == 1:
+      for _ in range(100):
+        controller.update(True, native['carState'], -.1, True, (-3.5, 2.))
+    native['controlsState'].longControlState = controller.long_control_state
+    planner.update(native, now_ns=native.now_ns, drive_id=DRIVE_NS)
+    assert planner.mpc.solution_status == 0
+    for _ in range(5):
+      output = controller.update(True, native['carState'], float(planner.output_a_target),
+                                 planner.output_should_stop, (-3.5, 2.))
+      assert -3.5 <= output <= 2.
+      endpoint.pump(1, main=5, mask=3, speed=max(sample['vEgoRaw'], 0.))
+      stopping = controller.long_control_state == structs.CarControl.Actuators.LongControlState.stopping
+      # Pack the actual LongControl result and keep the original native TX guard.
+      address, data, source = fordcan.create_acc_msg(endpoint.packer, bus, True, -5. if output < 0. else output,
+        float(output), stopping, output < 0., v_ego_kph=80.)
+      assert endpoint.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, source, data))
+      packet_parser.update([(native.now_ns, [(address, data, source)])])
+      if sample['routeSeconds'] >= 596.:
+        assert decision.requested_experimental, ('Lost accepted Ford stop', sample['routeSeconds'])
+        assert planner.output_should_stop
+        assert packet_parser.vl['ACCDATA']['AccStopStat_B_Rq'] == 1
+        assert output <= 0., ('Released brake during model stop', sample['routeSeconds'])
+        checked += 1
+  assert checked > 200

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "4fbaf31de9ce97de61391bd3154386c5a99b98292a8eba48f596e2a32e409bf6", "schema_version": 1}
+{"protected_inventory_sha256": "bb1dc1ce8c1af7372da449f43e9f4a862d884cfb648387040351b3ed4b39d4f4", "schema_version": 1}