From ef5b526af78b32d8ae7d30874d1c6596d3372b61 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Fri, 17 Jul 2026 18:14:33 -0500 Subject: [PATCH] Traffic mode v2 --- common/params_keys.h | 2 +- selfdrive/controls/controlsd.py | 3 +- selfdrive/controls/lib/longcontrol.py | 8 ++- selfdrive/controls/tests/test_longcontrol.py | 52 ++++++++++++++++++++ system/manager/manager.py | 28 +++++++++++ system/manager/test/test_manager.py | 27 ++++++++++ 6 files changed, 116 insertions(+), 4 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index ac30fc0d3..4b4401332 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -616,7 +616,7 @@ inline static std::unordered_map keys = { {"TinygradUpdateAvailable", {PERSISTENT, BOOL, "0", "0", 1}}, {"ToyotaDoors", {PERSISTENT, BOOL, "1", "0", 0}}, {"TrailerLoad", {PERSISTENT, INT, "0", "0", 2}}, - {"TrafficFollow", {PERSISTENT, FLOAT, "0.5", "0.5", 2}}, + {"TrafficFollow", {PERSISTENT, FLOAT, "0.75", "0.75", 2}}, {"TrafficJerkAcceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"TrafficJerkDanger", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"TrafficJerkDeceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c66a87979..2aa38d129 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -325,7 +325,8 @@ class Controls: pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode) actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, - self.starpilot_toggles, has_lead=long_plan.hasLead), + self.starpilot_toggles, has_lead=long_plan.hasLead, + traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled), self.starpilot_toggles.max_desired_acceleration)) # Steering PID loop and lateral MPC diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 8575c842c..3d717deb1 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -396,7 +396,7 @@ class LongControl: ) return a_target * effective_gain - def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False): + def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False, traffic_mode_enabled=False): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] @@ -419,7 +419,11 @@ class LongControl: self.reset(preserve_stop_release=True) elif self.long_control_state == LongCtrlState.starting: - if starpilot_toggles.human_acceleration: + if traffic_mode_enabled: + # Traffic Mode has its own soft launch curve (a_target); bypass the raw + # StartAccel kick used elsewhere so launches stay within the traffic cap. + output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) + elif starpilot_toggles.human_acceleration: output_accel = a_target elif getattr(starpilot_toggles, "custom_accel_profile", False): output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 789320a49..92c52fdff 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -170,6 +170,58 @@ def test_starting_accel_obeys_a_target_cap_when_custom_profile_enabled(): assert output_accel == 0.1 +def test_starting_accel_obeys_a_target_cap_when_traffic_mode_enabled(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + # Large manually-tuned startAccel override (e.g. 3.5) should not fire a raw + # launch kick while Traffic Mode is active; output must track the soft a_target. + output_accel = lc.update( + active=True, + CS=CS, + a_target=1.10, + should_stop=False, + accel_limits=(-3.0, 4.0), + starpilot_toggles=make_toggles(startAccel=3.5), + traffic_mode_enabled=True, + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel == pytest.approx(1.10) + + +def test_starting_accel_uses_raw_start_accel_when_traffic_mode_disabled(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=1.10, + should_stop=False, + accel_limits=(-3.0, 4.0), + starpilot_toggles=make_toggles(startAccel=3.5), + traffic_mode_enabled=False, + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel == pytest.approx(3.5) + + def test_update_requires_sustained_moderate_positive_target_to_leave_stopping(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] diff --git a/system/manager/manager.py b/system/manager/manager.py index c745bd7a6..e8a87e269 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -62,6 +62,7 @@ STARPILOT_DEFAULTS_PARITY_MIGRATION_FLAG = Path("/data") / "starpilot_defaults_p STARPILOT_HUMANLIKE_DISABLE_MIGRATION_FLAG = Path("/data") / "starpilot_humanlike_disable_v1" STARPILOT_CLUSTER_OFFSET_MIGRATION_FLAG = Path("/data") / "starpilot_cluster_offset_v1" STARPILOT_TRAFFIC_SMOOTH_MIGRATION_FLAG = Path("/data") / "starpilot_traffic_smooth_v1" +STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG = Path("/data") / "starpilot_traffic_follow_v1" STARPILOT_PARAM_RENAME_MIGRATION_FLAG = Path("/data") / "starpilot_param_rename_v1" STARPILOT_PARAM_CANONICALIZATION_MIGRATION_FLAG = Path("/data") / "starpilot_param_canonicalization_v1" STARPILOT_PC_ROOT_MIGRATION_FLAG = Path("/data") / "starpilot_pc_root_v1" @@ -636,6 +637,32 @@ def migrate_traffic_mode_smooth_defaults(params: Params, params_cache: Params) - cloudlog.exception(f"Failed to write migration flag: {STARPILOT_TRAFFIC_SMOOTH_MIGRATION_FLAG}") +def migrate_traffic_follow_default(params: Params, params_cache: Params) -> None: + # TrafficFollow's initial smooth-mode default (0.5s) proved too tight in on-road + # testing (frequent closing-on-lead); raised to 0.75s. Rewrite persisted legacy + # 0.5 only; user-tuned values are preserved. + if STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG.exists(): + return + + raw_value = _read_raw_param_bytes(params, "TrafficFollow") + if raw_value: + try: + parsed_value = float(raw_value.decode("utf-8", errors="strict").strip()) + except Exception: + parsed_value = None + + if parsed_value is not None and abs(parsed_value - 0.5) < 1e-6: + params.put_float("TrafficFollow", 0.75) + params_cache.put_float("TrafficFollow", 0.75) + cloudlog.warning("Applied one-time TrafficFollow migration from 0.5 to 0.75") + + try: + STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG.parent.mkdir(parents=True, exist_ok=True) + STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG.write_text(f"{datetime.datetime.now(datetime.UTC).isoformat()}\n") + except Exception: + cloudlog.exception(f"Failed to write migration flag: {STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG}") + + def _read_raw_param_bytes(params: Params, key: str | bytes): try: path = params.get_param_path(key) @@ -856,6 +883,7 @@ def manager_init() -> None: migrate_disable_humanlike_defaults(params, params_cache) migrate_cluster_offset_default(params, params_cache) migrate_traffic_mode_smooth_defaults(params, params_cache) + migrate_traffic_follow_default(params, params_cache) last_timing = _log_boot_timing("manager_init", "starpilot_migrations", manager_init_start, last_timing) # set unset params to their default value diff --git a/system/manager/test/test_manager.py b/system/manager/test/test_manager.py index faa3f7d85..5723761fa 100644 --- a/system/manager/test/test_manager.py +++ b/system/manager/test/test_manager.py @@ -375,6 +375,33 @@ class TestManager: assert params.get("TrafficJerkAcceleration") == "80.0" assert params_cache.get("TrafficJerkAcceleration") is None + def test_migrate_traffic_follow_default_resets_legacy_default_only(self, tmp_path, monkeypatch): + monkeypatch.setattr(manager, "STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG", tmp_path / "starpilot_traffic_follow_v1") + + params = FileBackedFakeParams(tmp_path / "params", { + "TrafficFollow": 0.5, + }) + params_cache = FileBackedFakeParams(tmp_path / "cache", {}) + + manager.migrate_traffic_follow_default(params, params_cache) + + assert params.get("TrafficFollow") == "0.75" + assert params_cache.get("TrafficFollow") == "0.75" + assert manager.STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG.exists() + + def test_migrate_traffic_follow_default_preserves_custom_values(self, tmp_path, monkeypatch): + monkeypatch.setattr(manager, "STARPILOT_TRAFFIC_FOLLOW_MIGRATION_FLAG", tmp_path / "starpilot_traffic_follow_v1") + + params = FileBackedFakeParams(tmp_path / "params", { + "TrafficFollow": 1.2, + }) + params_cache = FileBackedFakeParams(tmp_path / "cache", {}) + + manager.migrate_traffic_follow_default(params, params_cache) + + assert params.get("TrafficFollow") == "1.2" + assert params_cache.get("TrafficFollow") is None + def test_migrate_traffic_mode_smooth_defaults_runs_once(self, tmp_path, monkeypatch): monkeypatch.setattr(manager, "STARPILOT_TRAFFIC_SMOOTH_MIGRATION_FLAG", tmp_path / "starpilot_traffic_smooth_v1")