Ford: parse the PSCM LatCtl status broadcast + opt-in hands-on context for steerSaturated

cereal/custom.capnp

@@ -554,6 +554,7 @@ struct CarStateBP @0xb057204d7deadf3f {
   hybridDrive @0 :HybridDrive;
   hybridBattery @1 :HybridBattery;
   brakeLightStatus @2 :BrakeLightStatus;
+  pscmLatCtl @3 :PscmLatCtl;
 
   struct HybridDrive {
     dataAvailable @0 :Bool;
@@ -580,6 +581,15 @@ struct CarStateBP @0xb057204d7deadf3f {
     dataAvailable @0 :Bool;
     brakeLightsOn @1 :Bool;
   }
+
+  # Ford PSCM lateral-control status broadcast (Lane_Assist_Data3_FD1)
+  struct PscmLatCtl {
+    dataAvailable @0 :Bool;
+    laActAvail @1 :UInt8;  # LaActAvail_D_Actl feature matrix: bit1 = LCA/LKA centering available, bit0 = LDW not suppressed; values 0/1 = centering policy-suppressed (Q3: below ~40 km/h)
+    laActDeny @2 :Bool;  # LaActDeny_B_Actl
+    laHandsOff @3 :Bool;  # LaHandsOff_B_Actl: PSCM hands-off estimate, more sensitive than steeringPressed
+    tjaHandsOnConfidence @4 :Bool;  # TjaHandsOnCnfdnc_B_Est
+  }
 }
 
 struct CustomReserved15 @0xbd443b539493bc68 {

common/params_keys.h

@@ -318,6 +318,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"vbatt_pause_charging", {PERSISTENT | BACKUP, FLOAT, "11.8"}},
     {"show_lead_speed", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"FordPrefSteerAngleCurvature", {PERSISTENT | BACKUP, BOOL, "0"}},  // pinion-sourced curvature measurement (bad-yaw-sensor workaround); read at car init
+    {"FordPrefHideSteerSaturatedAlerts", {PERSISTENT | BACKUP, BOOL, "0"}},  // hide steerSaturated while the Ford PSCM reports hands-on; read by selfdrived
     {"FordPrefShowRadarLeadOverlay", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"FordPrefRadarOverlaySize", {PERSISTENT | BACKUP, INT, "1"}},
     {"FordPrefHybridBatteryStatus", {PERSISTENT | BACKUP, BOOL, "0"}},

opendbc_repo/opendbc/car/ford/carstate.py

@@ -225,6 +225,9 @@ def get_can_parsers(CP, CP_SP):
     else:
       pt_messages += [
         ("INSTRUMENT_PANEL", 1),
+        # BluePilot: PSCM LatCtl status telemetry — broadcast by CAN platforms too (measured 33Hz on
+        # Ford Q3), but presence isn't guaranteed fleet-wide, so keep it out of CAN validity
+        ("Lane_Assist_Data3_FD1", float('nan')),
       ]
 
     if CP.transmissionType == TransmissionType.automatic:

opendbc_repo/opendbc/sunnypilot/car/ford/carstate_ext.py

@@ -90,6 +90,9 @@ def __init__(self, CP, CP_SP):
     self.cruise_enabled_prev = False
     # Track if mainCruise was pressed recently (to handle delayed cruise enable)
     self.main_cruise_pressed_recently = False
+    # Latches True once Lane_Assist_Data3_FD1 is seen — the message is non-critical in the
+    # parser, so cp.vl would otherwise report zeros on cars that never broadcast it
+    self.lane_assist_data3_seen = False
 
   def update(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser]):
     """
@@ -340,6 +343,23 @@ def update_car_state_bp(self, cp, cp_cam):
     brake_light_status.dataAvailable = False
     brake_light_status.brakeLightsOn = False
 
+    pscm_lat_ctl = dat.carStateBP.pscmLatCtl
+    pscm_lat_ctl.dataAvailable = False
+
+    # PSCM lateral-control status (Lane_Assist_Data3_FD1)
+    try:
+      if len(cp.vl_all["Lane_Assist_Data3_FD1"]["LaActAvail_D_Actl"]) > 0:
+        self.lane_assist_data3_seen = True
+      if self.lane_assist_data3_seen:
+        lad3 = cp.vl["Lane_Assist_Data3_FD1"]
+        pscm_lat_ctl.dataAvailable = True
+        pscm_lat_ctl.laActAvail = int(lad3["LaActAvail_D_Actl"])
+        pscm_lat_ctl.laActDeny = bool(lad3["LaActDeny_B_Actl"])
+        pscm_lat_ctl.laHandsOff = bool(lad3["LaHandsOff_B_Actl"])
+        pscm_lat_ctl.tjaHandsOnConfidence = bool(lad3["TjaHandsOnCnfdnc_B_Est"])
+    except (KeyError, AttributeError):
+      pass
+
     # Brake light status — try BCM message first, then fallback to BrakeSysFeatures_2
     brake_lights_detected = False
 

selfdrive/selfdrived/selfdrived.py

@@ -9,6 +9,7 @@
 from msgq.visionipc import VisionIpcClient, VisionStreamType
 
 
+from openpilot.common.bluepilot import is_bluepilot
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
 from openpilot.common.swaglog import cloudlog
@@ -91,7 +92,10 @@ def __init__(self, CP=None, CP_SP=None):
     # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
-    ignore = self.sensor_packets + self.gps_packets + ['alertDebug', 'lateralManeuverPlan'] + ['modelDataV2SP']
+    # BluePilot: carStateBP is only published by brands whose carstate builds the message
+    # (Ford today), so it must never gate all_checks()/commIssue on other cars
+    ignore = self.sensor_packets + self.gps_packets + ['alertDebug', 'lateralManeuverPlan'] + ['modelDataV2SP'] \
+             + (['carStateBP'] if is_bluepilot() else [])
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
@@ -102,6 +106,7 @@ def __init__(self, CP=None, CP_SP=None):
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
                                    'lateralManeuverPlan', 'modelDataV2SP', 'longitudinalPlanSP'] + \
+                                   (['carStateBP'] if is_bluepilot() else []) + \
                                    self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -110,6 +115,8 @@ def __init__(self, CP=None, CP_SP=None):
     self.is_metric = self.params.get_bool("IsMetric")
     self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
     self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+    # BluePilot: hide steerSaturated while the EPS confirms hands on the wheel (Ford PSCM)
+    self.hide_steer_sat_hands_on = self.params.get_bool("FordPrefHideSteerSaturatedAlerts")
 
     car_recognized = self.CP.brand != 'mock'
 
@@ -444,7 +451,16 @@ def update_events(self, CS):
       turning = abs(desired_lateral_accel) > 1.0
       # TODO: lac.saturated includes speed and other checks, should be pulled out
       if undershooting and turning and lac.saturated:
-        self.events.add(EventName.steerSaturated)
+        # BluePilot: the Ford PSCM's LaHandsOff broadcast detects hands at ~0.5 Nm where
+        # steeringPressed needs ~1.0 -- and those sub-threshold resisting hands both cause
+        # these episodes and prove the driver is already engaged with the wheel. Optionally
+        # hide the alert in exactly that case; if the EPS says hands-off, it always shows.
+        suppress = False
+        if self.hide_steer_sat_hands_on and 'carStateBP' in self.sm.data:
+          pscm = self.sm['carStateBP'].pscmLatCtl
+          suppress = pscm.dataAvailable and not pscm.laHandsOff
+        if not suppress:
+          self.events.add(EventName.steerSaturated)
 
     # Check for FCW
     stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
@@ -620,6 +636,7 @@ def params_thread(self, evt):
       self.is_metric = self.params.get_bool("IsMetric")
       self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
       self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+      self.hide_steer_sat_hands_on = self.params.get_bool("FordPrefHideSteerSaturatedAlerts")
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
       self.personality = self.params.get("LongitudinalPersonality", return_default=True)
 

selfdrive/ui/bp/layouts/settings/bluepilot.py

@@ -70,6 +70,7 @@ def __init__(self):
     self._refresh_toggles = (
       ("send_hands_free_cluster_msg", self._show_hands_free_ui),
       ("FordPrefSteerAngleCurvature", self._steer_angle_curvature),
+      ("FordPrefHideSteerSaturatedAlerts", self._hide_steer_sat_alerts),
       ("BPDisableLaneLineStatusColor", self._disable_lane_line_status_color),
       ("BPHideCameraView", self._hide_camera_view),
       ("BPRadRacerTheme", self._rad_racer_theme),
@@ -121,6 +122,14 @@ def _initialize_items(self):
       icon="monitoring.png"
     )
 
+    self._hide_steer_sat_alerts = toggle_item(
+      lambda: tr("Hide Steering-Limit Alerts While Holding the Wheel"),
+      lambda: tr('Hides the "Turn Exceeds Steering Limit" warning only while the power steering itself reports your hands on the wheel. Light pressure against the turn usually triggers these warnings, and the steering rack detects that grip well below the pressure the driving software needs. If the rack reports hands-off, the warning always shows.'),
+      initial_state=self._safe_get_bool(self._params, "FordPrefHideSteerSaturatedAlerts"),
+      callback=lambda state: self._toggle_callback(state, "FordPrefHideSteerSaturatedAlerts"),
+      icon="monitoring.png"
+    )
+
     # Lane line status color toggle (issue #109: option to keep lane lines grey instead of green when engaged)
     self._disable_lane_line_status_color = toggle_item(
       lambda: tr("Disable Lane Line Status Color"),
@@ -655,6 +664,7 @@ def _section(title: str, items: list) -> list:
       _section(tr("Vehicle"), [
         self._show_hands_free_ui,
         self._steer_angle_curvature,
+        self._hide_steer_sat_alerts,
         self._vbatt_pause_charging,
       ]) +
       _section(tr("Audio"), [

selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py

@@ -19,17 +19,20 @@ def __init__(self, back_callback: Callable[[], None] | None = None):
     self.show_hands_free_ui = BigParamControlBP("Show BlueCruise UI on Cluster", "send_hands_free_cluster_msg")
     # Init-time param (read once at car init, mirrored into panda safety); takes effect after restart
     self.steer_angle_curvature = BigParamControlBP("Use Pinion Yaw Sensor", "FordPrefSteerAngleCurvature")
+    self.hide_steer_sat_alerts = BigParamControlBP("Hide Steer-Limit Alerts (Hands On)", "FordPrefHideSteerSaturatedAlerts")
     self.vbatt_pause_charging = BigParamFloatControl("12V Battery Limit", "vbatt_pause_charging", min=11.0, max=14.0, step=0.1)
 
     self._scroller.add_widgets([
       self.show_hands_free_ui,
       self.steer_angle_curvature,
+      self.hide_steer_sat_alerts,
       self.vbatt_pause_charging,
     ])
 
     self._refresh_toggles = (
       ("send_hands_free_cluster_msg", self.show_hands_free_ui),
       ("FordPrefSteerAngleCurvature", self.steer_angle_curvature),
+      ("FordPrefHideSteerSaturatedAlerts", self.hide_steer_sat_alerts),
     )
 
     ui_state.add_offroad_transition_callback(self._update_toggles)

sunnypilot/sunnylink/settings_ui.json

@@ -2129,6 +2129,12 @@
             }
           ]
         },
+        {
+          "key": "FordPrefHideSteerSaturatedAlerts",
+          "widget": "toggle",
+          "title": "[Vehicle] Hide Steering-Limit Alerts While Holding the Wheel",
+          "description": "Hides the \"Turn Exceeds Steering Limit\" warning only while the power steering itself reports your hands on the wheel. Light pressure against the turn is what usually triggers these warnings, and the steering rack detects that grip well below the pressure the driving software needs to notice it. If the steering rack reports hands-off, the warning always shows."
+        },
         {
           "key": "BPUseCustomSounds",
           "widget": "toggle",

sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -57,6 +57,14 @@ sections:
       the Edge (its pinion sensor only reports a relative angle).'
     enablement:
     - $ref: '#/macros/offroad'
+  - key: FordPrefHideSteerSaturatedAlerts
+    widget: toggle
+    title: '[Vehicle] Hide Steering-Limit Alerts While Holding the Wheel'
+    description: 'Hides the "Turn Exceeds Steering Limit" warning only while the power
+      steering itself reports your hands on the wheel. Light pressure against the turn
+      is what usually triggers these warnings, and the steering rack detects that grip
+      well below the pressure the driving software needs to notice it. If the steering
+      rack reports hands-off, the warning always shows.'
   # --- Audio ---
   - key: BPUseCustomSounds
     widget: toggle