diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 3764da223f..f81fb6a6fa 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -30,11 +30,15 @@ A_CRUISE_MAX_V = { } RISE_RATE = {ECO: 0.05, NORMAL: STOCK_RISE_RATE, SPORT: 0.06} # ECO rise = stock so launch ramps promptly -# Early soft braking: predicted brake need (m/s^2) -> early decel target (m/s^2). +# Early soft braking: predicted brake need (m/s^2) -> early decel target (m/s^2). Front-loads a gentle +# decel as soon as the 3s plan lookahead predicts a brake, so decel is spread out instead of arriving as +# one late firm onset. The old ECO row was near-flat (-0.07 at brake_need~1.0 vs an eventual ~-0.88 plan +# brake) so it barely front-loaded -> late, jerky onsets on route 00000456. Deepened toward (but kept +# gentler than) NORMAL. Hard brakes (brake_need>=HARD_BRAKE_NEED or raw<=HARD_BRAKE_TARGET_ACCEL) still +# bypass to stock, and min(.,raw) keeps it never weaker than the plan. SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4] SMOOTH_DECEL_V = { - ECO: [0.00, -0.02, -0.05, -0.10, -0.25, -0.40, -0.60], - #ECO: [0.00, -0.08, -0.20, -0.30, -0.40, -0.70, -0.80], + ECO: [0.00, -0.08, -0.20, -0.35, -0.55, -0.78, -1.00], NORMAL: [0.00, -0.13, -0.30, -0.55, -0.84, -1.12, -1.40], SPORT: [0.00, -0.17, -0.40, -0.72, -1.05, -1.35, -1.65], } diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index af3fde8b56..1ed1ea8656 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -16,7 +16,6 @@ from openpilot.common.realtime import DT_MDL HOLD_MAX_FRAMES = 10 # ~0.5s cap, measured since the last SUSTAINED lead (not reset by 1-frame flicker) SUSTAIN_FRAMES = 2 # consecutive valid frames to (re)arm and reset the wall-clock DROPOUT_DREL = 1.0 -MIN_PROB = 0.2 FCW_PROB_CAP = 0.9 # held lead can't reach the FCW gate (>0.9) -> no false FCW MIN_HELD_DREL = 0.5 @@ -56,7 +55,11 @@ class _LeadHold: self.__init__() def step(self, raw): - if raw.status and raw.dRel > DROPOUT_DREL and raw.modelProb > MIN_PROB: + # Validity mirrors the MPC, which keys off status alone (long_mpc process_lead). modelProb is NOT a + # gate: radard's low_speed_override emits a real closest-track lead with modelProb=0.0, so gating on + # prob wrongly rejected real close stop-and-go leads and substituted a stale farther held lead -> + # under-brake -> stopping too close. FCW stays bounded by FCW_PROB_CAP on the held output below. + if raw.status and raw.dRel > DROPOUT_DREL: self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb) self._sustained += 1 if self._sustained >= SUSTAIN_FRAMES: 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 8029c5c2a6..87e5777611 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 @@ -64,6 +64,25 @@ def test_holds_after_sustained_dropout(): assert held.dRel == pytest.approx(30.0 - 4.0 * 0.05, abs=1e-6) +def test_low_speed_override_lead_passthrough(): + # radard low_speed_override emits a real closest-track lead with modelProb=0.0. It must be honored as a + # real lead (passthrough), NOT rejected and replaced by a stale farther held lead (would under-brake at + # stop-and-go and stop too close). + c = ctrl() + one = lead(status=True, dRel=2.5, vRel=0.0, vLead=0.0, modelProb=0.0) + out = c.smooth_radarstate(rs(one)) + assert out.leadOne is one # passed straight through, not substituted + + +def test_low_speed_override_lead_arms_hold(): + # a sustained prob=0 real lead should arm the hold like any real lead + c = ctrl() + for _ in range(3): + c.smooth_radarstate(rs(lead(status=True, dRel=3.0, vRel=-0.5, vLead=1.0, modelProb=0.0))) + held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne + assert held.status is True # armed off the prob=0 lead, holds through dropout + + def test_obstacle_monotone_during_hold(): c = ctrl() for _ in range(3):