Add HEMExpAuthority parameter and enhance hybrid experimental mode logging

common/params_keys.h

@@ -241,6 +241,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"HybridExpBias", {PERSISTENT, FLOAT, "0", "0", 1}},
     {"HybridExperimental", {PERSISTENT, BOOL, "0", "0", 1}},
     {"HybridVisionBrakeSensitivity", {PERSISTENT, FLOAT, "1", "1", 1}},
+    {"HEMExpAuthority", {CLEAR_ON_MANAGER_START, FLOAT, "0.5", "0.5", 2}},
     {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
     {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},

selfdrive/controls/lib/longitudinal_planner.py

@@ -7,6 +7,7 @@ from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.starpilot.common.model_versions import is_tinygrad_model_version
 from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode
@@ -134,6 +135,8 @@ VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN = 0.75
 VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45
 VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20
 PLANNER_SAFETY_WARNING_INTERVAL = 5.0
+HEM_STATUS_LOG_INTERVAL = 10.0
+HEM_AUTH_PUB_INTERVAL = 0.5
 VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30
 VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75
 VISION_LEAD_APPROACH_CONFIRM_TIME = 0.25
@@ -598,6 +601,10 @@ class LongitudinalPlanner:
     self.prev_experimental_mode = None
     self.experimental_release_accel_until = 0.0
     self.hybrid_controller = HybridExperimentalMode()
+    self._hem_status_log_t = 0.0
+    self._hem_logged_active = False
+    self._hem_auth_pub_t = 0.0
+    self._hem_params_memory = None
 
     if self.is_preap:
       try:
@@ -1886,6 +1893,38 @@ class LongitudinalPlanner:
       floor = min(LC_MERGE_ACCEL_BIAS, cruise_cap)
     return floor
 
+  def _log_hem_status(self, now_t, active, v_ego, a_chill, a_exp, a_fused):
+    if active != self._hem_logged_active:
+      self._hem_logged_active = active
+      self._hem_status_log_t = 0.0
+      print(f"[HEM] mode {'ON' if active else 'OFF'}")
+    if not active:
+      return
+    if now_t - self._hem_status_log_t < HEM_STATUS_LOG_INTERVAL:
+      return
+    self._hem_status_log_t = now_t
+    hc = self.hybrid_controller
+    print(
+      f"[HEM] v={v_ego:5.1f} chill={a_chill:6.2f} exp={a_exp:6.2f} "
+      + f"fused={a_fused:6.2f} auth={hc.exp_authority:4.2f} "
+      + f"w_vis={hc.last_w_vision:4.2f} {hc.last_regime}"
+      + (" stop" if hc.last_standstill else "")
+    )
+
+  def _publish_hem_authority(self, now_t):
+    if self._hem_params_memory is None:
+      try:
+        self._hem_params_memory = Params(memory=True)
+      except Exception:
+        return
+    if now_t - self._hem_auth_pub_t < HEM_AUTH_PUB_INTERVAL:
+      return
+    self._hem_auth_pub_t = now_t
+    try:
+      self._hem_params_memory.put("HEMExpAuthority", float(self.hybrid_controller.exp_authority))
+    except Exception:
+      pass
+
   def update(self, sm, starpilot_toggles):
     if self.is_preap:
       self._preap_param_frame += 1
@@ -2314,15 +2353,19 @@ class LongitudinalPlanner:
         a_exp=output_a_target_e2e,
         t_follow=effective_t_follow,
       )
+      self._log_hem_status(now_t, True, scene_v_ego, output_a_target_mpc, output_a_target_e2e, output_a_target)
+      self._publish_hem_authority(now_t)
       output_should_stop = output_should_stop_mpc or output_should_stop_e2e
     elif tinygrad_model and self.mode != 'acc' and self.generation != 'v9':
       output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
       output_should_stop_e2e = sm['modelV2'].action.shouldStop
       output_a_target = min(output_a_target_mpc, output_a_target_e2e)
       output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+      self._log_hem_status(now_t, False, scene_v_ego, output_a_target_mpc, output_a_target_e2e, output_a_target)
     else:
       output_a_target = output_a_target_mpc
       output_should_stop = output_should_stop_mpc
+      self._log_hem_status(now_t, False, scene_v_ego, output_a_target_mpc, float('nan'), output_a_target)
 
     comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
     vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego))

selfdrive/ui/lib/starpilot_status.py

@@ -33,6 +33,22 @@ def _is_hybrid_experimental_mode(state: UIState) -> bool:
   return bool(state.starpilot_toggles.get("hybrid_experimental_mode", False))
 
 
+def _hem_exp_authority(state: UIState) -> float:
+  """Exp/E2E authority weight (0.0 chill-only .. 1.0 exp-only) published by the planner."""
+  params_memory = getattr(state, "params_memory", None)
+  if params_memory is None:
+    return 0.5
+  try:
+    return float(params_memory.get("HEMExpAuthority") or 0.5)
+  except (TypeError, ValueError):
+    return 0.5
+
+
+def _hem_border_color(state: UIState) -> rl.Color:
+  """Blue when chill dominates the fusion, orange when experimental/vision dominates (like CEM)."""
+  return EXPERIMENTAL_COLOR if _hem_exp_authority(state) > 0.5 else HYBRID_EXPERIMENTAL_COLOR
+
+
 def _override_color_applies(state: UIState) -> bool:
   """Only gray the status when the active control mode is being overridden."""
   if state.status != UIStatus.OVERRIDE:
@@ -64,7 +80,7 @@ def get_border_color(state: UIState):
     return AOL_COLOR
   # Only color the border for CEM/experimental while actually enabled.
   if enabled and _is_hybrid_experimental_mode(state):
-    return HYBRID_EXPERIMENTAL_COLOR
+    return _hem_border_color(state)
   if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES:
     return CEM_OVERRIDE_COLOR
   if enabled and state.sm["selfdriveState"].experimentalMode:
@@ -76,7 +92,7 @@ def get_border_color(state: UIState):
 
 def get_path_edge_color(state: UIState):
   if state.sm["selfdriveState"].enabled and _is_hybrid_experimental_mode(state):
-    return HYBRID_EXPERIMENTAL_COLOR
+    return _hem_border_color(state)
   if state.conditional_status in CEM_ACTIVE_STATUSES:
     return EXPERIMENTAL_COLOR
   return get_border_color(state)
@@ -96,7 +112,7 @@ def get_screen_edge_color(state: UIState):
   # Keep the screen edge disengaged-blue when experimental mode is only the
   # requested longitudinal mode, not the active driving state.
   if enabled and _is_hybrid_experimental_mode(state):
-    return HYBRID_EXPERIMENTAL_COLOR
+    return _hem_border_color(state)
   if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES:
     return CEM_OVERRIDE_COLOR
   if enabled and state.sm["selfdriveState"].experimentalMode:

selfdrive/ui/tests/test_starpilot_status.py

@@ -3,6 +3,7 @@ from types import SimpleNamespace
 from openpilot.selfdrive.ui.lib.starpilot_status import (
   DISENGAGED_COLOR,
   ENGAGED_COLOR,
+  EXPERIMENTAL_COLOR,
   HYBRID_EXPERIMENTAL_COLOR,
   LONGITUDINAL_ONLY_COLOR,
   AOL_COLOR,
@@ -14,7 +15,8 @@ from openpilot.selfdrive.ui.lib.starpilot_status import (
 from openpilot.selfdrive.ui.ui_state import UIStatus
 
 
-def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=(), hybrid=False):
+def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=(), hybrid=False, hem_authority=None):
+  params_memory = {"HEMExpAuthority": f"{hem_authority:.3f}"} if hem_authority is not None else {}
   return SimpleNamespace(
     sm={
       "selfdriveState": SimpleNamespace(enabled=enabled, experimentalMode=False),
@@ -27,6 +29,7 @@ def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=()
     traffic_mode_enabled=False,
     conditional_status=0,
     starpilot_toggles={"hybrid_experimental_mode": hybrid},
+    params_memory=SimpleNamespace(get=lambda key, default=None: params_memory.get(key, default)),
   )
 
 
@@ -47,8 +50,24 @@ def test_lateral_active_colors_remain_unchanged():
   assert _rgb(get_border_color(_state())) == _rgb(DISENGAGED_COLOR)
 
 
-def test_hybrid_experimental_mode_uses_purple_border():
-  state = _state(enabled=True, hybrid=True)
+def test_hybrid_experimental_mode_uses_blue_border():
+  state = _state(enabled=True, lat_active=True, hybrid=True)
+
+  assert _rgb(get_border_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
+  assert _rgb(get_screen_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
+  assert _rgb(get_path_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
+
+
+def test_hybrid_experimental_mode_uses_orange_when_exp_dominates():
+  state = _state(enabled=True, lat_active=True, hybrid=True, hem_authority=0.8)
+
+  assert _rgb(get_border_color(state)) == _rgb(EXPERIMENTAL_COLOR)
+  assert _rgb(get_screen_edge_color(state)) == _rgb(EXPERIMENTAL_COLOR)
+  assert _rgb(get_path_edge_color(state)) == _rgb(EXPERIMENTAL_COLOR)
+
+
+def test_hybrid_experimental_mode_keeps_blue_when_chill_dominates():
+  state = _state(enabled=True, lat_active=True, hybrid=True, hem_authority=0.3)
 
   assert _rgb(get_border_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
   assert _rgb(get_screen_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR)
@@ -57,7 +76,7 @@ def test_hybrid_experimental_mode_uses_purple_border():
 
 def test_hybrid_experimental_mode_requires_enabled():
   assert _rgb(get_border_color(_state(hybrid=True))) == _rgb(DISENGAGED_COLOR)
-  assert _rgb(get_border_color(_state(enabled=True))) == _rgb(ENGAGED_COLOR)
+  assert _rgb(get_border_color(_state(enabled=True, lat_active=True))) == _rgb(ENGAGED_COLOR)
 
 
 def test_override_color_matches_active_control_mode():

starpilot/controls/lib/hybrid_experimental_mode.py

@@ -47,6 +47,11 @@ class HybridExperimentalMode:
     self.prev_a_target = 0.0
     self.exp_authority = 0.5
 
+    # Last-frame diagnostics surfaced to live logs
+    self.last_w_vision = 0.0
+    self.last_regime = "throttle"
+    self.last_standstill = False
+
     # User tuning
     self.HYBRID_EXP_BIAS = 0.2            # [-1.0, 1.0]
     self.VISION_BRAKE_SENSITIVITY = 1.2   # [0.0, 2.0]
@@ -60,6 +65,9 @@ class HybridExperimentalMode:
     """Seed target with actual vehicle acceleration on engagement to prevent torque bumps."""
     self.prev_a_target = float(a_ego) if np.isfinite(a_ego) else 0.0
     self.exp_authority = 0.5
+    self.last_w_vision = 0.0
+    self.last_regime = "throttle"
+    self.last_standstill = False
 
   def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float, t_follow=None, jerk_factor=None):
     self.HYBRID_EXP_BIAS = float(np.clip(exp_bias, -1.0, 1.0))
@@ -224,4 +232,7 @@ class HybridExperimentalMode:
     a_out = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta))
     if np.isfinite(a_out):
       self.prev_a_target = a_out
-    return self.prev_a_target
+    self.last_w_vision = w_vision
+    self.last_regime = "brake" if is_braking_phase else "throttle"
+    self.last_standstill = standstill_weight > 0.0
+    return self.prev_a_target