diff --git a/common/params_keys.h b/common/params_keys.h index 6988b0055..908fa5e43 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -241,7 +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}}, + {"HEMExpDominant", {CLEAR_ON_MANAGER_START, BOOL, "0", "0", 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 576e6855b..3aaec6017 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1911,7 +1911,7 @@ class LongitudinalPlanner: + (" stop" if hc.last_standstill else "") ) - def _publish_hem_authority(self, now_t): + def _publish_hem_status(self, now_t): if self._hem_params_memory is None: try: self._hem_params_memory = Params(memory=True) @@ -1921,7 +1921,7 @@ class LongitudinalPlanner: return self._hem_auth_pub_t = now_t try: - self._hem_params_memory.put("HEMExpAuthority", float(self.hybrid_controller.exp_authority)) + self._hem_params_memory.put_bool("HEMExpDominant", bool(self.hybrid_controller.last_exp_dominant)) except Exception: pass @@ -2354,7 +2354,7 @@ class LongitudinalPlanner: 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) + self._publish_hem_status(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 diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index 66a1c8df3..a6cff451c 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -33,20 +33,24 @@ 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.""" +def _hem_exp_dominant(state: UIState) -> bool: + """True when the fused output tracks the E2E/vision input more than chill ACC. + + Uses the planner's per-frame comparison (HEMExpDominant) rather than the raw + exp_authority, which is inflated by the E2E Authority Bias baseline. + """ params_memory = getattr(state, "params_memory", None) if params_memory is None: - return 0.5 + return False try: - return float(params_memory.get("HEMExpAuthority") or 0.5) + return bool(int(params_memory.get("HEMExpDominant") or 0)) except (TypeError, ValueError): - return 0.5 + return False 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 + return EXPERIMENTAL_COLOR if _hem_exp_dominant(state) else HYBRID_EXPERIMENTAL_COLOR def _override_color_applies(state: UIState) -> bool: diff --git a/selfdrive/ui/tests/test_starpilot_status.py b/selfdrive/ui/tests/test_starpilot_status.py index 0aee98002..287b73a8f 100644 --- a/selfdrive/ui/tests/test_starpilot_status.py +++ b/selfdrive/ui/tests/test_starpilot_status.py @@ -15,8 +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, hem_authority=None): - params_memory = {"HEMExpAuthority": f"{hem_authority:.3f}"} if hem_authority is not None else {} +def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=(), hybrid=False, hem_exp_dominant=None): + params_memory = {"HEMExpDominant": b"1"} if hem_exp_dominant else {} return SimpleNamespace( sm={ "selfdriveState": SimpleNamespace(enabled=enabled, experimentalMode=False), @@ -59,7 +59,7 @@ def test_hybrid_experimental_mode_uses_blue_border(): def test_hybrid_experimental_mode_uses_orange_when_exp_dominates(): - state = _state(enabled=True, lat_active=True, hybrid=True, hem_authority=0.8) + state = _state(enabled=True, lat_active=True, hybrid=True, hem_exp_dominant=True) assert _rgb(get_border_color(state)) == _rgb(EXPERIMENTAL_COLOR) assert _rgb(get_screen_edge_color(state)) == _rgb(EXPERIMENTAL_COLOR) @@ -67,7 +67,7 @@ def test_hybrid_experimental_mode_uses_orange_when_exp_dominates(): def test_hybrid_experimental_mode_keeps_blue_when_chill_dominates(): - state = _state(enabled=True, lat_active=True, hybrid=True, hem_authority=0.3) + state = _state(enabled=True, lat_active=True, hybrid=True, hem_exp_dominant=False) assert _rgb(get_border_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) assert _rgb(get_screen_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index 1ba223159..c4618f3c2 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -51,6 +51,7 @@ class HybridExperimentalMode: self.last_w_vision = 0.0 self.last_regime = "throttle" self.last_standstill = False + self.last_exp_dominant = False # User tuning self.HYBRID_EXP_BIAS = 0.2 # [-1.0, 1.0] @@ -68,6 +69,7 @@ class HybridExperimentalMode: self.last_w_vision = 0.0 self.last_regime = "throttle" self.last_standstill = False + self.last_exp_dominant = 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)) @@ -235,4 +237,9 @@ class HybridExperimentalMode: self.last_w_vision = w_vision self.last_regime = "brake" if is_braking_phase else "throttle" self.last_standstill = standstill_weight > 0.0 + # Border hint: True when the fused output tracks the E2E/vision input more + # closely than chill ACC. Setting-independent, unlike raw exp_authority which + # includes the E2E Authority Bias baseline. + out = self.prev_a_target + self.last_exp_dominant = abs(out - a_exp) < abs(out - a_chill) - 0.03 return self.prev_a_target diff --git a/starpilot/controls/tests/test_hybrid_experimental_mode.py b/starpilot/controls/tests/test_hybrid_experimental_mode.py index 36c4d39db..9daf986c4 100644 --- a/starpilot/controls/tests/test_hybrid_experimental_mode.py +++ b/starpilot/controls/tests/test_hybrid_experimental_mode.py @@ -259,4 +259,26 @@ def test_vision_stop_uses_emergency_brake_ramp(): controller = make_controller(prev=0.0) model = FakeModel(velocity=list(np.linspace(20.0, 0.2, 33)), position=list(np.linspace(0.0, 40.0, 33))) out = [controller.update(20.0, 25.0, FakeLead(status=False), model, 0.0, -3.5) for _ in range(5)] - assert out[-1] <= -2.5, f"Vision stop should ramp braking fast, got {out}" \ No newline at end of file + assert out[-1] <= -2.5, f"Vision stop should ramp braking fast, got {out}" + + +def test_last_exp_dominant_true_when_vision_braking(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=list(np.linspace(20.0, 0.2, 33)), position=list(np.linspace(0.0, 40.0, 33))) + run(controller, v_ego=20.0, v_cruise=25.0, lead=FakeLead(status=False), + model=model, a_chill=0.0, a_exp=-3.0, frames=60) + assert controller.last_exp_dominant + + +def test_last_exp_dominant_false_when_chill_brakes_for_lead(): + controller = make_controller(prev=0.0) + lead = FakeLead(status=True, d_rel=5.0, v_lead=0.0) + model = FakeModel(velocity=[15.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + run(controller, v_ego=15.0, v_cruise=20.0, lead=lead, model=model, a_chill=-2.5, a_exp=1.0, frames=40) + assert not controller.last_exp_dominant + + +def test_last_exp_dominant_false_at_neutral_cruise(): + controller = make_controller(prev=0.0) + run(controller, a_chill=0.0, a_exp=0.05, frames=10) + assert not controller.last_exp_dominant \ No newline at end of file