From 5cfa627b1ecae39860ee11f8795f5b182cdfd2fe Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 1 Jul 2026 10:17:36 -0700 Subject: [PATCH] feat(long): hard brake --- common/params_keys.h | 3 ++ .../test_longitudinal_smoothing_wiring.py | 1 + .../lib/radar_distance/radar_distance.py | 28 +++++++++- .../tests/test_radar_distance.py | 51 ++++++++++++++++++- sunnypilot/sunnylink/params_metadata.json | 4 ++ sunnypilot/sunnylink/settings_ui.json | 20 ++++++++ .../settings_ui_src/pages/cruise.yaml | 9 ++++ 7 files changed, 113 insertions(+), 3 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 9955802e75..7637f33302 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -246,6 +246,9 @@ inline static std::unordered_map keys = { // 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"}}, + // Lead Decel Anticipate: start braking earlier for a hard-braking lead so the brake isn't a late catch-up + {"LeadDecelAnticipate", {PERSISTENT | BACKUP, BOOL, "0"}}, + // sunnypilot model params {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py index 9cf658c4f5..144096013b 100644 --- a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py +++ b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py @@ -14,6 +14,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) + assert re.search(r'"LeadDecelAnticipate", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys) def test_longitudinal_smoothing_stays_planner_side(): diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index bebab55cdc..35d5a8dd05 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -65,6 +65,15 @@ SPEED_GAP_TF_V = [0.0, 0.25] # s: follow-time added to the gap at the 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 +# Lead-decel anticipation: when the lead is braking (aLeadK negative) and we are closing, report it a bit +# nearer so the MPC brings its brake forward and spreads it, rather than a late hard catch-up. The offset +# grows with how hard the lead is braking. Only reports nearer (obstacle only shrinks => brake >= stock). +# Gated by the LeadDecelAnticipate param. +LEAD_DECEL_ALEAD_BP = [0.5, 3.0] # |aLeadK| m/s^2 (lead braking) mapped to the anticipation offset +LEAD_DECEL_OFFSET_V = [0.0, 5.0] # m: dRel reduction across that band +LEAD_DECEL_MAX_VREL = -0.3 # m/s: only anticipate when closing (vRel below this) +LEAD_DECEL_MIN_DREL = 4.0 # m: never report the lead nearer than this + class _BiasedLead: __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') @@ -225,6 +234,7 @@ class RadarDistanceController: self._v_ego = 0.0 self._enabled = self._params.get_bool("RadarDistance") self._stop_gap_bias_enabled = self._params.get_bool("StopGapBias") + self._lead_decel_enabled = self._params.get_bool("LeadDecelAnticipate") self._lead_smooth_enabled = LEAD_SMOOTH_ENABLED self._one = _LeadHold() self._two = _LeadHold() @@ -238,6 +248,7 @@ class RadarDistanceController: self._two.reset() self._enabled = enabled self._stop_gap_bias_enabled = self._params.get_bool("StopGapBias") + self._lead_decel_enabled = self._params.get_bool("LeadDecelAnticipate") def update(self, sm) -> None: if self._frame % int(1. / DT_MDL) == 0: @@ -276,18 +287,31 @@ class RadarDistanceController: return lead return _BiasedLead(lead, new_dRel) + def _lead_decel_bias(self, lead): + # Report a braking, closing lead a bit nearer so the MPC brakes earlier and spreads it (anticipation). + if not self._lead_decel_enabled or not lead.status: + return lead + if lead.aLeadK >= 0.0 or lead.vRel > LEAD_DECEL_MAX_VREL: + return lead + offset = float(np.interp(-lead.aLeadK, LEAD_DECEL_ALEAD_BP, LEAD_DECEL_OFFSET_V)) + new_dRel = max(lead.dRel - offset, LEAD_DECEL_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: - one_b = self._stop_gap_bias(radarstate.leadOne) # stop-gap bias works standalone; else passthrough + one_b = self._lead_decel_bias(self._stop_gap_bias(radarstate.leadOne)) # standalone biases (self-gated) 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: - one_b = self._stop_gap_bias(radarstate.leadOne) # low speed = stock lead, only the stop-gap bias + one_b = self._lead_decel_bias(self._stop_gap_bias(radarstate.leadOne)) # crawl: stop-gap + anticipation 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) + one = self._lead_decel_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 6ef06d5f5d..a0ab28e635 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 @@ -11,7 +11,7 @@ import pytest from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \ RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V, \ - SPEED_GAP_MAX_M, SPEED_GAP_MIN_DREL + SPEED_GAP_MAX_M, SPEED_GAP_MIN_DREL, LEAD_DECEL_MIN_DREL, LEAD_DECEL_OFFSET_V COMFORT_BRAKE = 2.5 @@ -276,6 +276,55 @@ def test_speed_gap_via_smooth_radarstate_keeps_obstacle_le(): assert obstacle(out.leadOne) <= obstacle(one) + 1e-6 # biased obstacle never farther (brake >= stock) +# --- lead-decel anticipation (brake earlier for a braking, closing lead) ------ + +def _decel_ctrl(v_ego=12.0): + c = ctrl() + c._lead_decel_enabled = True + c._v_ego = v_ego + return c + +def test_lead_decel_reports_nearer_when_braking_and_closing(): + out = _decel_ctrl()._lead_decel_bias(lead(dRel=30.0, vRel=-3.0, aLeadK=-2.0)) + assert out.dRel < 30.0 # nearer => MPC brakes earlier + assert out.vLead == 18.0 and out.status # other fields preserved + +def test_lead_decel_offset_scales_with_brake(): + c = _decel_ctrl() + soft = c._lead_decel_bias(lead(dRel=40.0, vRel=-3.0, aLeadK=-1.0)).dRel + hard = c._lead_decel_bias(lead(dRel=40.0, vRel=-3.0, aLeadK=-3.0)).dRel + assert hard < soft # harder lead brake => more anticipation + assert 40.0 - hard <= max(LEAD_DECEL_OFFSET_V) + 1e-6 # capped + +def test_lead_decel_noop_when_not_braking(): + ld = lead(dRel=30.0, vRel=-3.0, aLeadK=0.0) + assert _decel_ctrl()._lead_decel_bias(ld) is ld # lead not braking -> no anticipation + +def test_lead_decel_noop_when_not_closing(): + ld = lead(dRel=30.0, vRel=0.5, aLeadK=-2.0) + assert _decel_ctrl()._lead_decel_bias(ld) is ld # not closing -> no anticipation + +def test_lead_decel_monotone_never_farther(): + c = _decel_ctrl() + for dr in (6.0, 15.0, 40.0): + assert c._lead_decel_bias(lead(dRel=dr, vRel=-3.0, aLeadK=-2.5)).dRel <= dr + 1e-6 + +def test_lead_decel_min_floor(): + out = _decel_ctrl()._lead_decel_bias(lead(dRel=5.0, vRel=-3.0, aLeadK=-3.0)) + assert out.dRel >= LEAD_DECEL_MIN_DREL - 1e-6 + +def test_lead_decel_off_no_change(): + ld = lead(dRel=30.0, vRel=-3.0, aLeadK=-2.0) + assert ctrl()._lead_decel_bias(ld) is ld # param off -> passthrough + +def test_lead_decel_independent_of_radar_distance(): + c = ctrl(enabled=False) + c._lead_decel_enabled = True + c._v_ego = 12.0 + out = c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-3.0, aLeadK=-2.0))) + assert out.leadOne.dRel < 30.0 # anticipation works standalone + + def test_obstacle_monotone_during_hold(): c = ctrl() for _ in range(3): diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index af0d91750f..6f76c83a67 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -600,6 +600,10 @@ "title": "Lateral Maneuver Mode", "description": "" }, + "LeadDecelAnticipate": { + "title": "Anticipate Braking Lead", + "description": "When the lead brakes hard, starts slowing earlier and spreads the brake instead of a late hard catch-up. Braking is never reduced below stock." + }, "LeadDepartAlert": { "title": "Lead Departure Alert (Beta)", "description": "A chime and on-screen alert (TIZI/TICI only) will play when you are stopped, and the vehicle in front of you start moving.
Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly." diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index fc171da578..6236e45f49 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -633,6 +633,26 @@ } ] }, + { + "key": "LeadDecelAnticipate", + "widget": "toggle", + "title": "Anticipate Braking Lead", + "description": "When the lead brakes hard, starts slowing earlier and spreads the brake instead of a late hard catch-up. 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": "DisengageOnAccelerator", "widget": "toggle", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 1f8b21f97c..ecac1886a1 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -43,6 +43,15 @@ sections: - $ref: '#/macros/longitudinal' enablement: - $ref: '#/macros/longitudinal' + - key: LeadDecelAnticipate + widget: toggle + title: Anticipate Braking Lead + description: When the lead brakes hard, starts slowing earlier and spreads the brake instead of a late + hard catch-up. Braking is never reduced below stock. + visibility: + - $ref: '#/macros/longitudinal' + enablement: + - $ref: '#/macros/longitudinal' - key: DisengageOnAccelerator widget: toggle title: Disengage Cruise on Accelerator Pedal