From c241b7f9e5481aeda76dcaaa116a0656a323c380 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 30 Jun 2026 21:08:15 -0700 Subject: [PATCH] refactor(long): --- common/params_keys.h | 4 +- selfdrive/controls/controlsd.py | 1 - .../test_longitudinal_smoothing_wiring.py | 10 +-- .../selfdrive/controls/controlsd_ext.py | 5 -- .../selfdrive/controls/lib/longcontrol_ext.py | 43 ---------- .../lib/radar_distance/radar_distance.py | 8 +- .../tests/test_radar_distance.py | 7 ++ .../lib/tests/test_longcontrol_ext.py | 81 ------------------- sunnypilot/sunnylink/params_metadata.json | 4 +- sunnypilot/sunnylink/settings_ui.json | 4 +- .../settings_ui_src/pages/cruise.yaml | 6 +- 11 files changed, 26 insertions(+), 147 deletions(-) delete mode 100644 sunnypilot/selfdrive/controls/lib/longcontrol_ext.py delete mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py diff --git a/common/params_keys.h b/common/params_keys.h index 3e72e29ef1..9955802e75 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -243,8 +243,8 @@ inline static std::unordered_map keys = { // Radar Distance: hold a lead through radar flicker/dropout so the MPC doesn't lose+regain it {"RadarDistance", {PERSISTENT | BACKUP, BOOL, "0"}}, - // Stop Settle Soften: ease the final brake-pressure build below walking speed for a smoother stop - {"StopSettleSoften", {PERSISTENT | BACKUP, BOOL, "0"}}, + // Stop Gap Bias: stop a bit farther back from a stopped lead so it doesn't crawl in too close + {"StopGapBias", {PERSISTENT | BACKUP, BOOL, "0"}}, // sunnypilot model params {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 72d6002e46..c242871125 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -55,7 +55,6 @@ class Controls(ControlsExt): self.calibrated_pose: Pose | None = None self.LoC = LongControl(self.CP, self.CP_SP) - self.LoC = ControlsExt.initialize_longitudinal_control(self, self.LoC) self.VM = VehicleModel(self.CP) self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: diff --git a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py index d99035d3ff..9cf658c4f5 100644 --- a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py +++ b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py @@ -13,6 +13,7 @@ def test_smoothing_params_default_off(): assert re.search(r'"AccelPersonalityEnabled", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys) assert re.search(r'"RadarDistance", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys) + assert re.search(r'"StopGapBias", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys) def test_longitudinal_smoothing_stays_planner_side(): @@ -43,12 +44,11 @@ def test_dec_model_stop_target_not_reintroduced(): def test_long_feature_gates(): - # The surviving opt-in long features all default OFF (byte-stock until enabled + on-road verified): - # AccelController jerk-limiter, RadarDistance lead-smoother + stop-gap bias. + # The surviving opt-in long features default OFF (byte-stock until enabled + on-road verified): + # AccelController jerk-limiter and RadarDistance lead-smoother are module flags; the StopGapBias stop-gap + # rides on a param (its default is checked in test_smoothing_params_default_off). from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import JERK_LIMIT_ENABLED - from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \ - LEAD_SMOOTH_ENABLED, STOP_GAP_BIAS_ENABLED + from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import LEAD_SMOOTH_ENABLED assert JERK_LIMIT_ENABLED is False assert LEAD_SMOOTH_ENABLED is False - assert STOP_GAP_BIAS_ENABLED is False diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index e614393130..c8d054243f 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -17,7 +17,6 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0 -from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol_ext import LongControlExt class ControlsExt(ModelStateBase): @@ -35,10 +34,6 @@ class ControlsExt(ModelStateBase): self.sm_services_ext = ['radarState', 'selfdriveStateSP'] self.pm_services_ext = ['carControlSP'] - def initialize_longitudinal_control(self, _loc): - # The softener self-gates on the StopSettleSoften param (read live), so it is byte-stock when off. - return LongControlExt(self.CP, self.CP_SP, self.params) - def initialize_lateral_control(self, lac, CI, dt): enforce_torque_control = self.params.get_bool("EnforceTorqueControl") torque_versions = self.params.get("TorqueControlTune") diff --git a/sunnypilot/selfdrive/controls/lib/longcontrol_ext.py b/sunnypilot/selfdrive/controls/lib/longcontrol_ext.py deleted file mode 100644 index c34a3ba8d0..0000000000 --- a/sunnypilot/selfdrive/controls/lib/longcontrol_ext.py +++ /dev/null @@ -1,43 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. - -Eases the final brake-pressure build as the car settles to a stop. In the stopping state the output ramps -toward the hold accel at a fixed rate; below SETTLE_V_BP[-1] this scales that per-step build down so the -last fraction of a m/s tapers in instead of clamping on (approach braking is untouched). This is the one -regime where the output is intentionally softer than stock. Gated by the StopSettleSoften param (read live). -""" - -import numpy as np - -from openpilot.common.params import Params -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState - -SETTLE_V_BP = [0.3, 1.2, 2.5] # m/s: the build step is eased below the top point, full rate at/above it -SETTLE_SCALE_V = [0.25, 0.6, 1.0] # fraction of the per-step brake build applied across the band - - -class LongControlExt(LongControl): - def __init__(self, CP, CP_SP, params=None): - super().__init__(CP, CP_SP) - self._params = params or Params() - self._frame = 0 - self._settle_soft = self._params.get_bool("StopSettleSoften") - - def update(self, active, CS, a_target, should_stop, accel_limits): - if self._frame % int(1.0 / DT_CTRL) == 0: - self._settle_soft = self._params.get_bool("StopSettleSoften") - self._frame += 1 - - prev_accel = self.last_output_accel - accel = super().update(active, CS, a_target, should_stop, accel_limits) - # Soften only while the stop ramp is adding brake (accel going more negative) from an already-coasting - # output, so we shrink the build step without ever turning it into throttle or reducing held brake. - if self._settle_soft and self.long_control_state == LongCtrlState.stopping and prev_accel <= 0.0 and accel < prev_accel: - scale = float(np.interp(CS.vEgo, SETTLE_V_BP, SETTLE_SCALE_V)) - accel = float(np.clip(prev_accel + (accel - prev_accel) * scale, accel_limits[0], accel_limits[1])) - self.last_output_accel = accel - return self.last_output_accel diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index eb8f31c57c..bebab55cdc 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -49,7 +49,7 @@ LEAD_SMOOTH_HOLD = 20 # frames (~1s): keep smoothing through brief chur # Stop-gap bias: near a (near-)stopped lead at low speed, report dRel up to STOP_GAP_BIAS_M closer so the MPC # runs its own smooth stop but terminates that much farther back (stock crawl-creeps to ~2m). Monotone (closer # => brake >= stock). Ramps in over the regime edge and out as the lead moves (no step, releases on launch). -STOP_GAP_BIAS_ENABLED = False +# Gated by the StopGapBias param, independent of the rest of the controller. STOP_GAP_BIAS_M = 2.0 # m: max dRel reduction = added standstill gap STOP_BIAS_VEGO = 8.0 # m/s: only below this ego speed STOP_BIAS_VLEAD = 1.5 # m/s: only behind a (near-)stopped lead; ramps out as vLead rises to this @@ -224,7 +224,7 @@ class RadarDistanceController: self._frame = 0 self._v_ego = 0.0 self._enabled = self._params.get_bool("RadarDistance") - self._stop_gap_bias_enabled = STOP_GAP_BIAS_ENABLED + self._stop_gap_bias_enabled = self._params.get_bool("StopGapBias") self._lead_smooth_enabled = LEAD_SMOOTH_ENABLED self._one = _LeadHold() self._two = _LeadHold() @@ -237,6 +237,7 @@ class RadarDistanceController: self._one.reset() self._two.reset() self._enabled = enabled + self._stop_gap_bias_enabled = self._params.get_bool("StopGapBias") def update(self, sm) -> None: if self._frame % int(1. / DT_MDL) == 0: @@ -278,7 +279,8 @@ class RadarDistanceController: def smooth_radarstate(self, radarstate): self._stability.update(radarstate.leadOne, self._v_ego) # telemetry, runs every cycle if not self._enabled: - return radarstate + one_b = self._stop_gap_bias(radarstate.leadOne) # stop-gap bias works standalone; else passthrough + return radarstate if one_b is radarstate.leadOne else _RadarStateProxy(one_b, radarstate.leadTwo) one = self._one.step(radarstate.leadOne) two = self._two.step(radarstate.leadTwo) if self._v_ego < LOW_SPEED_PASSTHROUGH_V: diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py index 7d5e6d37ee..6ef06d5f5d 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py @@ -226,6 +226,13 @@ def test_stop_bias_via_smooth_radarstate_low_speed(): out = _biased_ctrl().smooth_radarstate(rs(lead(dRel=8.0, vLead=0.0, vRel=-2.0))) assert out.leadOne.dRel < 8.0 # biased proxy returned at low speed +def test_stop_bias_independent_of_radar_distance(): + c = ctrl(enabled=False) # RadarDistance off ... + c._stop_gap_bias_enabled = True # ... but StopGapBias on + c._v_ego = 2.0 + out = c.smooth_radarstate(rs(lead(dRel=8.0, vLead=0.0, vRel=-2.0))) + assert out.leadOne.dRel < 8.0 # stop-gap still applies standalone + # --- speed-gap bias (wider gap at speed) ------------------------------------- diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py b/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py deleted file mode 100644 index 832a72fc2a..0000000000 --- a/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py +++ /dev/null @@ -1,81 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" - -from types import SimpleNamespace - -from openpilot.selfdrive.controls.lib.longcontrol import LongControl -from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol_ext import LongControlExt, SETTLE_V_BP - - -class FakeParams: - def __init__(self, store=None): - self.store = dict(store or {}) - - def get_bool(self, key): - return bool(self.store.get(key, False)) - - -def _CP(): - tuning = SimpleNamespace(kpBP=[0.0], kpV=[1.0], kiBP=[0.0], kiV=[0.0]) - return SimpleNamespace(longitudinalTuning=tuning, stopAccel=-2.0, stoppingDecelRate=0.8, - startAccel=0.0, vEgoStarting=0.5, startingState=False) - - -def _CS(v_ego, brake=False, standstill=False): - return SimpleNamespace(vEgo=v_ego, aEgo=0.0, brakePressed=brake, - cruiseState=SimpleNamespace(standstill=standstill)) - - -CP_SP = SimpleNamespace(enableGasInterceptor=False) -LIMITS = (-3.0, 2.0) - - -def _stock(): - return LongControl(_CP(), CP_SP) - - -def _ext(enabled=True): - return LongControlExt(_CP(), CP_SP, params=FakeParams({"StopSettleSoften": enabled})) - - -def _run(c, v_ego, frames): - return [c.update(True, _CS(v_ego), 0.0, True, LIMITS) for _ in range(frames)] - - -def test_disabled_matches_stock(): - assert _run(_ext(enabled=False), 0.3, 30) == _run(_stock(), 0.3, 30) # off => byte-stock - - -def test_low_speed_softens_brake_build(): - soft = _run(_ext(), 0.3, 30) - stock = _run(_stock(), 0.3, 30) - assert soft[-1] > stock[-1] # softer (less brake) at the final settle - assert all(s >= b - 1e-9 for s, b in zip(soft, stock, strict=True)) # never harder than stock anywhere - - -def test_high_speed_unchanged(): - v = SETTLE_V_BP[-1] + 0.5 # above the band => full stock rate - assert _run(_ext(), v, 20) == _run(_stock(), v, 20) - - -def test_never_adds_throttle_or_releases_brake(): - c = _ext() - prev = c.last_output_accel - for _ in range(40): - a = c.update(True, _CS(0.3), 0.0, True, LIMITS) - assert a <= 1e-9 # never turns the stop into throttle - assert a <= prev + 1e-9 # only ever builds brake, never releases it - prev = a - - -def test_only_acts_in_stopping_state(): - # moving, not stopping => pid state => identical to stock - ext = _ext() - stock = _stock() - out_ext = [ext.update(True, _CS(15.0), -0.5, False, LIMITS) for _ in range(10)] - out_stock = [stock.update(True, _CS(15.0), -0.5, False, LIMITS) for _ in range(10)] - assert out_ext == out_stock diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index 8bfdfcbd96..af0d91750f 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -1259,9 +1259,9 @@ "title": "[TIZI/TICI only] Standstill Timer", "description": "Show a timer on the HUD when the car is at a standstill." }, - "StopSettleSoften": { + "StopGapBias": { "title": "Smooth Stop", - "description": "Eases the brake pressure as the car settles the last few mph to a stop, so it stops with a soft taper instead of a firm catch. Only affects the final crawl, not normal braking." + "description": "Stops a touch farther back from a stopped lead at low speed, so sunnypilot settles smoothly instead of crawling in close. Braking is never reduced below stock." }, "SubaruStopAndGo": { "title": "Subaru Stop and Go", diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 8a4512380e..fc171da578 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -614,10 +614,10 @@ ] }, { - "key": "StopSettleSoften", + "key": "StopGapBias", "widget": "toggle", "title": "Smooth Stop", - "description": "Eases the brake pressure as the car settles the last few mph to a stop, so it stops with a soft taper instead of a firm catch. Only affects the final crawl, not normal braking.", + "description": "Stops a touch farther back from a stopped lead at low speed, so sunnypilot settles smoothly instead of crawling in close. Braking is never reduced below stock.", "visibility": [ { "type": "capability", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 02cae7c084..1f8b21f97c 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -34,11 +34,11 @@ sections: - $ref: '#/macros/longitudinal' enablement: - $ref: '#/macros/longitudinal' - - key: StopSettleSoften + - key: StopGapBias widget: toggle title: Smooth Stop - description: Eases the brake pressure as the car settles the last few mph to a stop, so it stops with - a soft taper instead of a firm catch. Only affects the final crawl, not normal braking. + description: Stops a touch farther back from a stopped lead at low speed, so sunnypilot settles smoothly + instead of crawling in close. Braking is never reduced below stock. visibility: - $ref: '#/macros/longitudinal' enablement: