From 1cae2e14b9f0292a5223e8d261dd0ea1a716c437 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 30 Jun 2026 12:21:06 -0700 Subject: [PATCH] feat(long): tfllow --- common/params_keys.h | 3 + selfdrive/controls/controlsd.py | 1 + .../selfdrive/controls/controlsd_ext.py | 5 ++ .../selfdrive/controls/lib/longcontrol_ext.py | 43 ++++++++++ .../lib/radar_distance/radar_distance.py | 24 ++++++ .../tests/test_radar_distance.py | 47 ++++++++++- .../lib/tests/test_longcontrol_ext.py | 81 +++++++++++++++++++ sunnypilot/sunnylink/params_metadata.json | 4 + sunnypilot/sunnylink/settings_ui.json | 22 ++++- .../settings_ui_src/pages/cruise.yaml | 12 ++- 10 files changed, 238 insertions(+), 4 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/longcontrol_ext.py create mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py diff --git a/common/params_keys.h b/common/params_keys.h index 7e91d9ff0d..3e72e29ef1 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -243,6 +243,9 @@ 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"}}, + // sunnypilot model params {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c242871125..72d6002e46 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -55,6 +55,7 @@ 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/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index c8d054243f..e614393130 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -17,6 +17,7 @@ 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): @@ -34,6 +35,10 @@ 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 new file mode 100644 index 0000000000..c34a3ba8d0 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/longcontrol_ext.py @@ -0,0 +1,43 @@ +""" +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 6c3e496b5f..eb8f31c57c 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -9,11 +9,15 @@ farther-or-faster lead than reality, so braking is always >= stock: - flicker-hold: keep a just-dropped, recently-sustained lead alive through a radar dropout. - lead-jitter smoother: de-jitter a churning (trackId-flipping) lead so the MPC does not hunt the gap. - stop-gap bias: report a near-stopped lead slightly closer so the MPC stops a touch farther back. + - speed-gap bias: at higher speed report the lead slightly nearer so the MPC settles a wider following + gap and reacts to a lead's deceleration sooner. Also publishes a read-only lead-instability flag (telemetry). Default off => stock passthrough. """ from collections import deque +import numpy as np + from opendbc.car import structs from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL @@ -53,6 +57,14 @@ STOP_BIAS_REGIME_DREL = 12.0 # m: bias ramps in below this dRel STOP_BIAS_RAMP_BAND = 2.0 # m: ramp-in band (full offset below REGIME_DREL - RAMP_BAND) STOP_BIAS_MIN_DREL = 2.0 # m: never report a lead closer than this +# Speed-gap bias: widen the following gap at higher speed by reporting the lead a touch nearer +# (offset_m = dt(v) * v_ego). The MPC regulates the reported gap onto its target, so the real gap settles +# wider and braking onto a slowing lead starts sooner. Only reports nearer, so braking stays >= stock. +SPEED_GAP_V_BP = [14.0, 28.0] # m/s: gap widening ramps in across this band, flat above +SPEED_GAP_TF_V = [0.0, 0.25] # s: follow-time added to the gap at the band ends +SPEED_GAP_MAX_M = 5.0 # m: cap on the reported reduction +SPEED_GAP_MIN_DREL = 3.0 # m: never report the lead nearer than this + class _BiasedLead: __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') @@ -252,6 +264,17 @@ class RadarDistanceController: return lead return _BiasedLead(lead, max(lead.dRel - offset, STOP_BIAS_MIN_DREL)) + def _speed_gap_bias(self, lead): + # Report the lead nearer at speed so the MPC holds a wider gap and brakes sooner onto a slowing lead. + if not lead.status: + return lead + tf = float(np.interp(self._v_ego, SPEED_GAP_V_BP, SPEED_GAP_TF_V)) + offset = min(tf * self._v_ego, SPEED_GAP_MAX_M) + new_dRel = max(lead.dRel - offset, SPEED_GAP_MIN_DREL) + if new_dRel >= lead.dRel - 0.05: + return lead + return _BiasedLead(lead, new_dRel) + def smooth_radarstate(self, radarstate): self._stability.update(radarstate.leadOne, self._v_ego) # telemetry, runs every cycle if not self._enabled: @@ -262,6 +285,7 @@ class RadarDistanceController: one_b = self._stop_gap_bias(radarstate.leadOne) # low speed = stock lead, only the stop-gap bias return radarstate if one_b is radarstate.leadOne else _RadarStateProxy(one_b, radarstate.leadTwo) one = self._stop_gap_bias(one) + one = self._speed_gap_bias(one) if self._lead_smooth_enabled: one = self._smoother.update(one, self._stability.churn) # de-jitter a churning lead (anti follow-hunt) return _RadarStateProxy(one, two) 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 bc925007e8..7d5e6d37ee 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 @@ -10,7 +10,8 @@ from types import SimpleNamespace import pytest from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \ - RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V + RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V, \ + SPEED_GAP_MAX_M, SPEED_GAP_MIN_DREL COMFORT_BRAKE = 2.5 @@ -38,7 +39,7 @@ def obstacle(ld): def ctrl(enabled=True): c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled})) - c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0 # default above the gate so hold-logic tests exercise the flicker-hold + c._v_ego = 10.0 # above the low-speed gate (hold logic runs) but below the speed-gap onset (it stays inert here) return c @@ -226,6 +227,48 @@ def test_stop_bias_via_smooth_radarstate_low_speed(): assert out.leadOne.dRel < 8.0 # biased proxy returned at low speed +# --- speed-gap bias (wider gap at speed) ------------------------------------- + +def _speed_ctrl(v_ego=20.0): + c = ctrl() # speed-gap rides on the controller being enabled + c._v_ego = v_ego + return c + +def test_speed_gap_reports_closer_at_speed(): + out = _speed_ctrl(v_ego=20.0)._speed_gap_bias(lead(dRel=60.0)) + assert out.dRel < 60.0 # reported closer => MPC keeps a wider real gap + assert out.status and out.vLead == 18.0 # other fields preserved + +def test_speed_gap_monotone_never_farther(): + c = _speed_ctrl(v_ego=25.0) + for dr in (10.0, 30.0, 60.0, 100.0): + assert c._speed_gap_bias(lead(dRel=dr)).dRel <= dr + 1e-6 # only ever closer (brake >= stock) + +def test_speed_gap_offset_capped(): + out = _speed_ctrl(v_ego=40.0)._speed_gap_bias(lead(dRel=120.0)) + assert out.dRel >= 120.0 - SPEED_GAP_MAX_M - 1e-6 # reduction never exceeds the cap + +def test_speed_gap_min_floor(): + out = _speed_ctrl(v_ego=30.0)._speed_gap_bias(lead(dRel=4.0)) + assert out.dRel >= SPEED_GAP_MIN_DREL - 1e-6 # close cut-in not reported past the floor + +def test_speed_gap_off_when_controller_disabled(): + c = ctrl(enabled=False) + c._v_ego = 20.0 + r = rs(lead(dRel=60.0)) + assert c.smooth_radarstate(r) is r # controller off -> no speed-gap, byte-stock + +def test_speed_gap_low_speed_no_change(): + ld = lead(dRel=20.0) + assert _speed_ctrl(v_ego=7.0)._speed_gap_bias(ld) is ld # below the bottom speed bp -> no offset (no step) + +def test_speed_gap_via_smooth_radarstate_keeps_obstacle_le(): + c = _speed_ctrl(v_ego=22.0) + one = lead(dRel=70.0, vLead=20.0) + out = c.smooth_radarstate(rs(one)) + assert obstacle(out.leadOne) <= obstacle(one) + 1e-6 # biased obstacle never farther (brake >= stock) + + def test_obstacle_monotone_during_hold(): c = ctrl() for _ in range(3): diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py b/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py new file mode 100644 index 0000000000..832a72fc2a --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_ext.py @@ -0,0 +1,81 @@ +""" +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 d7087bb653..8bfdfcbd96 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -1259,6 +1259,10 @@ "title": "[TIZI/TICI only] Standstill Timer", "description": "Show a timer on the HUD when the car is at a standstill." }, + "StopSettleSoften": { + "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." + }, "SubaruStopAndGo": { "title": "Subaru Stop and Go", "description": "" diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 1b02a2aa6c..8a4512380e 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -597,7 +597,27 @@ "key": "RadarDistance", "widget": "toggle", "title": "Radar Distance", - "description": "Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock.", + "description": "Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, and at higher speed keeps a slightly wider following gap so it starts slowing sooner when the lead does. Smooths the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock.", + "visibility": [ + { + "type": "capability", + "field": "has_longitudinal_control", + "equals": true + } + ], + "enablement": [ + { + "type": "capability", + "field": "has_longitudinal_control", + "equals": true + } + ] + }, + { + "key": "StopSettleSoften", + "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.", "visibility": [ { "type": "capability", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 6e7698d929..02cae7c084 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -28,7 +28,17 @@ sections: widget: toggle title: Radar Distance description: Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab - it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock. + it, and at higher speed keeps a slightly wider following gap so it starts slowing sooner when the + lead does. Smooths the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock. + visibility: + - $ref: '#/macros/longitudinal' + enablement: + - $ref: '#/macros/longitudinal' + - key: StopSettleSoften + 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. visibility: - $ref: '#/macros/longitudinal' enablement: