Allow Ford stock conventional cruise with always-on lateral

openpilot/selfdrive/car/car_events.py

@@ -7,6 +7,7 @@ from opendbc.car.toyota.values import ToyotaFlags
 
 from openpilot.selfdrive.selfdrived.events import Events
 from openpilot.starpilot.car.events import event_rules, unsupported_gear
+from openpilot.starpilot.car.ford.aol import stock_conventional_cruise_supported
 
 ButtonType = structs.CarState.ButtonEvent.Type
 GearShifter = structs.CarState.GearShifter
@@ -138,7 +139,7 @@ class CarEvents:
       events.add(EventName.stockLkas)
     if CS.vEgo > MAX_CTRL_SPEED:
       events.add(EventName.speedTooHigh)
-    if CS.cruiseState.nonAdaptive:
+    if CS.cruiseState.nonAdaptive and not stock_conventional_cruise_supported(self.CP):
       events.add(EventName.wrongCruiseMode)
     if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl:
       events.add(EventName.brakeHold)

openpilot/starpilot/car/ford/aol.py

@@ -15,6 +15,12 @@ def policy_for(cp):
   return AolVehiclePolicy()
 
 
+
+def stock_conventional_cruise_supported(cp):
+  """The marked stock-cruise AOL profile does not require adaptive speed control."""
+  return not cp.openpilotLongitudinalControl and qualified(cp, marked_only=True)
+
+
 def native_profile_supported(model, param):
   return model == int(structs.CarParams.SafetyModel.ford) and profile_word(param) in AOL_WORDS
 

openpilot/starpilot/tests/ford_lifecycle_helpers.py

@@ -167,7 +167,7 @@ def ford_joined(identity, alpha, hands_free, monkeypatch, record_property):
           native.pump(1, main=0 if getattr(monkeypatch, 'ford_first_main', False) else 3, mask=0, heartbeat=False, speed=15.0)
         extras: list[tuple[str, int, dict[str, float]]] = [
           ('SteeringPinion_Data', 0, {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3}),
-          ('Cluster_Info1_FD1', 0, {'AccEnbl_B_RqDrv': 1}),
+          ('Cluster_Info1_FD1', 0, {'AccEnbl_B_RqDrv': int(not getattr(monkeypatch, 'ford_non_adaptive', False))}),
           ('BodyInfo_3_FD1', 0, {}),
           ('RCMStatusMessage2_FD1', 0, {'FirstRowBuckleDriver': 1}),
           ('INSTRUMENT_PANEL', 0, {}),
@@ -219,6 +219,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
   long_parser = CANParser(DBC[host.CP.carFingerprint][Bus.pt], [('ACCDATA', 50)], 0)
   lateral_name = 'LateralMotionControl2' if host.CP.flags & FordFlags.CANFD else 'LateralMotionControl'
   lateral_parser = CANParser(DBC[host.CP.carFingerprint][Bus.pt], [(lateral_name, 20)], 0)
+  joined.stock_cruise_active_samples = 0
   joined.nonzero_after_brake = 0
   joined.path_angle_after_brake = 0
   joined.active_ui_frames = 0
@@ -261,7 +262,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
         return None if source_gap_active[0] and service == 'aolIntentWire' else send(service, raw)
 
       local.setattr(host.pm, 'send', capture)
-      if scenario == 'model-loss':
+      if scenario in ('model-loss', 'conventional-model-loss'):
         model_update = sd.sm.update_msgs
         def drop_model(timestamp, messages):
           return model_update(timestamp, [m for m in messages if not (model_lost[0] and m.which() == 'modelV2')])
@@ -392,7 +393,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
         assert permission & 1 == 0 or heartbeat and request & 1 and native_request & 1
         extras = [
           ('SteeringPinion_Data', 0, {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3}),
-          ('Cluster_Info1_FD1', 0, {'AccEnbl_B_RqDrv': 1}),
+          ('Cluster_Info1_FD1', 0, {'AccEnbl_B_RqDrv': int(not getattr(monkeypatch, 'ford_non_adaptive', False))}),
           ('BodyInfo_3_FD1', 0, {}),
           ('RCMStatusMessage2_FD1', 0, {'FirstRowBuckleDriver': 1}),
           ('INSTRUMENT_PANEL', 0, {}),
@@ -496,6 +497,8 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
         state, _ = host.state_update()
         host.state_publish(state, None)
         assert 'aolIntentWire' in published
+        if getattr(monkeypatch, 'ford_non_adaptive', False):
+          assert state.cruiseState.nonAdaptive
         parser_ready = bool(
           state.canValid and not state.canTimeout and (state.cruiseState.available or scenario == 'route49' or warmup and main_off) and
           str(state.gearShifter) == 'drive' and safety.safety_config_valid()
@@ -624,6 +627,8 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
                   joined.nonzero_after_brake = nonzero_after_brake
                   joined.path_angle_after_brake += int(abs(decoded['LatCtlPath_An_Actl']) > 1e-7)
             elif address == 0x186:
+              if getattr(monkeypatch, 'ford_non_adaptive', False):
+                assert host.CP.openpilotLongitudinalControl, 'Stock cruise received a comma acceleration message'
               long_parser.update([(source_ns, [(address, bytes(data), frame.src - joined.offset)])])
               active = bool(long_parser.vl['ACCDATA']['Cmbb_B_Enbl'])
               assert active == bool(command.longActive)
@@ -678,6 +683,12 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
                                  tuple(str(event.name) for event in host.sm['onroadEvents']),
                                  host.aol_card_intent.latch_clear_reason,
                                  str(axis.mode)))
+        if getattr(monkeypatch, 'ford_non_adaptive', False) and not host.CP.openpilotLongitudinalControl:
+          assert not axis.desiredLongitudinal and not command.longActive and not permission & 2
+          assert safety.safety_fwd_hook(2, 0x186) == 0, 'Stock cruise acceleration forwarding was blocked'
+          if state.cruiseState.enabled and command.latActive and not cancel and not brake and fault is None:
+            assert not command.cruiseControl.cancel, 'AOL cancelled the driver-owned conventional cruise'
+            joined.stock_cruise_active_samples += 1
         return state, axis, sd_state, command
 
       def joined_packet(frame):
@@ -1137,6 +1148,24 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
         assert not command.latActive and not command.longActive
         assert not native.safety.aol_get_permission_mask()
         return
+      if scenario == 'conventional-long-refusal':
+        for _ in range(40):
+          state, axis, sd_state, command = step(engaged=True)
+        assert host.CP.openpilotLongitudinalControl and state.cruiseState.nonAdaptive
+        assert sd_state.alertType == 'wrongCruiseMode/noEntry'
+        assert command.cruiseControl.cancel and not command.latActive and not command.longActive
+        assert not axis.desiredLateral and not axis.desiredLongitudinal
+        assert not safety.aol_get_permission_mask() and active_lat_frames == active_long_frames == 0
+        return
+      if scenario == 'conventional-model-loss':
+        for _ in range(40):
+          state, axis, sd_state, command = step(engaged=True)
+        assert command.latActive and not command.longActive
+        for _ in range(450):
+          state, axis, sd_state, command = step(engaged=True, fault='model-loss')
+        assert not sd.sm.alive['modelV2'] and not controls.sm.alive['modelV2']
+        assert not command.latActive and not command.longActive and not safety.aol_get_permission_mask()
+        return
       if scenario == 'configuration-parity':
         for _ in range(40):
           state, axis, sd_state, command = step(engaged=True)

openpilot/starpilot/tests/test_ford_configuration_parity.py

@@ -175,3 +175,101 @@ def test_native_rejected_ui_cannot_replace_stock_cluster(ford_joined, monkeypatc
   assert ford_joined.rejected_ui_packets
   assert all(address in (0x18A, 0x3D8) for _, address in ford_joined.rejected_ui_packets)
 
+
+
+@pytest.fixture
+def conventional_ford_joined(request, monkeypatch, mode, identity, alpha, hands_free):
+  monkeypatch.ford_non_adaptive = True
+  monkeypatch.ford_bluepilot = True
+  monkeypatch.ford_angle = mode == 'angle'
+  monkeypatch.ford_starpilot = mode == 'starpilot'
+  monkeypatch.ford_pinion = True
+  monkeypatch.ford_nonzero = True
+  monkeypatch.ford_native_first_pedal = True
+  monkeypatch.ford_native_10hz = True
+  return request.getfixturevalue('ford_joined')
+
+
+@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
+@pytest.mark.parametrize('alpha', [False])
+@pytest.mark.parametrize('hands_free', [False, True])
+@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
+@pytest.mark.parametrize('mode', ['curvature', 'angle', 'starpilot'])
+@pytest.mark.parametrize('scenario', ['configuration-parity', 'cancel-native-cadence'])
+def test_conventional_cruise_keeps_actual_aol_without_longitudinal(conventional_ford_joined, monkeypatch, scenario):
+  joined = conventional_ford_joined
+  assert not joined.host.CP.openpilotLongitudinalControl
+  run_actual_producer(joined, monkeypatch, scenario=scenario)
+  assert joined.stock_cruise_active_samples > 0
+  if scenario == 'configuration-parity':
+    assert joined.nonzero_after_brake > 0
+
+
+@pytest.mark.parametrize('identity', FORD_FACTORIES)
+@pytest.mark.parametrize('alpha', [False, True])
+@pytest.mark.parametrize('marked', [False, True])
+@pytest.mark.parametrize('topology', ['automatic'])
+def test_conventional_cruise_event_requires_stock_marked_ford(identity, alpha, marked):
+  from opendbc.car import gen_empty_fingerprint, structs
+  from opendbc.car.ford.interface import CarInterface
+  from openpilot.cereal import log
+  from openpilot.selfdrive.car.car_events import CarEvents
+  fp = gen_empty_fingerprint()
+  fp[0].update({0x5A: 8, 0x3A6: 8, 0x3A7: 8})
+  fp[2].update({0x3D6: 8, 0x186: 8})
+  cp = CarInterface.get_params(identity[0], fp, [], alpha, False, False)
+  cp.alternativeExperience = 32 if marked else 0
+  state = structs.CarState.new_message(canValid=True, gearShifter='drive',
+      cruiseState={'available': True, 'enabled': True, 'nonAdaptive': True})
+  events = CarEvents(cp).create_common_events(state, structs.CarState.new_message())
+  assert (log.OnroadEvent.EventName.wrongCruiseMode in events.names) == (alpha or not marked)
+  state.steerFaultPermanent = state.accFaulted = True
+  faults = CarEvents(cp).create_common_events(state, state)
+  assert log.OnroadEvent.EventName.steerUnavailable in faults.names
+  assert log.OnroadEvent.EventName.accFaulted in faults.names
+
+
+@pytest.mark.parametrize('topology', ['automatic'])
+@pytest.mark.parametrize('invalid', ['passive', 'dashcam', 'not-car', 'missing-panda', 'extra-owner', 'wrong-word', 'wrong-brand'])
+def test_conventional_cruise_never_admits_an_unsupported_profile(invalid):
+  from opendbc.car import gen_empty_fingerprint
+  from opendbc.car.ford.interface import CarInterface
+  from openpilot.starpilot.car.ford.aol import stock_conventional_cruise_supported
+  fp = gen_empty_fingerprint()
+  fp[0][0x5A] = 8
+  fp[2].update({0x3D6: 8, 0x186: 8})
+  cp = CarInterface.get_params(FORD_FACTORIES[11][0], fp, [], False, False, False)
+  cp.alternativeExperience = 32
+  if invalid == 'passive':
+    cp.passive = True
+  elif invalid == 'dashcam':
+    cp.dashcamOnly = True
+  elif invalid == 'not-car':
+    cp.notCar = True
+  elif invalid == 'missing-panda':
+    cp.safetyConfigs = []
+  elif invalid == 'extra-owner':
+    cp.safetyConfigs = [cp.safetyConfigs[0].to_dict()] * 2
+  elif invalid == 'wrong-word':
+    cp.safetyConfigs[0].safetyParam = 3
+  else:
+    cp.brand = 'toyota'
+  assert not stock_conventional_cruise_supported(cp)
+
+
+@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
+@pytest.mark.parametrize('alpha', [True])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic'])
+@pytest.mark.parametrize('mode', ['curvature', 'angle', 'starpilot'])
+def test_conventional_cruise_keeps_alpha_long_guard(conventional_ford_joined, monkeypatch):
+  run_actual_producer(conventional_ford_joined, monkeypatch, scenario='conventional-long-refusal')
+
+
+@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
+@pytest.mark.parametrize('alpha', [False])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic'])
+@pytest.mark.parametrize('mode', ['starpilot'])
+def test_conventional_cruise_stops_for_lost_model(conventional_ford_joined, monkeypatch):
+  run_actual_producer(conventional_ford_joined, monkeypatch, scenario='conventional-model-loss')

openpilot/starpilot/ui/onroad_state.py

@@ -183,8 +183,6 @@ class OnroadState:
   speed_mps: float | None
   cruise_kph: float | None
   speed_limit: SpeedLimitObservation
-  ford_rear_fog_on: bool = False
-  ford_rear_fog_source_ns: int = 0
   appearance: OnroadAppearance = field(default_factory=OnroadAppearance)
   alert: OnroadAlert = OnroadAlert()
   metric: bool = False
@@ -248,6 +246,8 @@ class OnroadState:
   stop_sign_observed_ns: int = 0
   weather_location: tuple[float, float] | None = None
   weather: WeatherReading | None = None
+  ford_rear_fog_on: bool = False
+  ford_rear_fog_source_ns: int = 0
 
   def __post_init__(self) -> None:
     if self.speed_mps is not None and (not math.isfinite(self.speed_mps) or self.speed_mps < 0):
@@ -267,6 +267,7 @@ class OnroadState:
       raise ValueError("Model confidence must be normalized")
 
 
+
 @dataclass(frozen=True)
 class OnroadRequest:
   kind: str

openpilot/starpilot/ui/runtime_app.py

@@ -1309,9 +1309,11 @@ class StarShellSession:
     if getattr(self, "rear_fog", None) is None:
       self.rear_fog = RearFogReceiver()
     ui = self.adapter.ui_state
-    enabled = snapshot.onroad.customization["layouts"][str(self.profile)]["rear_fog"]["enabled"]
-    fog_on, fog_stamp = self.rear_fog.sample(ui.CP, active=mode == ShellMode.ONROAD and bool(ui.started) and enabled)
-    snapshot = replace(snapshot, onroad=replace(snapshot.onroad, ford_rear_fog_on=fog_on, ford_rear_fog_source_ns=fog_stamp))
+    cp = getattr(ui, "CP", None)
+    enabled = snapshot.onroad.customization.get("layouts", {}).get(str(self.profile), {}).get("rear_fog", {}).get("enabled", False)
+    fog_on, fog_stamp = self.rear_fog.sample(cp, active=mode == ShellMode.ONROAD and bool(getattr(ui, "started", False)) and cp is not None and enabled)
+    if (fog_on, fog_stamp) != (snapshot.onroad.ford_rear_fog_on, snapshot.onroad.ford_rear_fog_source_ns):
+      snapshot = replace(snapshot, onroad=replace(snapshot.onroad, ford_rear_fog_on=fog_on, ford_rear_fog_source_ns=fog_stamp))
     if mode == ShellMode.ONROAD and self.profile == Profile.LARGE:
       width = getattr(self, '_onroad_page_width', 2160.0) - (SIDEBAR_WIDTH if snapshot.onroad.appearance.show_developer_sidebar else 0)
       fitted = fit_to_road(snapshot.onroad.customization, width)

openpilot/starpilot/ui/tests/test_ford_rear_fog.py

@@ -232,3 +232,30 @@ def test_boot_clock_matches_native_mac_fallback(monkeypatch):
   monkeypatch.setattr(rear_fog.time, "clock_gettime_ns", clock)
   assert rear_fog.boot_time_ns() == 10_000_000_000
   clock.assert_called_once_with(rear_fog.time.CLOCK_MONOTONIC)
+
+
+@pytest.mark.parametrize('missing', ['CP', 'started', 'layout'])
+def test_runtime_fog_is_off_without_vehicle_or_drive_context(missing):
+  from openpilot.starpilot.ui.runtime_app import StarShellSession
+  from openpilot.starpilot.ui.shell import ShellSnapshot, ShellMode
+  from openpilot.starpilot.ui.presentation import Profile
+  from openpilot.starpilot.ui.settings_state import Destination
+  from openpilot.starpilot.ui.onroad_state import OnroadState, SpeedLimitObservation
+  state = OnroadState(False, True, 0, None, SpeedLimitObservation(), customization=default_document())
+  session = StarShellSession.__new__(StarShellSession)
+  session.adapter = Mock()
+  session.adapter.ui_state = NS(sm=NS(frame=1), CP=cp(), started=True)
+  if missing == 'layout':
+    state = replace(state, customization={})
+  else:
+    delattr(session.adapter.ui_state, missing)
+  session.adapter.build.return_value = ShellSnapshot(ShellMode.ONROAD, Mock(), Mock(), state)
+  session.selected = Destination.STAR
+  session.compact_y = session.compact_scroll_x = 0
+  session.sidebar_expanded = True
+  session.profile = Profile.COMPACT
+  session._snapshot_cache = None
+  session.rear_fog = Mock()
+  session.rear_fog.sample.return_value = (False, 0)
+  assert not session.snapshot(ShellMode.ONROAD).onroad.ford_rear_fog_on
+  assert session.rear_fog.sample.call_args.kwargs == {'active': False}

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "c910386ebeafeca30850b91d3b991c99b37b6e59381208d60a6d2662dafc2f5d", "schema_version": 1}
+{"protected_inventory_sha256": "9d58d8c35076a0cc97bb85692283b41108a28ba6a8cd0d7091e7a396b20d372c", "schema_version": 1}