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/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/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 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