Keep Ford angle AOL within native steering limits through turns and pedals

opendbc_repo/opendbc/bluepilot_lateral/core/angle.py

@@ -38,6 +38,8 @@ class Inputs:
   lane_direction: int = 0
   limit_status: int = 0
   geometry: LaneGeometry | None = None
+  speed_min: float | None = None
+  speed_max: float | None = None
 
 
 @dataclass(frozen=True)
@@ -85,6 +87,12 @@ class AngleController:
     if not i.active or not all(math.isfinite(v) for v in values) or i.speed < 0.0:
       self.reset()
       return Command()
+    speed_min = max(i.speed, 0.1) if i.speed_min is None else i.speed_min
+    speed_max = max(i.speed, 0.1) if i.speed_max is None else i.speed_max
+    if (not all(math.isfinite(v) for v in (speed_min, speed_max)) or
+        not 0 < speed_min <= max(i.speed, 0.1) <= speed_max):
+      self.reset()
+      return Command()
     if i.steering_pressed and not self.press_last:
       self.preturned = abs(i.steering_angle) > 45.0
     self.press_last = i.steering_pressed
@@ -152,7 +160,7 @@ class AngleController:
     # measured-curvature interval and the modern ISO envelope. If those bands
     # cannot meet the previous actuator ROC, send a neutral mode-0 frame.
     effective_speed = max(i.speed, 0.1)
-    cap = min(bounds.CURVATURE_MAX, bounds.LATERAL_ACCEL / max(i.speed, 1.0) ** 2)
+    cap = min(bounds.CURVATURE_MAX, bounds.LATERAL_ACCEL / max(speed_max, 1.0) ** 2)
     low_kappa, high_kappa = -cap, cap
     if i.speed > 9.0:
       low_kappa = max(low_kappa, i.measured_curvature - 0.0018)
@@ -161,8 +169,10 @@ class AngleController:
     # gain (0.95..1.3), so the worst equivalent curvature remains bounded.
     lower_gain = bounds.GAIN_MAX if low_kappa >= 0.0 else bounds.GAIN_MIN
     upper_gain = bounds.GAIN_MIN if high_kappa >= 0.0 else bounds.GAIN_MAX
-    lower = max(-bounds.PATH_MAX, low_kappa * effective_speed * lower_gain, self.path_last - roc)
-    upper = min(-bounds.PATH_MIN, high_kappa * effective_speed * upper_gain, self.path_last + roc)
+    lower_speed = speed_max if low_kappa >= 0.0 else speed_min
+    upper_speed = speed_min if high_kappa >= 0.0 else speed_max
+    lower = max(-bounds.PATH_MAX, low_kappa * lower_speed * lower_gain, self.path_last - roc)
+    upper = min(-bounds.PATH_MIN, high_kappa * upper_speed * upper_gain, self.path_last + roc)
     # Bound the actual-angle equivalent derivative at the worst admitted gain.
     # 45ms core margin remains inside the native elapsed-time/50ms cap.
     jerk_delta = bounds.LATERAL_JERK / max(i.speed - 1.0, 1.0) ** 2 * bounds.CORE_STEER_PERIOD_S * bounds.GAIN_MIN * effective_speed

opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py

@@ -88,6 +88,7 @@ class Host:
     self.cp = cp
     self.controller = AngleController(low_gain=low, high_gain=high)
     self.inputs = None
+    self.tx_blocked = None
 
   def update(self, CC, CS):
     if self.inputs is not None:
@@ -102,6 +103,25 @@ class Host:
              (not aol or CS.out.gearShifter in (structs.CarState.GearShifter.drive,
                                               structs.CarState.GearShifter.sport, structs.CarState.GearShifter.low)) and
              not CS.out.steerFaultTemporary and not CS.out.steerFaultPermanent and not CS.out.vehicleSensorsInvalid)
+    # Rejected frames do not advance Panda's accepted counter/path history.
+    # One real blocked-TX increment requires an accepted neutral resynchronization,
+    # rather than sending an indefinitely rejected chain of active counters.
+    counter_reader = getattr(self.inputs, "native_tx_blocked", None)
+    blocked = counter_reader() if ready and callable(counter_reader) else None
+    if not ready:
+      self.tx_blocked = None
+    elif type(blocked) is int and 0 <= blocked <= 0xFFFFFFFF:
+      changed = self.tx_blocked is not None and blocked != self.tx_blocked
+      self.tx_blocked = blocked
+      ready = not changed
+    speed_min = speed_max = None
+    speed_window = getattr(CS, "bluepilot_speed_window", None)
+    if speed_window is not None:
+      low, high, step = speed_window
+      # Allow source progress across the existing 100 ms CAN freshness window.
+      margin = max(0.05, 5 * step)
+      speed_min = max(0.1, min(low, CS.out.vEgoRaw) - margin)
+      speed_max = max(high, CS.out.vEgoRaw, 0.1) + margin
     meta = model.meta if model is not None else None
     return self.controller.step(Inputs(
       active=ready, speed=CS.out.vEgoRaw, measured_curvature=-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1),
@@ -111,4 +131,4 @@ class Host:
       lane_change=int(getattr(meta.laneChangeState, "raw", meta.laneChangeState)) in (1, 2, 3) if meta else False,
       lane_direction=int(getattr(meta.laneChangeDirection, "raw", meta.laneChangeDirection)) if meta else 0,
       limit_status=getattr(CS, "lat_ctl_lim_stat", 0),
-      geometry=model_geometry(model, fresh=snapshot is not None)))
+      geometry=model_geometry(model, fresh=snapshot is not None), speed_min=speed_min, speed_max=speed_max))

opendbc_repo/opendbc/car/ford/carstate.py

@@ -40,6 +40,8 @@ class CarState(CarStateBase):
     self.lkas_available = False
     self.lkas_available_ts_nanos = 0
     self.lateral_motion_control = None
+    self.bluepilot_speed_samples = [0] * 6 if CP.safetyConfigs and CP.safetyConfigs[-1].safetyParam & 128 else None
+    self.bluepilot_speed_window = None
 
   def get_car_gps(self):
     return self.car_gps_tracker.get()
@@ -63,6 +65,14 @@ class CarState(CarStateBase):
 
     # car speed
     ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
+    if self.bluepilot_speed_samples is not None:
+      # Match Panda's six raw ABS speed samples, including its millimetre/second rounding.
+      for value in cp.vl_all["BrakeSysFeatures"]["Veh_V_ActlBrk"]:
+        self.bluepilot_speed_samples.append(math.floor(value * CV.KPH_TO_MS * 1000 + .5))
+      self.bluepilot_speed_samples = self.bluepilot_speed_samples[-6:]
+      samples = self.bluepilot_speed_samples
+      step = max(abs(b - a) for a, b in zip(samples, samples[1:], strict=False)) / 1000
+      self.bluepilot_speed_window = (min(samples) / 1000, max(samples) / 1000, step)
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
     ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -1762,8 +1762,9 @@ class SelfdriveD:
     # Ioniq6 also checks the physical brake before its separately published host sample.
     # Retain only its already granted LAT; all proof, health and LONG withdrawal gates remain.
     # The exact vehicle policies and all proof/health checks above remain required.
+    from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported as ford_early_long
     return bool(gm_native_accepts_cp(self.CP, native.safetyModel, native.safetyParam) or
-                early_native_long_withdrawal_supported(self.CP) or
+                early_native_long_withdrawal_supported(self.CP) or ford_early_long(self.CP) or
                 toyota_aol_qualified(self.CP, marked_only=True) or
                 not native.pandaInventory[owners[0]].controlsAllowed and
                 not self.sm['pandaStates'][owners[0]].controlsAllowed)

openpilot/starpilot/aol/transport_pause.py

@@ -320,7 +320,8 @@ class TransportPauseFeedback:
     from openpilot.starpilot.car.hyundai.aol import qualified_ccnc_long_aol as ev9_qualified
     from opendbc.car import structs
     ev9 = cp.brand == 'hyundai' and ev9_qualified(cp, marked_only=True)
-    early_long = gm or cp.brand == 'hyundai' and early_native_long_withdrawal_supported(cp) or ev9
+    from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported as ford_early_long
+    early_long = gm or cp.brand == 'hyundai' and early_native_long_withdrawal_supported(cp) or ev9 or ford_early_long(cp)
     from openpilot.starpilot.car.gm.aol import f1_conventional_lateral_owner, gm_physical_pedal_withdrawal
     gm_pedal = gm_physical_pedal_withdrawal(cp, cs) and not f1_conventional_lateral_owner(cp)
     event_ns = int(sm.logMonoTime['onroadEvents'])

openpilot/starpilot/car/ford/aol.py

@@ -23,6 +23,13 @@ def native_accepts_cp(cp, model, param):
   return qualified(cp, marked_only=True) and native_profile_supported(model, param) and param == aol_safety_config(cp).safetyParam
 
 
+def early_native_long_withdrawal_supported(cp):
+  # Panda can observe a pedal before the parsed host sample. The existing
+  # independent-LAT proof remains mandatory; this never authorizes LONG.
+  return bool(qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl and
+              aol_safety_config(cp).safetyParam & ANGLE)
+
+
 def native_latch_rejected(cp, native):
   return qualified(cp, marked_only=True) and native.requestedLateral and not native.lateralAllowed
 

openpilot/starpilot/controller_extensions.py

@@ -123,6 +123,14 @@ class ManualTurnInputs:
     except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
       return False
 
+  def native_tx_blocked(self):
+    """Read only after native_permission qualified this same Panda snapshot."""
+    try:
+      value = self.sm["pandaStates"][-1].safetyTxBlocked
+      return int(value) if 0 <= value <= 0xFFFFFFFF else None
+    except (AttributeError, IndexError, KeyError, TypeError, ValueError, OverflowError):
+      return None
+
   def assist_permission(self):
     if not self.track_assist_permission:
       return (), False

openpilot/starpilot/tests/ford_lifecycle_helpers.py

@@ -233,7 +233,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
           for original in messages:
             if original.which() == 'modelV2':
               original = original.as_builder()
-              original.modelV2.action.desiredCurvature = 0.0001
+              original.modelV2.action.desiredCurvature = getattr(monkeypatch, "ford_test_curvature", 0.0001)
               original = original.as_reader()
             snapshots.append(original)
           return control_update(timestamp, snapshots)
@@ -256,6 +256,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
 
       local.setattr(sd.sm, 'update_msgs', location_update)
 
+      delayed_pedal_frames = []
+      delayed_pedals_seen = set()
+
       def step(*, engaged=False, brake=False, reverse=False, warmup=False, fault=None, main_off=False, availability_transition=False):
         nonlocal steps, warmup_waits, qualified_samples, active_lat_frames, active_long_frames, nonzero_after_brake, brake_seen
         brake_seen |= brake
@@ -267,7 +270,8 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
         request = 0 if prior_axis is None else (int(prior_axis.aolAxisState.desiredLateral) | (int(prior_axis.aolAxisState.desiredLongitudinal) << 1))
         arm_heartbeat = retained_arm(published.get('aolIntentWire'), prior_axis, now[0])
         heartbeat = bool(request or prior_state is not None and prior_state.selfdriveState.enabled or arm_heartbeat)
-        frames = []
+        frames = delayed_pedal_frames[:]
+        delayed_pedal_frames.clear()
 
         def receive(name, values):
           fields = dict(values)
@@ -281,7 +285,14 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
           if not (fault == 'source-loss' and name == 'Yaw_Data_FD1'):
             accepted = safety.safety_rx_hook(joined_packet(frame))
             assert bool(accepted) == (not (fault == 'rx-quality' and name == 'BrakeSysFeatures'))
-          frames.append(frame)
+          pedal = name == 'EngVehicleSpThrottle' and fault == 'gas' or name == 'EngBrakeData' and brake
+          if getattr(monkeypatch, 'ford_native_first_pedal', False) and pedal and name not in delayed_pedals_seen:
+            # The real native RX already consumed this frame above. Deliver it
+            # to the host on the next update, retaining native authority now.
+            delayed_pedals_seen.add(name)
+            delayed_pedal_frames.append(frame)
+          else:
+            frames.append(frame)
 
         with patch.object(native, 'rx', side_effect=receive):
           native.pump(
@@ -290,7 +301,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
             gear=1 if reverse else 3,
             mask=request,
             heartbeat=heartbeat and fault != 'heartbeat',
-            speed=15.0,
+            speed=(3.1 + .12 * ((steps // 2) % 12) / 11 if getattr(monkeypatch, "ford_speed_window", False) else 15.0),
             eps=1 if fault == 'temporary-eps' else 0,
           )
         assert native.tick == steps
@@ -385,7 +396,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
         qualified_samples += 1
         parsed_before_producer = state.to_dict()
         assert (main_off or state.cruiseState.available or availability_transition and not state.cruiseState.enabled) and not state.steeringPressed
-        assert bool(state.gasPressed) == (fault == 'gas')
+        assert bool(state.gasPressed) == (fault == 'gas' and not any(frame[0] == 0x204 for frame in delayed_pedal_frames))
         axis, sd_state, command = drive(
           steps,
           car_state_message=published['carState'],
@@ -401,6 +412,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
           panda_state_message=panda,
           safety_message=wire,
         )
+        if delayed_pedal_frames:
+          assert command.latActive and host.aol_card_intent.allowed_latch, 'Native-first pedal erased granted lateral intent'
+          assert not command.longActive, 'Native LONG withdrawal was ignored'
         assert state.to_dict() == parsed_before_producer, 'Authority harness changed actual parsed CarState'
         previous_sd.update(sd.pm.messages)
         control = controls.pm.messages['carControl']
@@ -493,12 +507,18 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
         assert bool(command.longActive) == joined.expected_long
         for _ in range(30):
           state, axis, sd_state, command = step(engaged=True, fault='gas')
+          if getattr(monkeypatch, 'ford_native_first_pedal', False):
+            assert command.latActive and host.aol_card_intent.allowed_latch, 'Native-first gas erased lateral intent'
+            assert not command.longActive or joined.last_permission & 2
         assert command.latActive and not command.longActive and host.aol_card_intent.allowed_latch
         for _ in range(30):
           state, axis, sd_state, command = step(engaged=True)
         assert command.latActive and bool(command.longActive) == joined.expected_long
         for _ in range(30):
           state, axis, sd_state, command = step(brake=True)
+          if getattr(monkeypatch, 'ford_native_first_pedal', False):
+            assert command.latActive and host.aol_card_intent.allowed_latch, 'Native-first brake erased lateral intent'
+            assert not command.longActive or joined.last_permission & 2
         assert host.aol_card_intent.allowed_latch and command.latActive and not command.longActive
         if getattr(monkeypatch, 'ford_angle', False):
           for _ in range(20):

openpilot/starpilot/tests/test_ford_bluepilot_aol.py

@@ -24,3 +24,108 @@ def test_angle_aol_brake_reverse_fault_and_hands_free(ford_joined,monkeypatch):
 def test_mache_nonzero_angle_after_brake_with_hands_free(ford_joined,monkeypatch):
   monkeypatch.ford_nonzero = True
   run_actual_producer(ford_joined,monkeypatch,scenario='configuration-parity')
+
+
+@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)])
+@pytest.mark.parametrize('alpha', [False, True])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
+def test_tight_turn_speed_window_stays_inside_native_caps(ford_joined, monkeypatch):
+  # Route19's first blocked frames exceed Panda's cap by using the latest
+  # speed instead of its six-sample minimum. Exercise real parser/native windows.
+  monkeypatch.ford_nonzero = True
+  monkeypatch.ford_speed_window = True
+  monkeypatch.ford_test_curvature = .03
+  run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity')
+
+
+@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)])
+@pytest.mark.parametrize('alpha', [False, True])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic'])
+def test_real_rejected_packet_resynchronizes_without_permission_bypass(ford_joined, monkeypatch):
+  from types import SimpleNamespace
+  from opendbc.car import structs
+  from opendbc.safety.tests.libsafety import libsafety_py
+
+  joined = ford_joined
+  native, safety = joined.native, joined.safety
+  native.arm()
+  adapter = joined.host.CI.CC.bp_lat
+  inputs = adapter.host.inputs
+  blocked = 0
+  monkeypatch.setattr(inputs, 'update', lambda: None)
+  monkeypatch.setattr(inputs, 'lateral_snapshot', lambda speed: None)
+  # Isolate the adapter recovery while preserving actual physical native authority.
+  # The full caller matrix above retains real wire/inventory/freshness qualification.
+  monkeypatch.setattr(inputs, 'native_permission', lambda cp, max_age_ns: bool(safety.aol_get_permission_mask() & 1))
+  CC = structs.CarControl(enabled=True, latActive=True)
+  CC.actuators.curvature = .0001
+  state = structs.CarState(canValid=True, vEgoRaw=15., gearShifter=structs.CarState.GearShifter.drive)
+  state.cruiseState.available = True
+  CS = SimpleNamespace(out=state, lat_ctl_lim_stat=0)
+  rejected = recovered = False
+  neutral_after_rejection = False
+  for frame in range(180):
+    native.pump(1, mask=1, speed=15.)
+    now = (1_000_000 + native.tick * 10_000) * 1000
+    from openpilot.cereal import messaging
+    panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now)
+    panda.pandaStates[0].safetyTxBlocked = blocked
+    inputs.sm.update_msgs(now / 1e9, [panda.as_reader()])
+    packets = []
+    if frame % 5 == 0:
+      packets += adapter.step_lateral(CC, CS, native.packer, joined.host.CI.CC.CAN, frame, now)
+    if frame % 3 == 0:
+      packets += adapter.step_lka(native.packer, joined.host.CI.CC.CAN, now)
+    for address, original, bus in packets:
+      data = bytearray(original)
+      active = address == 0x3D6 and (data[0] >> 4) & 7 == 1
+      if active and frame >= 80 and not rejected:
+        data[1] ^= 1  # One actual bad checksum; native must reject it.
+        assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))
+        blocked += 1
+        rejected = True
+        continue
+      accepted = safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))
+      assert accepted, (frame, hex(address), data.hex())
+      if rejected and address == 0x3D6:
+        neutral_after_rejection |= not active
+        recovered |= active and neutral_after_rejection
+  assert rejected and neutral_after_rejection and recovered
+  # Host recovery never replaces native authorization.
+  safety.aol_set_host_request(0)
+  native.pump(5, mask=0, speed=15.)
+  now = (1_000_000 + native.tick * 10_000) * 1000
+  neutral = adapter.step_lateral(CC, CS, native.packer, joined.host.CI.CC.CAN, 180, now)[0]
+  assert (neutral[1][0] >> 4) & 7 == 0
+
+
+@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32), (CAR.FORD_F_150_MK14, 66)])
+@pytest.mark.parametrize('alpha', [True])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
+def test_native_first_pedal_preserves_only_granted_lateral(ford_joined, monkeypatch):
+  monkeypatch.ford_nonzero = True
+  monkeypatch.ford_native_first_pedal = True
+  run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity')
+
+
+@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)])
+@pytest.mark.parametrize('alpha', [True])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
+def test_early_pedal_admission_requires_exact_marked_angle_long_profile(ford_joined):
+  from opendbc.car import structs
+  from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported
+  cp = ford_joined.host.CP
+  assert early_native_long_withdrawal_supported(cp)
+  for field, value in (('alternativeExperience', 0), ('openpilotLongitudinalControl', False),
+                       ('passive', True), ('dashcamOnly', True), ('brand', 'hyundai')):
+    changed = structs.CarParams(**cp.to_dict())
+    setattr(changed, field, value)
+    assert not early_native_long_withdrawal_supported(changed)
+  for mask in (128, 1, 16):
+    changed = structs.CarParams(**cp.to_dict())
+    changed.safetyConfigs[-1].safetyParam ^= mask
+    assert not early_native_long_withdrawal_supported(changed)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "28e81fb375b1a055d806280af30b92101095fcc3a11d1bb200e9a645a3d84d25", "schema_version": 1}
+{"protected_inventory_sha256": "689ef29810231d81be86a79ecf7c83429f53f5bee5e38ae268e9a3ae977cfcbb", "schema_version": 1}