diff --git a/common/params_keys.h b/common/params_keys.h index 5d79d5772..6988b0055 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -241,6 +241,7 @@ inline static std::unordered_map 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}}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5a7999057..576e6855b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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)) diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index 4ad04e89f..66a1c8df3 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/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: diff --git a/selfdrive/ui/tests/test_starpilot_status.py b/selfdrive/ui/tests/test_starpilot_status.py index 28efd7ece..0aee98002 100644 --- a/selfdrive/ui/tests/test_starpilot_status.py +++ b/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(): diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index 1a4eb777d..1ba223159 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/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 \ No newline at end of file + 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