Ford angle-mode lateral: on-device auto-calibration gauges

bluepilot/selfdrive/car/bp_card_publisher.py

@@ -104,6 +104,9 @@ def _refresh_settings_cache() -> dict:
     "bmsMinimumSpeedToPauseLaneChange": _get_int(p, "BlinkerMinLateralControlSpeed", 20),
     "bmsShowLateralControlMode":       _get_bool(p, "BpShowLateralControl"),
     # --- Angle Tuning ---
+    # bmsAngleAutoCalibrate / bmsAngleAutoCalState are intentionally NOT here: they are
+    # ground truth from the live controller (set below from CI.CC every publish) — a
+    # param-snapshot copy would be a second source of truth that is silently overwritten.
     "bmsLowSpeedAdjustmentFactor":     _get_float(p, "FordLowSpeedFactor_ang", 1.0),
     "bmsHighSpeedAdjustmentFactor":    _get_float(p, "FordHighSpeedFactor_ang", 1.0),
     "bmsLaneChangeFactorHighAngle":    _get_float(p, "lane_change_factor_high_ang", 1.0),
@@ -131,6 +134,7 @@ def publish_controller_state_bp(CI, pm):
     cs_bp.curvatureDeviationLimited = getattr(CI.CC, "curvatureDeviationLimited", False)
     cs_bp.humanTurnLateralPaused = bool(getattr(CI.CC, "humanTurnLateralPaused", False))
     cs_bp.stallBlipActive = bool(getattr(CI.CC, "stallBlipActive", False))
+    cs_bp.angleSaturated = bool(getattr(CI.CC, "bp_angle_saturated", False))
     # BluePilot: mode the controller actually ran, straight off the car controller (not Params).
     if getattr(CI.CC, "disable_BP_lat_UI", True):
       cs_bp.activeLateralMode = structs.ControllerStateBP.LateralMode.openpilot
@@ -152,6 +156,17 @@ def publish_controller_state_bp(CI, pm):
       for field, value in _settings_cache.items():
         setattr(cs_bp, field, value)
 
+    # BluePilot: auto-cal fields are GROUND TRUTH from the live controller, not the param
+    # snapshot — a device once had params armed while the controller ran disarmed, and the
+    # param-sourced telemetry made that undiagnosable from logs. bp_autocal_status carries
+    # the controller's own view (armed/evidence/nudges, "off", "locked", or an error).
+    cc = CI.CC
+    if hasattr(cc, "autocal_enabled"):
+      cs_bp.bmsAngleAutoCalibrate = bool(cc.autocal_enabled)
+    status = getattr(cc, "bp_autocal_status", "")
+    if status:
+      cs_bp.bmsAngleAutoCalState = str(status)
+
     # BluePilot: fingerprint info -- plain attribute reads on CarParams, no Params round-trip
     # needed, so no caching required (fingerprint never changes after startup).
     CP = getattr(CI, "CP", None)

bluepilot/ui/widgets/debug/autocal_bars.py

@@ -0,0 +1,153 @@
+"""Auto-calibration band gauges for the on-device lateral debug graph.
+
+Two phone-battery-style vertical gauges — BLUE = low band (curves under 30 mph),
+RED = high band (over 60 mph) — in the strip left of the plot. Each fills bottom-up as
+that band charges toward calibrated: partway while collecting evidence, up while a step
+is being checked, and full once the band is calibrated. A full band stays full; when the
+whole calibration locks the gauges become padlocks. Hidden while auto-calibration is off.
+
+Status is parsed only when it changes (~1 Hz), off a conflated socket, so the widget is a
+handful of rectangles per frame — cheap on the UI process.
+"""
+import json
+
+import pyray as rl
+
+from openpilot.system.ui.widgets import Widget
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+
+try:
+  import cereal.messaging as _messaging
+  _STATUS_SOCK = _messaging.sub_sock("controllerStateBP", conflate=True, timeout=0)
+except Exception:  # PC/dev hosts without cereal sockets: gauges stay hidden
+  _messaging = None
+  _STATUS_SOCK = None
+
+_BLUE = (77, 163, 255)
+_RED = (230, 88, 88)
+_BATT_W, _BATT_H = 22, 44
+_NUB_W, _NUB_H = 10, 4
+_LABEL_GAP = 4
+_LABEL_FONT = 12
+_ROW_PITCH = _NUB_H + _BATT_H + _LABEL_GAP + _LABEL_FONT + 10
+
+# Fill is a single monotonic "progress toward calibrated" so the battery never fills then
+# un-fills as the anchor moves between phases. Evidence alone tops out below full; only a
+# calibrated band reads full (and latches there — see _fill's hysteresis).
+_COLLECT_TOP = 0.7   # evidence progress maps into [0, _COLLECT_TOP]
+_VERIFY_TOP = 0.9    # a step being checked maps into [_COLLECT_TOP, _VERIFY_TOP]
+
+
+def poll_status() -> str | None:
+  """Newest bmsAngleAutoCalState string, or None when nothing new / no socket."""
+  if _STATUS_SOCK is None:
+    return None
+  msg = _messaging.recv_one_or_none(_STATUS_SOCK)
+  if msg is None:
+    return None
+  return str(msg.controllerStateBP.bmsAngleAutoCalState)
+
+
+def band_fill(band_st: dict, was_full: bool) -> tuple[float, bool]:
+  """Monotonic 'progress toward calibrated' fill (0..1) for one band, plus the new
+  latched-full state. Pure, so it is unit-tested directly. 'good' latches full and stays
+  full through borderline good/propose flicker; a fresh 'verify' step or a 'collect' reset
+  drops it. Evidence alone (collect) tops out below full — the fill-then-un-fill bug was
+  showing three different metrics per phase; this is one."""
+  ph = band_st.get("ph", "collect")
+  full = was_full
+  if ph == "good":
+    full = True
+  elif ph in ("verify", "collect"):
+    full = False
+  if full:
+    return 1.0, full
+  if ph == "verify":
+    need = max(float(band_st.get("vneed", 6)), 1e-6)
+    return _COLLECT_TOP + (_VERIFY_TOP - _COLLECT_TOP) * min(1.0, float(band_st.get("vw", 0.0)) / need), full
+  if ph == "propose":
+    return _COLLECT_TOP, full
+  need = max(float(band_st.get("need", 10)), 1e-6)  # collect
+  return _COLLECT_TOP * min(1.0, float(band_st.get("w", 0.0)) / need), full
+
+
+class AutoCalBars(Widget):
+  """Hidden whenever auto-calibration is off / status is not renderable."""
+
+  WIDTH = _BATT_W + 8
+  HEIGHT = 2 * _ROW_PITCH
+
+  def __init__(self):
+    super().__init__()
+    self._raw = None
+    self._st = None                        # dict (armed JSON) | "locked" | None
+    self._full = {"low": False, "high": False}  # latched-full state per band (hysteresis)
+
+  def update_status(self, status: str | None):
+    if status is None or status == self._raw:
+      return
+    self._raw = status
+    if status.startswith("{"):
+      try:
+        d = json.loads(status)
+        self._st = d if ("low" in d and "high" in d) else None
+      except ValueError:
+        self._st = None
+    elif status == "locked":
+      self._st = "locked"
+    else:
+      self._st = None
+
+  @property
+  def active(self) -> bool:
+    return self._st is not None
+
+  @property
+  def locked(self) -> bool:
+    return self._st == "locked"
+
+  def _fill(self, band: str) -> float:
+    fill, self._full[band] = band_fill(self._st.get(band, {}), self._full[band])
+    return fill
+
+  def _draw_battery(self, x: int, y: int, fill: float, rgb):
+    col = rl.Color(rgb[0], rgb[1], rgb[2], 235)
+    body_y = int(y + _NUB_H)
+    rl.draw_rectangle(x + (_BATT_W - _NUB_W) // 2, int(y), _NUB_W, _NUB_H, rl.Color(150, 155, 165, 200))
+    body = rl.Rectangle(x, body_y, _BATT_W, _BATT_H)
+    rl.draw_rectangle_rounded(body, 0.25, 6, rl.Color(38, 42, 52, 200))
+    fh = int((_BATT_H - 4) * fill)
+    if fh > 0:
+      rl.draw_rectangle(x + 2, body_y + _BATT_H - 2 - fh, _BATT_W - 4, fh, col)
+    rl.draw_rectangle_rounded_lines_ex(body, 0.25, 6, 1.5, rl.Color(150, 155, 165, 200))
+
+  def _draw_lock(self, x: int, y: int, rgb):
+    col = rl.Color(rgb[0], rgb[1], rgb[2], 235)
+    body_w, body_h = _BATT_W, 26
+    body_y = int(y + _NUB_H + _BATT_H - body_h)
+    # shackle: a half-ring sitting on the body
+    cx = x + _BATT_W / 2
+    rl.draw_ring(rl.Vector2(cx, body_y), 6.0, 9.0, 180.0, 360.0, 16, col)
+    body = rl.Rectangle(x, body_y, body_w, body_h)
+    rl.draw_rectangle_rounded(body, 0.3, 6, col)
+    rl.draw_circle(int(cx), body_y + body_h // 2, 2.5, rl.Color(20, 22, 28, 255))  # keyhole
+
+  def _render(self, rect: rl.Rectangle):
+    if self._st is None:
+      return
+    font = gui_app.font(FontWeight.NORMAL)
+    y = rect.y
+    x = int(rect.x)
+    for band, rgb, label in (("low", _BLUE, "<30"), ("high", _RED, ">60")):
+      if self.locked:
+        self._draw_lock(x, int(y), rgb)
+      else:
+        try:
+          self._draw_battery(x, int(y), self._fill(band), rgb)
+        except (TypeError, ValueError, KeyError):
+          pass
+      rl.draw_text_ex(font, label,
+                      rl.Vector2(x + (_BATT_W - _LABEL_FONT * len(label) * 0.55) / 2,
+                                 y + _NUB_H + _BATT_H + _LABEL_GAP),
+                      _LABEL_FONT, 0, rl.Color(160, 165, 175, 210))
+      y += _ROW_PITCH

bluepilot/ui/widgets/debug/tests/test_autocal_bars.py

@@ -0,0 +1,48 @@
+"""Battery-gauge fill logic (bluepilot/ui/widgets/debug/autocal_bars.band_fill).
+
+Regression for the 'fills full then un-fills' bug: the fill is now one monotonic
+progress-toward-calibrated metric, and a calibrated ('good') band latches full.
+"""
+from bluepilot.ui.widgets.debug.autocal_bars import band_fill, _COLLECT_TOP, _VERIFY_TOP
+
+
+def test_collect_never_reads_full():
+  # Evidence alone must top out below full — the old bug filled to 1.0 on evidence, then
+  # dropped when the phase moved on.
+  fill, full = band_fill({"ph": "collect", "w": 100, "need": 10}, False)
+  assert fill <= _COLLECT_TOP and not full
+
+
+def test_collect_progresses():
+  low, _ = band_fill({"ph": "collect", "w": 0, "need": 10}, False)
+  mid, _ = band_fill({"ph": "collect", "w": 5, "need": 10}, False)
+  assert low == 0.0 and 0.0 < mid < _COLLECT_TOP
+
+
+def test_good_is_full_and_latches():
+  fill, full = band_fill({"ph": "good"}, False)
+  assert fill == 1.0 and full
+
+
+def test_full_survives_propose_flicker():
+  # Once full, a borderline flip to 'propose' must NOT drop the battery.
+  fill, full = band_fill({"ph": "propose", "t": 1.1}, True)
+  assert fill == 1.0 and full
+
+
+def test_verify_drops_from_full_into_band():
+  # A genuine new step being checked drops out of full, into [_COLLECT_TOP, _VERIFY_TOP].
+  fill, full = band_fill({"ph": "verify", "vw": 0, "vneed": 6}, True)
+  assert not full and _COLLECT_TOP <= fill <= _VERIFY_TOP
+
+
+def test_verify_progresses_within_band():
+  lo, _ = band_fill({"ph": "verify", "vw": 0, "vneed": 6}, False)
+  hi, _ = band_fill({"ph": "verify", "vw": 6, "vneed": 6}, False)
+  assert lo == _COLLECT_TOP and abs(hi - _VERIFY_TOP) < 1e-9
+
+
+def test_collect_reset_drops_full():
+  # Evidence lost (back to collect) un-latches full.
+  fill, full = band_fill({"ph": "collect", "w": 0, "need": 10}, True)
+  assert not full and fill == 0.0

cereal/custom.capnp

@@ -558,6 +558,10 @@ struct ControllerStateBP @0xcd96dafb67a082d0 {
     curvature @1;
     angle @2;
   }
+
+  bmsAngleAutoCalibrate @55 :Bool;  # FordAngleAutoCal toggle state
+  bmsAngleAutoCalState @56 :Text;  # live controller status (bp_autocal_status): "off"/"locked"/"reset" or armed JSON
+  angleSaturated @57 :Bool;  # angle mode: PSCM authority limit or DBC clamp modified this frame's command
 }
 
 struct CarStateBP @0xb057204d7deadf3f {

common/params_keys.h

@@ -310,6 +310,13 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"FordLowSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
     {"FordHighSpeedFactor_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
     {"FordHighSpeedDampening_ang", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
+    {"FordAngleAutoCal", {PERSISTENT | BACKUP, BOOL, "0"}},           // one-time auto-calibration of the angle speed factors
+    {"FordAngleAutoCalState", {PERSISTENT | BACKUP, STRING, ""}},     // "" = collecting; JSON = evidence; "locked"/"done ..." = finished
+    {"FordAngleAutoCalError", {PERSISTENT, STRING, ""}},              // diagnostics only — separate channel so an error can never clobber evidence
+    {"FordAngleAutoCalReset", {PERSISTENT, BOOL, "0"}},               // erase calibration memory: controller wipes evidence + resets factors to 1.00, then clears this
+    {"FordAngleAutoCalLock", {PERSISTENT | BACKUP, BOOL, "1"}},       // on: calibration freezes when stable (default); off: never locks, keeps adapting — turning off an existing lock resumes it
+    {"FordAngleSmoothing", {PERSISTENT | BACKUP, BOOL, "1"}},         // anti-weave smoothing of the angle command path
+    {"FordAngleSmoothStrength", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // 0.0=minimal .. 1.0=tuned default .. 1.5=strong
     {"BPLateralSchemeParamsMigratedV1", {PERSISTENT | BACKUP, STRING, "0"}},
 
     {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}},