feat(long): hard brake

common/params_keys.h

@@ -246,6 +246,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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():

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)

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):

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. <br>Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly."

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",

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