Restore Mach-E stopped-lead braking on rolling approaches

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -36,6 +36,8 @@ from openpilot.starpilot.longitudinal.follow_jerk import FollowJerk
 from openpilot.starpilot.longitudinal.lead_takeoff import LeadTakeoff, project as project_takeoff
 from openpilot.starpilot.longitudinal.lead_behavior import adjust as adjust_lead_behavior
 from openpilot.starpilot.longitudinal.hkg_vision_stop import owner_for as vision_stop_owner
+from openpilot.starpilot.car.ford.stopped_lead import owner_for as ford_stop_owner
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
 from openpilot.starpilot.longitudinal.lead_approach import LeadApproach, LeadApproachKey, project as project_approach
 from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy
 
@@ -132,6 +134,7 @@ class LongitudinalPlanner:
     self.lead_takeoff = LeadTakeoff()
     self.vehicle_far_follow = far_follow_owner(CP, dt)
     self.vision_stop_buffer = vision_stop_owner(CP)
+    self.ford_stop_approach = ford_stop_owner(CP, STOP_DISTANCE - .5)
     self.preap_adaptive = preap_adaptive_owner(CP)
 
   def update(self, sm, *, cruise_ceiling: CruiseCeiling | None = None,
@@ -474,6 +477,10 @@ class LongitudinalPlanner:
         active=profile_eligible and not reset_state and self.mpc.solution_status == 0)
     if preap_acceleration_cap is not None:
       output_a_target = min(output_a_target, preap_acceleration_cap)
+    if self.ford_stop_approach is not None:
+      output_a_target, self.output_should_stop = self.ford_stop_approach.sample(
+        sm, now_ns=sample_now_ns, drive_id=drive_id, target=float(output_a_target),
+        stopping=self.output_should_stop, accel_min=ACCEL_MIN, active=profile_eligible and not reset_state)
     self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
 
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0

openpilot/starpilot/car/ford/stopped_lead.py

@@ -0,0 +1,87 @@
+"""Dom's stopped-lead cap on a rolling Mach-E approach; no launch ownership."""
+import math
+
+from opendbc.car.ford.aol import qualified
+from opendbc.car.ford.values import CAR
+
+SOURCES = ('carState', 'radarState', 'modelV2', 'carControl', 'controlsState', 'selfdriveState')
+MAX_AGE_NS = 250_000_000
+MAX_EGO_SPEED = .5
+MAX_LEAD_SPEED = .45
+MAX_LEAD_DELTA = .5
+MIN_MODEL_PROB = .95
+MAX_LATERAL_OFFSET = 1.75
+MIN_DISTANCE = 3.
+DISTANCE_MARGIN = 3.
+MIN_BRAKE = .16
+MAX_BRAKE = .26
+
+
+def owner_for(cp, stop_distance):
+  try:
+    if (cp.brand == 'ford' and cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and
+        cp.openpilotLongitudinalControl and qualified(cp)):
+      return RollingStoppedLead(stop_distance)
+  except (AttributeError, TypeError, ValueError):
+    pass
+  return None
+
+
+class RollingStoppedLead:
+  def __init__(self, stop_distance):
+    self.stop_distance = stop_distance
+    self.reset()
+
+  def reset(self):
+    self.drive_id = 0
+    self.rolling = False
+
+  def cap(self, lead, speed, accel_min):
+    if not lead.present or not lead.radar and lead.modelProb < MIN_MODEL_PROB or abs(lead.yRel) > MAX_LATERAL_OFFSET:
+      return None
+    lead_speed = max(float(lead.vLead), 0.)
+    delta = lead_speed - speed
+    distance = max(MIN_DISTANCE, self.stop_distance + DISTANCE_MARGIN)
+    if speed > MAX_EGO_SPEED or lead.dRel > distance or lead_speed > MAX_LEAD_SPEED or delta > MAX_LEAD_DELTA:
+      return None
+    distance_factor = min(max((distance - lead.dRel) / max(distance - MIN_DISTANCE, .1), 0.), 1.)
+    speed_factor = min(max(lead_speed / max(MAX_LEAD_SPEED, .1), 0.), 1.)
+    delta_factor = min(max((MAX_LEAD_DELTA - delta) / max(MAX_LEAD_DELTA, .1), 0.), 1.)
+    brake = min(max(MIN_BRAKE + .06 * distance_factor + .02 * speed_factor + .02 * delta_factor, MIN_BRAKE), MAX_BRAKE)
+    return -brake if accel_min >= 0. else max(accel_min, -brake)
+
+  def sample(self, sm, *, now_ns, drive_id, target, stopping, accel_min, active):
+    try:
+      car = sm['carState']
+      if (type(now_ns) is not int or type(drive_id) is not int or not 0 < drive_id < now_ns or not active or
+          not all(sm.valid[name] and sm.alive[name] and drive_id < sm.logMonoTime[name] <= now_ns and
+                  now_ns - sm.logMonoTime[name] <= MAX_AGE_NS for name in SOURCES) or
+          not car.canValid or car.canTimeout or car.accFaulted or car.gasPressed or car.brakePressed or
+          str(car.gearShifter) not in ('drive', 'low', 'sport') or
+          not sm['carControl'].longActive or not sm['selfdriveState'].enabled or sm['controlsState'].forceDecel):
+        self.reset()
+        return target, stopping
+      if self.drive_id != drive_id:
+        self.reset()
+        self.drive_id = drive_id
+      speed = float(car.vEgo)
+      leads = (sm['radarState'].leadOne, sm['radarState'].leadTwo)
+      if (not all(math.isfinite(float(v)) for v in (speed, target, accel_min, self.stop_distance)) or
+          speed < 0. or self.stop_distance < 0. or accel_min > 0. or any(
+            not all(math.isfinite(float(getattr(lead, field))) for field in ('dRel', 'vLead', 'yRel', 'modelProb')) or
+            lead.dRel < 0. or not 0. <= lead.modelProb <= 1. for lead in leads if lead.present)):
+        self.reset()
+        return target, stopping
+      if car.standstill:
+        self.rolling = False
+      elif speed > MAX_EGO_SPEED:
+        self.rolling = True
+      # The original stopped-car departure owner remains responsible after an
+      # actual standstill. Missing history cannot invent a new rolling approach.
+      if not self.rolling or speed > MAX_EGO_SPEED:
+        return target, stopping
+      caps = [value for lead in leads if (value := self.cap(lead, speed, accel_min)) is not None]
+      return (min(target, min(caps)), True) if caps else (target, stopping)
+    except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
+      self.reset()
+      return target, stopping

openpilot/starpilot/tests/test_ford_configuration_parity.py

@@ -12,6 +12,8 @@ def configuration_topology(monkeypatch, topology, request):
   if request.node.name.startswith(('test_brake_retains_lateral_with_native_first_feedback', 'test_cancel_retains_lateral_with_native_first_feedback')):
     monkeypatch.ford_native_10hz = True
     monkeypatch.ford_angle = request.node.callspec.params['angle']
+  if request.node.name.startswith('test_recorded_radar_creep_enters_native_stopping'):
+    monkeypatch.ford_angle = request.node.callspec.params['angle']
   if request.node.name.startswith('test_route49_first_main_engagement'):
     monkeypatch.ford_first_main = True
     monkeypatch.ford_angle = request.node.callspec.params['angle']
@@ -50,3 +52,67 @@ def test_brake_retains_lateral_with_native_first_feedback(ford_joined, monkeypat
 @pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
 def test_cancel_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch, angle):
   run_actual_producer(ford_joined, monkeypatch, scenario='cancel-native-cadence')
+
+
+@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
+@pytest.mark.parametrize('alpha', [True])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
+@pytest.mark.parametrize('angle', [False, True])
+def test_recorded_radar_creep_enters_native_stopping(ford_joined, angle):
+  import json
+  import gzip
+  from pathlib import Path
+  from opendbc.car import structs
+  from opendbc.car.ford import fordcan
+  from opendbc.safety.tests.libsafety import libsafety_py
+  from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+  from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+  from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import Frame, DRIVE_NS
+
+  recorded = json.loads(gzip.decompress((Path(__file__).parent / 'testdata/ford_mache_radar_creep.json.gz').read_bytes()))['frames']
+  joined = ford_joined
+  endpoint, safety = joined.native, joined.safety
+  endpoint.arm()
+  endpoint.pump(10, main=5, mask=3)
+  assert safety.aol_get_permission_mask() == 3
+  planner, controller = LongitudinalPlanner(joined.host.CP, init_v=recorded[0]['vEgo']), LongControl(joined.host.CP)
+  checked = 0
+  for index, sample in enumerate(recorded):
+    sm = Frame(index)
+    # Retain the route's actual cross-service age and validity, including radar cadence.
+    for service, age in sample['sourceAgeNs'].items():
+      sm.logMonoTime[service] = sm.now_ns - 10_000_000 - age
+      sm.valid[service] = sample['sourceValid'][service]
+    car = sm['carState']
+    car.vEgo, car.vEgoRaw, car.aEgo = sample['vEgo'], sample['vEgoRaw'], sample['aEgo']
+    car.standstill, car.gasPressed, car.brakePressed = sample['standstill'], sample['gas'], sample['brake']
+    car.gearShifter = 'drive'
+    car.cruiseState.available = car.cruiseState.enabled = True
+    sm['carControl'].longActive = sample['longActive']
+    sm['selfdriveState'].enabled = sample['enabled']
+    sm['selfdriveState'].experimentalMode = sample['experimental']
+    sm['controlsState'].longControlState = controller.long_control_state
+    sm['modelV2'].action.shouldStop = sample['shouldStop']
+    sm['modelV2'].action.desiredAcceleration = sample['aTarget']
+    for key, values in sample['model'].items():
+      getattr(sm['modelV2'], key).x = values
+    for lead, values in zip((sm['radarState'].leadOne, sm['radarState'].leadTwo), sample['leads'], strict=True):
+      for key, value in values.items():
+        setattr(lead, key, value)
+    planner.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+    assert planner.mpc.solution_status == 0
+    for _ in range(5):
+      output = controller.update(sample['longActive'], car, float(planner.output_a_target), planner.output_should_stop, (-3.5, 2.))
+      endpoint.pump(1, main=5, mask=3, speed=sample['vEgoRaw'])
+      stopping = controller.long_control_state == structs.CarControl.Actuators.LongControlState.stopping
+      active = sample['longActive']
+      gas = -5. if output < 0. or not active else float(output)
+      address, data, bus = fordcan.create_acc_msg(endpoint.packer, joined.host.CI.CC.CAN, active, gas,
+        float(output) if active else 0., stopping if active else False, bool(output < 0. and active), v_ego_kph=80.)
+      assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data))
+      if 821. <= sample['routeSeconds'] <= 828.:
+        assert planner.output_should_stop, ('Creeping toward stopped radar lead', sample['routeSeconds'])
+        assert stopping and output < 0.
+        checked += 1
+  assert checked > 500

openpilot/starpilot/tests/test_ford_release_long_preferences.py

@@ -60,3 +60,114 @@ def test_release_never_promotes_unowned_configuration(restriction, tmp_path):
   elif restriction == 'foreign-brand':
     cp.brand = 'hyundai'
   assert prepare_release_long(cp, saved(tmp_path), is_release=True, enabled=True) is cp
+
+
+class StopApproachFrame(dict):
+  def __init__(self):
+    from openpilot.starpilot.car.ford.stopped_lead import SOURCES
+    super().__init__(carState=SimpleNamespace(vEgo=.8, canValid=True, canTimeout=False, accFaulted=False,
+      gasPressed=False, brakePressed=False, gearShifter='drive', standstill=False),
+      radarState=SimpleNamespace(leadOne=SimpleNamespace(present=True, radar=True, modelProb=.999,
+        dRel=5.5, vLead=0., yRel=0.), leadTwo=SimpleNamespace(present=False)),
+      modelV2=SimpleNamespace(), carControl=SimpleNamespace(longActive=True),
+      controlsState=SimpleNamespace(forceDecel=False), selfdriveState=SimpleNamespace(enabled=True))
+    self.valid = dict.fromkeys(SOURCES, True)
+    self.alive = dict.fromkeys(SOURCES, True)
+    self.logMonoTime = dict.fromkeys(SOURCES, 1_000_000_000)
+
+
+def stopped_sample(owner, sm, **changes):
+  args = {'now_ns': 1_000_000_000, 'drive_id': 1, 'target': -.05, 'stopping': False, 'accel_min': -3.5, 'active': True}
+  args.update(changes)
+  return owner.sample(sm, **args)
+
+
+def primed_stop_approach():
+  from openpilot.starpilot.car.ford.stopped_lead import RollingStoppedLead
+  owner, sm = RollingStoppedLead(5.5), StopApproachFrame()
+  assert stopped_sample(owner, sm) == (-.05, False)
+  sm['carState'].vEgo = .4
+  return owner, sm
+
+
+def test_original_dom_rolling_stop_cap_and_stronger_braking_are_retained():
+  owner, sm = primed_stop_approach()
+  target, stopping = stopped_sample(owner, sm)
+  # Dom's original6m STOP_DISTANCE-.5 +3m margin, with its unchanged brake terms.
+  assert target == pytest.approx(-.21272727272727274) and stopping
+  assert stopped_sample(owner, sm, target=-3.5, stopping=True) == (-3.5, True)
+
+
+@pytest.mark.parametrize('defect', ['gas', 'brake', 'stale', 'future', 'invalid', 'off', 'disabled', 'force',
+                                   'inactive', 'can', 'gear', 'nan', 'drive'])
+def test_stop_approach_cannot_override_driver_invalid_source_or_new_drive(defect):
+  owner, sm = primed_stop_approach()
+  changes = {}
+  if defect in ('gas', 'brake'):
+    setattr(sm['carState'], defect + 'Pressed', True)
+  elif defect == 'stale':
+    sm.logMonoTime['radarState'] -= 250_000_001
+  elif defect == 'future':
+    sm.logMonoTime['radarState'] += 1
+  elif defect == 'invalid':
+    sm.valid['radarState'] = False
+  elif defect == 'off':
+    sm['carControl'].longActive = False
+  elif defect == 'disabled':
+    sm['selfdriveState'].enabled = False
+  elif defect == 'force':
+    sm['controlsState'].forceDecel = True
+  elif defect == 'inactive':
+    changes['active'] = False
+  elif defect == 'can':
+    sm['carState'].canValid = False
+  elif defect == 'gear':
+    sm['carState'].gearShifter = 'reverse'
+  elif defect == 'nan':
+    sm['radarState'].leadOne.dRel = float('nan')
+  elif defect == 'drive':
+    changes['drive_id'] = 2
+  assert stopped_sample(owner, sm, **changes) == (-.05, False)
+  assert not owner.rolling
+
+
+@pytest.mark.parametrize('defect', ['moving', 'far', 'offcenter', 'uncertain', 'absent'])
+def test_stop_approach_keeps_normal_moving_and_unqualified_leads_unchanged(defect):
+  owner, sm = primed_stop_approach()
+  lead = sm['radarState'].leadOne
+  if defect == 'moving':
+    lead.vLead = .450001
+  elif defect == 'far':
+    lead.dRel = 8.500001
+  elif defect == 'offcenter':
+    lead.yRel = 1.750001
+  elif defect == 'uncertain':
+    lead.radar, lead.modelProb = False, .949999
+  else:
+    lead.present = False
+  assert stopped_sample(owner, sm) == (-.05, False)
+
+
+def test_actual_standstill_and_normal_departure_keep_existing_release_owner():
+  owner, sm = primed_stop_approach()
+  assert stopped_sample(owner, sm)[1]
+  sm['carState'].standstill, sm['carState'].vEgo = True, 0.
+  assert stopped_sample(owner, sm, stopping=True) == (-.05, True)
+  sm['carState'].standstill, sm['carState'].vEgo = False, .2
+  sm['radarState'].leadOne.vLead = .2
+  assert stopped_sample(owner, sm, target=.3) == (.3, False)
+  owner.reset()
+  assert stopped_sample(owner, sm, target=.3) == (.3, False)
+
+
+def test_stopped_lead_owner_requires_existing_mache_open_long_configuration():
+  from openpilot.starpilot.car.ford.stopped_lead import owner_for
+  fp = gen_empty_fingerprint()
+  fp[0][0x5A] = 8
+  cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fp, [], True, False, False)
+  assert owner_for(cp, 5.5) is not None
+  for field, value in (('openpilotLongitudinalControl', False), ('passive', True), ('dashcamOnly', True),
+                       ('notCar', True), ('brand', 'hyundai'), ('carFingerprint', CAR.FORD_F_150_MK14)):
+    changed = cp.as_reader().as_builder()
+    setattr(changed, field, value)
+    assert owner_for(changed, 5.5) is None

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "e65ec8f6ee29277a9b06f2a9328e812bf90faadf99653b8c02843078f84f89d1", "schema_version": 1}
+{"protected_inventory_sha256": "2d276acabec2ab651f95c7ce00dc886c481d85cb130f2a851b0f7e8068d8192d", "schema_version": 1}