diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5c1334c754..d08345bb17 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -172,7 +172,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP): # Acceleration Personality: early soft braking (never weaker than the plan). No-op when disabled. output_a_target = self.accel.smooth_target_accel(output_a_target, self.a_desired_trajectory, CONTROL_N_T_IDX, - self.output_should_stop or force_slow_decel, reset=reset_state, stock_brake=is_e2e) + self.output_should_stop or force_slow_decel, reset=reset_state, stock_brake=is_e2e, + speed_trajectory=self.v_desired_trajectory) # Lower (braking) bound and the ceiling's downward slew stay at the stock rate; only the ceiling's # upward slew is tier-dependent (Acceleration Personality). diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 184043deef..82868c8402 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -17,7 +17,7 @@ from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \ NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, SMOOTH_DECEL_BP, \ SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, \ - MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, STOP_APPROACH_VEGO, \ + MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \ ONSET_JERK0, ONSET_JERK_GAIN, ONSET_GAP_SOFT, ONSET_GAP_GAIN, ONSET_JERK_MAX, ONSET_HANDBACK_JERK, \ SOFT_ONSET_MAX_BRAKE_NEED, SOFT_ONSET_MAX_INSTANT_ACCEL, SOFT_ONSET_REARM_FRAMES @@ -68,50 +68,49 @@ class AccelController: return float(np.interp(max(0.0, float(brake_need)), SMOOTH_DECEL_BP, SMOOTH_DECEL_V[self._personality])) def smooth_target_accel(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float], - should_stop: bool, reset: bool = False, stock_brake: bool = False) -> float: - raw_target_accel = float(raw_target_accel) - self._brake_need = self._compute_brake_need(raw_target_accel, accel_trajectory, t_idxs) + should_stop: bool, reset: bool = False, stock_brake: bool = False, + speed_trajectory: Sequence[float] | None = None) -> float: + raw = float(raw_target_accel) + self._brake_need = self._compute_brake_need(raw, accel_trajectory, t_idxs) self._decel_target = 0.0 + self._smooth_active = False self._soft_active = False - # convex shaper runs ONLY when enabled and personality != NORMAL (off==stock, NORMAL==stock). - # Single source of truth: when it cannot run, hard-reset all shaper state so nothing leaks across - # a param toggle (SPORT->NORMAL / enable->disable) or a bypass interlude. - convex_on = self._enabled and self._personality != NORMAL - if not convex_on: + + # The convex onset shaper runs ONLY for ECO/SPORT (NORMAL and disabled are stock). Reset its state + # whenever it cannot run so nothing leaks across a personality toggle or a passthrough interlude. + if not (self._enabled and self._personality != NORMAL): self._reset_onset() - # disabled, reset, or blended/e2e braking: hand straight to the plan - if reset or not self._enabled or (stock_brake and (raw_target_accel < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED)): - self._bypassed = False - return self._passthrough(raw_target_accel) + # Passthroughs (hand the plan straight through, no shaping): + if reset or not self._enabled or (stock_brake and (raw < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED)): + self._bypassed = False # disabled / reset / blended-e2e braking + return self._passthrough(raw) + self._bypassed = self._emergency_bypass(raw, should_stop) + if self._bypassed: # a hard brake must never be softened + return self._stand_down(raw) + if self._stop_imminent(speed_trajectory, t_idxs): # stop coming -> stock decel, no coast/creep + return self._stand_down(raw) - self._bypassed = self._emergency_bypass(raw_target_accel, should_stop) - if self._bypassed: - self._reset_onset() - return self._passthrough(raw_target_accel) + # Front-load a gentle early brake when a deeper brake is predicted ahead. The convex shaper owns the + # output when it governed this frame (soft_active); otherwise never weaker than the plan. + if self._brake_need >= MIN_SMOOTH_BRAKE_NEED: + self._smooth_active = True + self._decel_target = self.get_decel_target(self._brake_need) + slewed = self._slew(min(raw, self._decel_target)) + return self._finalize(slewed if self._soft_active else min(slewed, raw)) - # Low-speed creep-to-stop: stand down (incl. the brake_need-driven front-load, which softens even - # when raw~0). Softening the final approach to a stopped lead makes the car brake less -> coast - # farther -> halt too close (~1.3 m). Hand full stock decel through so it stops at the proper gap. - # Onset shaping still applies above STOP_APPROACH_VEGO. - if self._v_ego < STOP_APPROACH_VEGO: - self._reset_onset() - return self._passthrough(raw_target_accel) - - if self._brake_need < MIN_SMOOTH_BRAKE_NEED: - self._smooth_active = False - slewed = self._slew(raw_target_accel) - if self._soft_active: - return self._finalize(slewed) - return self._finalize(min(slewed, raw_target_accel) if raw_target_accel < 0.0 else slewed) - - # front-load a gentle early brake, never weaker than the plan (unless the convex onset is active) - self._smooth_active = True - self._decel_target = self.get_decel_target(self._brake_need) - slewed = self._slew(min(raw_target_accel, self._decel_target)) - if self._soft_active: + # Below the smooth-brake threshold: track the plan, never weaker than it while braking. + slewed = self._slew(raw) + if self._soft_active or raw >= 0.0: return self._finalize(slewed) - return self._finalize(min(slewed, raw_target_accel)) + return self._finalize(min(slewed, raw)) + + def _stop_imminent(self, speed_trajectory: Sequence[float] | None, t_idxs: Sequence[float]) -> bool: + # plan predicts a near-stop within the lookahead -> a stop is coming (lead or light/sign). + if speed_trajectory is None: + return False + return any(float(s) < STOP_IMMINENT_VEGO + for s, t in zip(speed_trajectory, t_idxs, strict=False) if float(t) <= STOP_IMMINENT_LOOKAHEAD_T) def _compute_brake_need(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float]) -> float: min_accel = float(raw_target_accel) @@ -153,33 +152,27 @@ class AccelController: def _slew_convex(self, target_accel: float, jmax: float) -> float: # target_accel is the effective plan to track (raw, or min(raw, decel_target) on the smooth branch). - p = self._personality + # Dispatch: armed -> gentle bite; firm zone with an open soft gap -> fast hand-back; else stock. last = self._last_target_accel - soft_armed = self._onset_soft_armed(target_accel) - armed = soft_armed and not self._onset_latched gap = max(0.0, last - target_accel) # m/s^2 currently shallower than the plan (last,target both <=0) - if not armed: - # entered the firm/deep zone: latch off any further (re)arming for this event. - if soft_armed: - self._onset_latched = True - # NEVER start softening a fresh firm brake. - if not (self._soft_episode and gap > _ZERO_ACCEL_EPS): - self._soft_episode = False - return self._clean_accel(max(target_accel, last - jmax * DT_MDL)) # stock; caller does min(.,raw) - # Plan left the gentle zone but a soft gap is still open: close it FAST (firm, jerk-limited so it - # is not a snap) so the output catches the plan before braking gets firm -> no late-brake lag. - out = max(last - ONSET_HANDBACK_JERK[p] * DT_MDL, target_accel) - if out <= target_accel + _ZERO_ACCEL_EPS: - self._soft_episode = False - self._soft_active = True - return self._clean_accel(out) + soft_armed = self._onset_soft_armed(target_accel) + if soft_armed and not self._onset_latched: + return self._onset_bite(target_accel, last, gap) + if soft_armed: # firm/deep zone -> latch off further (re)arming + self._onset_latched = True + if self._soft_episode and gap > _ZERO_ACCEL_EPS: + return self._onset_handback(target_accel) + self._soft_episode = False # no open gap: NEVER soften a fresh firm brake + return self._clean_accel(max(target_accel, last - jmax * DT_MDL)) # stock; caller does min(.,raw) + + def _onset_bite(self, target_accel: float, last: float, gap: float) -> float: + # Gentle convex onset. Depth-proportional jerk: gentle ONSET_JERK0 at the bite (a~0), growing with + # current decel depth -- da/dt = j0 + k*a integrates to a(t) = (j0/k)*(exp(k*t)-1), the exponential- + # growth profile. A stateless instantaneous-gap catch-up adds bounded jerk once realized lags the plan + # by more than ONSET_GAP_SOFT, hard-capped at ONSET_JERK_MAX so even the catch is never a grab. + p = self._personality self._soft_episode = True - # Depth-proportional convex jerk: gentle ONSET_JERK0 at the bite (a~0), growing with current decel - # depth. da/dt = j0 + k*a integrates to a(t) = (j0/k)*(exp(k*t)-1), the exponential-growth profile. jerk = ONSET_JERK0[p] + ONSET_JERK_GAIN[p] * abs(last) - # Instantaneous-gap catch-up (stateless): once realized lags the plan by more than ONSET_GAP_SOFT, - # add bounded jerk so the softening can't run away and the gap-close is never a snap; hard-capped at - # ONSET_JERK_MAX so even the catch (while still gentle/armed) is never a grab. jerk = min(jerk + ONSET_GAP_GAIN[p] * max(0.0, gap - ONSET_GAP_SOFT[p]), ONSET_JERK_MAX[p]) out = max(last - jerk * DT_MDL, target_accel) # never deeper than the plan -> only softer-or-equal if out <= target_accel + _ZERO_ACCEL_EPS: # gap closed -> episode complete @@ -187,6 +180,15 @@ class AccelController: self._soft_active = True return self._clean_accel(out) + def _onset_handback(self, target_accel: float) -> float: + # Plan left the gentle zone but a soft gap is still open: close it FAST (firm, jerk-limited so it is + # not a snap) so the output catches the plan before braking gets firm -> no late-brake lag. + out = max(self._last_target_accel - ONSET_HANDBACK_JERK[self._personality] * DT_MDL, target_accel) + if out <= target_accel + _ZERO_ACCEL_EPS: + self._soft_episode = False + self._soft_active = True + return self._clean_accel(out) + def _reset_onset(self) -> None: self._onset_latched = False self._onset_release = 0 @@ -207,6 +209,11 @@ class AccelController: self._soft_active = False return self._finalize(target_accel) + def _stand_down(self, target_accel: float) -> float: + # clear shaper state and hand the plan straight through (emergency / stop-imminent) + self._reset_onset() + return self._passthrough(target_accel) + def _finalize(self, target_accel: float) -> float: target_accel = self._clean_accel(target_accel) self._last_target_accel = target_accel diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index a49bb69463..3764da223f 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -47,10 +47,13 @@ MIN_SMOOTH_BRAKE_NEED = 0.2 HARD_BRAKE_TARGET_ACCEL = -1.5 HARD_BRAKE_NEED = 2.6 -# Below this ego speed the shaper stands down (full stock decel). Softening the creep-to-stop makes the -# car brake less -> coast farther -> halt too close to a stopped lead (~1.3 m). Stock decel below this -# speed stops at the proper gap; sub-3 m/s braking is gentle anyway. Onset shaping applies above it. -STOP_APPROACH_VEGO = 3.0 # m/s +# Stop-imminent stand-down. The shaper's gentle bite is softer than the plan, so on a STOP approach it +# coasts the car in -> halts too close / "stop-roll-stop" creep. When the plan predicts a near-stop +# within the lookahead, stand the shaper down (full stock decel) so it stops at the proper gap with no +# coast. Keyed on the PREDICTED speed reaching ~0 (covers lead AND light/sign stops), NOT raw ego speed +# -- so non-stop low-speed braking (slowing to a moving follow) keeps the gentle onset at every speed. +STOP_IMMINENT_VEGO = 1.0 # m/s plan-predicted speed below this within the lookahead == stop coming +STOP_IMMINENT_LOOKAHEAD_T = 3.0 # s # --- Convex brake-onset shaper (param-gated; ECO/SPORT only, NORMAL = stock passthrough) --- # The grabby bite is the raw MPC plan: stock deepening uses a CONSTANT jerk (integrates to a LINEAR diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index 1a08b2084f..e84f840f8e 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -107,17 +107,18 @@ def test_early_soft_braking_brakes_before_plan(): assert ctrl.brake_need() == pytest.approx(1.0) -def test_low_speed_stop_approach_stands_down(): - # Below STOP_APPROACH_VEGO the shaper must NOT soften the creep-to-stop (softening -> coast farther -> - # halt too close). Full stock decel passes through; onset shaping re-engages above the threshold. +def test_stop_imminent_stands_down_but_moving_follow_shapes(): + # Stop coming (plan speed -> ~0): stand down to stock decel so the gentle bite can't coast into the + # stop (creep). Slowing to a MOVING follow (plan stays > STOP_IMMINENT_VEGO): gentle onset stays active + # at every speed -> the gentle-brake goal is not regressed. ctrl = make_controller(personality=ECO) - ctrl.update({'carState': SimpleNamespace(vEgo=2.0)}) - out = ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False) + stopping = [3.0, 2.0, 1.0, 0.4, 0.0] + [0.0] * (len(T_IDXS) - 5) + out = ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False, speed_trajectory=stopping) assert not ctrl.smooth_active() - assert out == pytest.approx(-0.1, abs=_EPS) # stock passthrough, no front-load softening - ctrl.update({'carState': SimpleNamespace(vEgo=8.0)}) - ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False) - assert ctrl.smooth_active() # onset shaping still active above the threshold + assert out == pytest.approx(-0.1, abs=_EPS) # stock passthrough into the stop, no softening + moving = [8.0] * len(T_IDXS) # slowing to a moving follow, not a stop + ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False, speed_trajectory=moving) + assert ctrl.smooth_active() # gentle onset preserved (not stop-imminent) @pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])