From 604401aad9c9e2432bf0f680ac007b0995a7d73a Mon Sep 17 00:00:00 2001 From: Jacob Neulight Date: Mon, 20 Jul 2026 13:15:09 -0600 Subject: [PATCH 1/2] Ford: parse PSCM LatCtl status broadcast (Lane_Assist_Data3_FD1) into carStateBP The PSCM broadcasts its lateral-control availability policy and hands-on estimate on Lane_Assist_Data3_FD1 at 33Hz on CAN platforms too, not just CANFD. Parse it (non-critical, seen-latched) and publish LaActAvail_D_Actl, LaActDeny_B_Actl, LaHandsOff_B_Actl and TjaHandsOnCnfdnc_B_Est under carStateBP.pscmLatCtl so every route carries the PSCM's own account of whether it is serving lateral control and whether it detects hands. Per the DBC VAL_ table LaActAvail_D_Actl is a feature matrix: bit1 = LCA/LKA centering available, bit0 = LDW not suppressed. --- cereal/custom.capnp | 10 ++++++++++ opendbc_repo/opendbc/car/ford/carstate.py | 3 +++ .../sunnypilot/car/ford/carstate_ext.py | 20 +++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index d02bcbedf3..5ac0d94590 100644 --- a/cereal/custom.capnp +++ b/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 { diff --git a/opendbc_repo/opendbc/car/ford/carstate.py b/opendbc_repo/opendbc/car/ford/carstate.py index ef7dff9650..868865f7fd 100644 --- a/opendbc_repo/opendbc/car/ford/carstate.py +++ b/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: diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/carstate_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/carstate_ext.py index 781b2450e3..f782a02619 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/carstate_ext.py +++ b/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 From 4f5ac1a981a1120a329423722befffbf22332612 Mon Sep 17 00:00:00 2001 From: Jacob Neulight Date: Mon, 20 Jul 2026 13:19:20 -0600 Subject: [PATCH 2/2] Ford: wire FordPrefHideSteerSaturatedAlerts to the PSCM hands-on broadcast The param existed but was connected to nothing. Wire it conservatively: steerSaturated is hidden only while the EPS itself reports hands on the wheel (LaHandsOff_B_Actl == 0) -- the PSCM detects grip at ~0.5 Nm where steeringPressed needs ~1.0, and those sub-threshold resisting hands both cause the alert storms and prove the driver is already holding the wheel. If the EPS reports hands-off, the alert always shows. The toggle is exposed on both settings surfaces: the sunnylink source (settings_ui.json regenerated with the compiler) and the on-device vehicle settings (TICI and MICI layouts, directly under the pinion toggle). carStateBP joins selfdrived's SubMaster ignore lists: only brands whose carstate builds the message publish it (Ford today), so it must never gate all_checks()/commIssue on other cars. The key is registered in common/params_keys.h -- bluepilot/params/params.json is a separate registry that Params() does not consult, and reading an unregistered key raises UnknownKeyName (selfdrived crash-looped on device until this line existed). --- common/params_keys.h | 1 + selfdrive/selfdrived/selfdrived.py | 21 +++++++++++++++++-- selfdrive/ui/bp/layouts/settings/bluepilot.py | 10 +++++++++ .../bp/mici/layouts/settings/vehicle_mici.py | 3 +++ sunnypilot/sunnylink/settings_ui.json | 6 ++++++ .../settings_ui_src/pages/vehicle.yaml | 8 +++++++ 6 files changed, 47 insertions(+), 2 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index e183f1180d..c455592584 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -318,6 +318,7 @@ inline static std::unordered_map 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"}}, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 30605d0a27..f5dd8b2cc2 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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) diff --git a/selfdrive/ui/bp/layouts/settings/bluepilot.py b/selfdrive/ui/bp/layouts/settings/bluepilot.py index 858b6b3efd..5422b2158c 100644 --- a/selfdrive/ui/bp/layouts/settings/bluepilot.py +++ b/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"), [ diff --git a/selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py b/selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py index 454c482338..70603c20b8 100644 --- a/selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py +++ b/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) diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 70189f7755..9154a2f5e4 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/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", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index 3952730016..6972f3664f 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/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