From 09abbe1f284b844b35571a9709b49572eb67e105 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Wed, 15 Jul 2026 13:39:15 -0700 Subject: [PATCH] Make accel profiles shape MPC cruise response --- cereal/custom.capnp | 2 + .../lib/longitudinal_mpc_lib/long_mpc.py | 10 +- .../controls/lib/longitudinal_planner.py | 8 +- .../lib/accel_personality/accel_controller.py | 190 ++++++++++++-- .../tests/test_accel_controller.py | 242 ++++++++++++++++-- .../tests/test_accel_controller_interfaces.py | 30 ++- .../controls/lib/longitudinal_planner.py | 8 +- .../test_accel_controller_closed_loop.py | 75 +++++- 8 files changed, 504 insertions(+), 61 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 2c3b1c10ae..d5bfcea6f0 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -312,6 +312,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { usableGap @10 :Float32; closingSpeed @11 :Float32; requiredDecel @12 :Float32; + aMaxProfile @13 :Float32; + aMaxEffective @14 :Float32; enum Profile { eco @0; diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index deae416489..1594e33ae4 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -313,7 +313,7 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, cruise_accel_max: float | None = None): t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -329,9 +329,15 @@ class LongitudinalMpc: # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. + # sunnypilot can optionally shape only this cruise reference; this hook does + # not alter lead obstacles, braking constraints, or the MPC jerk cost. + if cruise_accel_max is None or not np.isfinite(cruise_accel_max): + cruise_accel_max = CRUISE_MAX_ACCEL + else: + cruise_accel_max = float(np.clip(cruise_accel_max, 0.0, CRUISE_MAX_ACCEL)) v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) + v_upper = v_ego + (T_IDXS * cruise_accel_max * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 58262efe94..2aa5b636f1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -138,7 +138,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): v_cruise = LongitudinalPlannerSP.update_accel_controller( self, sm, v_cruise, engaged=not reset_state, cruise_initialized=v_cruise_initialized, acc_selected=not is_e2e, planner_speed=self.v_desired_filter.x, previous_mpc_source=self.mpc.source, previous_should_stop=self.output_should_stop, - controller_fault=self.mpc.solution_status != 0, + stock_accel_max=accel_clip[1], planner_accel=self.a_desired, controller_fault=self.mpc.solution_status != 0, ) if force_slow_decel: @@ -146,7 +146,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) + self.mpc.update( + sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality, + cruise_accel_max=self.accel_controller_result.mpc_cruise_accel_max, + ) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -180,6 +183,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): for idx in range(2): accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) + self.output_a_target = min(self.output_a_target, self.accel_controller_result.output_accel_max) self.prev_accel_clip = accel_clip def publish(self, sm, pm): diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 1e8a2b409f..0ff63e41d2 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -7,6 +7,7 @@ import math import numpy as np from cereal import log +from opendbc.car.interfaces import ACCEL_MAX from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( LongitudinalMpc, @@ -35,16 +36,23 @@ class AccelControllerState(IntEnum): @dataclass(frozen=True) class ProfileConfig: comfort_decel: float - release_accel: float release_confirm: float PROFILE_CONFIGS = { - AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_accel=0.65, release_confirm=0.50), - AccelProfile.normal: ProfileConfig(comfort_decel=0.35, release_accel=0.85, release_confirm=0.35), - AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_accel=1.10, release_confirm=0.20), + AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_confirm=0.50), + AccelProfile.normal: ProfileConfig(comfort_decel=0.35, release_confirm=0.35), + AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_confirm=0.20), } +ACCEL_PROFILE_MAX_BP = [0.0, 10.0, 25.0, 40.0] +ACCEL_PROFILE_MAX_V = { + AccelProfile.eco: [1.00, 0.75, 0.45, 0.30], + AccelProfile.normal: [1.30, 1.00, 0.65, 0.45], + AccelProfile.sport: [1.55, 1.15, 0.78, 0.58], +} +LAUNCH_DELTA_V = 3.0 + CAP_FILTER_FRAMES = 5 RESTRICT_DEADBAND = 0.15 RELIEF_DEADBAND = 0.35 @@ -53,6 +61,10 @@ STOP_HOLD_CAP = 0.50 STOPPED_LEAD_SPEED = 0.30 STOP_HOLD_EXIT_CAP = 0.80 STOP_HOLD_EXIT_FRAMES = 4 +CLEAR_ROAD_PROFILE_SPEED = 0.10 +ACCEL_LIMIT_JERK = 1.0 +LAUNCH_ACCEL_JERK = 3.0 +LAUNCH_PACE_RATE = 5.0 @dataclass(frozen=True) @@ -71,7 +83,12 @@ class AccelControllerResult: enabled: bool active: bool shadow_active: bool + launching: bool profile: AccelProfile + profile_accel_max: float + effective_accel_max: float + output_accel_max: float + mpc_cruise_accel_max: float state: AccelControllerState shadow_state: AccelControllerState base_speed: float @@ -95,6 +112,7 @@ class _PacePath: departure_frames: int = 0 departing_from_stop: bool = False stopped_lead_hold: bool = False + accel_limit: float | None = None def reset(self) -> None: self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES) @@ -104,6 +122,7 @@ class _PacePath: self.departure_frames = 0 self.departing_from_stop = False self.stopped_lead_hold = False + self.accel_limit = None def update_filter(self, cap: float) -> float: self.cap_samples.append(cap) @@ -115,7 +134,7 @@ class _PacePath: class AccelController: - """A comfort-only relative-pace envelope applied before longitudinal MPC.""" + """A relative-pace governor with a positive-acceleration comfort ceiling.""" def __init__(self, CP, dt: float = DT_MDL): if not math.isfinite(dt) or dt <= 0.0: @@ -133,6 +152,15 @@ class AccelController: except (TypeError, ValueError): return AccelProfile.normal + @classmethod + def get_profile_accel_max(cls, profile: int | AccelProfile, v_ego: float) -> float: + """Return the profile's positive-acceleration ceiling at the current speed.""" + if not math.isfinite(v_ego): + return math.nan + + profile = cls._profile(profile) + return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile])) + def _delay(self) -> float: try: return float(self.CP.longitudinalActuatorDelay) + DT_MDL @@ -219,16 +247,28 @@ class AccelController: base_speed: float, v_ego: float, config: ProfileConfig, + accel_rate: float, previous_mpc_source, planner_speed: float, previous_should_stop: bool, has_nearly_stopped_lead: bool, + launch_delta_v: float, ) -> float: filtered_cap = path.update_filter(raw_cap) - if path.pace is None: + just_initialized = path.pace is None + if just_initialized: path.pace = min(base_speed, v_ego) path.state = AccelControllerState.free + # A clear-road standstill engagement should request motion immediately. A + # stopped/previously-stopping lead still goes through stop-hold confirmation. + if just_initialized and v_ego < STOP_HOLD_EGO_SPEED and not math.isfinite(raw_cap) and not previous_should_stop: + path.pace = min(base_speed, v_ego + launch_delta_v) + path.state = AccelControllerState.release + path.relief_time = config.release_confirm + path.departing_from_stop = True + return filtered_cap + # A lower non-controller target is authoritative, and is also the correct seed if it later clears. path.pace = min(path.pace, base_speed) if self._lead_source(previous_mpc_source) and not math.isfinite(raw_cap) and planner_speed < path.pace: @@ -237,7 +277,8 @@ class AccelController: if v_ego < STOP_HOLD_EGO_SPEED and (filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead): path.stopped_lead_hold = True - if v_ego >= STOP_HOLD_EGO_SPEED: + clear_road_launch_complete = path.departing_from_stop and not path.stopped_lead_hold and v_ego >= CLEAR_ROAD_PROFILE_SPEED + if v_ego >= STOP_HOLD_EGO_SPEED or clear_road_launch_complete: path.departing_from_stop = False path.stopped_lead_hold = False @@ -252,7 +293,11 @@ class AccelController: return filtered_cap if path.state == AccelControllerState.stopHold: - if filtered_cap > STOP_HOLD_EXIT_CAP: + # A continuously observed moving lead exits after exactly four raw frames. + # Total lead loss still waits for the five-frame median dropout guard first. + raw_departure = math.isfinite(raw_cap) and raw_cap > STOP_HOLD_EXIT_CAP and not has_nearly_stopped_lead + guarded_lead_loss = not math.isfinite(raw_cap) and filtered_cap > STOP_HOLD_EXIT_CAP + if raw_departure or guarded_lead_loss: path.departure_frames += 1 else: path.departure_frames = 0 @@ -265,6 +310,8 @@ class AccelController: path.relief_time = config.release_confirm path.departure_frames = 0 path.departing_from_stop = True + path.pace = min(base_speed, filtered_cap, v_ego + launch_delta_v) + return filtered_cap ceiling = min(base_speed, filtered_cap) if ceiling <= path.pace - RESTRICT_DEADBAND: @@ -282,7 +329,8 @@ class AccelController: release_allowed = path.relief_time >= config.release_confirm if release_allowed: - path.pace = min(ceiling, path.pace + config.release_accel * self.dt) + pace_rate = LAUNCH_PACE_RATE if path.departing_from_stop else accel_rate + path.pace = min(ceiling, path.pace + pace_rate * self.dt) path.state = AccelControllerState.release elif relief <= RELIEF_DEADBAND: path.relief_time = 0.0 @@ -290,9 +338,53 @@ class AccelController: return filtered_cap + def _update_accel_limit( + self, + path: _PacePath, + stock_accel_max: float, + planner_accel: float, + profile_accel_max: float, + ) -> tuple[float, float]: + """Return the raw stock/controller minimum and the controller-owned positive ceiling.""" + requested_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX)) + + if path.state == AccelControllerState.stopHold: + path.accel_limit = 0.0 + # Keep stock MPC warm while the output ceiling prevents an early launch + # during the four-frame lead-departure confirmation. + return min(stock_accel_max, 0.0), 0.0 + + if path.departing_from_stop: + # Start commanding acceleration on the first confirmed frame, then open + # quickly enough to avoid launch delay without a step in requested accel. + previous_limit = path.accel_limit if path.accel_limit is not None else 0.0 + path.accel_limit = min(requested_limit, previous_limit + LAUNCH_ACCEL_JERK * self.dt) + return min(stock_accel_max, path.accel_limit), path.accel_limit + + if path.accel_limit is None: + # Avoid a discontinuity when enabling around an already-positive command. + # The global OP limit bounds this seed; dynamic stock output constraints + # still retain their existing output-side enforcement and slew. + path.accel_limit = min(ACCEL_MAX, max(requested_limit, max(0.0, planner_accel))) + else: + max_step = ACCEL_LIMIT_JERK * self.dt + path.accel_limit = float(np.clip(requested_limit, path.accel_limit - max_step, path.accel_limit + max_step)) + + effective_limit = min(stock_accel_max, path.accel_limit) + return effective_limit, path.accel_limit + @staticmethod def _valid_context( - base_speed: float, v_ego: float, a_ego: float, planner_speed: float, delay: float, engaged: bool, cruise_initialized: bool, controller_fault: bool + base_speed: float, + v_ego: float, + a_ego: float, + planner_speed: float, + stock_accel_max: float, + planner_accel: float, + delay: float, + engaged: bool, + cruise_initialized: bool, + controller_fault: bool, ) -> bool: return ( engaged @@ -302,7 +394,7 @@ class AccelController: and v_ego >= 0.0 and planner_speed >= 0.0 and delay >= 0.0 - and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, delay)) + and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay)) ) def update( @@ -320,21 +412,47 @@ class AccelController: cruise_initialized: bool, previous_mpc_source, planner_speed: float, + stock_accel_max: float, + planner_accel: float, previous_should_stop: bool, controller_fault: bool = False, ) -> AccelControllerResult: """Update live and shadow acceleration controllers and return the target and additive telemetry.""" profile = self._profile(profile) config = PROFILE_CONFIGS[profile] + profile_accel_max = self.get_profile_accel_max(profile, v_ego) + launch_delta_v = LAUNCH_DELTA_V delay = self._delay() - valid_context = self._valid_context(base_speed, v_ego, a_ego, planner_speed, delay, engaged, cruise_initialized, controller_fault) + valid_context = self._valid_context( + base_speed, + v_ego, + a_ego, + planner_speed, + stock_accel_max, + planner_accel, + delay, + engaged, + cruise_initialized, + controller_fault, + ) envelope = self.calculate_energy_envelope(radar_state, v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope() if valid_context: shadow_filtered_cap = self._update_path( - self.shadow, envelope.cap, base_speed, v_ego, config, previous_mpc_source, planner_speed, previous_should_stop, envelope.has_nearly_stopped_lead + self.shadow, + envelope.cap, + base_speed, + v_ego, + config, + profile_accel_max, + previous_mpc_source, + planner_speed, + previous_should_stop, + envelope.has_nearly_stopped_lead, + launch_delta_v, ) + self._update_accel_limit(self.shadow, stock_accel_max, planner_accel, profile_accel_max) shadow_active = True else: self.shadow.reset() @@ -343,23 +461,35 @@ class AccelController: live_active = valid_context and bool(enabled) and bool(acc_selected) if live_active: - was_uninitialized = self.live.pace is None - was_departing_from_stop = self.live.departing_from_stop live_filtered_cap = self._update_path( - self.live, envelope.cap, base_speed, v_ego, config, previous_mpc_source, planner_speed, previous_should_stop, envelope.has_nearly_stopped_lead + self.live, + envelope.cap, + base_speed, + v_ego, + config, + profile_accel_max, + previous_mpc_source, + planner_speed, + previous_should_stop, + envelope.has_nearly_stopped_lead, + launch_delta_v, ) - initial_lead_warm = was_uninitialized and v_ego < STOP_HOLD_EGO_SPEED and envelope.selected_lead >= 0 - stopped_lead_owns_hold = ( - self.live.state == AccelControllerState.stopHold and self.live.stopped_lead_hold and envelope.selected_lead >= 0 + effective_accel_max, output_accel_max = self._update_accel_limit( + self.live, stock_accel_max, planner_accel, profile_accel_max ) - if initial_lead_warm or (v_ego < STOP_HOLD_EGO_SPEED and (self.live.departing_from_stop or stopped_lead_owns_hold)): - # Keep stock MPC warm while a present lead owns a latched stop, or after departure is confirmed. - # A dropout immediately pins both target and pace to zero instead of inheriting stale geometry. + # Stock cruise shaping during a confirmed stop/departure keeps takeoff + # immediate. Once rolling, the profile rate shapes MPC's cruise horizon. + mpc_cruise_accel_max = ( + math.inf if self.live.state == AccelControllerState.stopHold or self.live.departing_from_stop else output_accel_max + ) + if self.live.state == AccelControllerState.stopHold: + # Keep stock MPC warm while the zero output ceiling pins a confirmed stop + # and confirms four moving-lead frames before allowing takeoff. + target_speed = base_speed + elif self.live.departing_from_stop and v_ego < STOP_HOLD_EGO_SPEED and envelope.selected_lead >= 0: + # A moving lead keeps stock MPC well-conditioned during a confirmed + # departure. Clear-road launches retain the bounded live pace below. target_speed = base_speed - elif was_departing_from_stop and v_ego >= STOP_HOLD_EGO_SPEED: - # Resume the confirmed relative envelope once the stock start state has moved the car. - self.live.pace = min(base_speed, live_filtered_cap) - target_speed = self.live.pace else: target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed) else: @@ -367,13 +497,21 @@ class AccelController: live_filtered_cap = math.inf # Preserve the stock target bit-for-bit on every bypass, including stock's own invalid-value handling. target_speed = base_speed + effective_accel_max = math.inf + output_accel_max = math.inf + mpc_cruise_accel_max = math.inf return AccelControllerResult( target_speed=target_speed, enabled=bool(enabled), active=live_active, shadow_active=shadow_active, + launching=live_active and self.live.departing_from_stop, profile=profile, + profile_accel_max=profile_accel_max if live_active else math.inf, + effective_accel_max=effective_accel_max, + output_accel_max=output_accel_max, + mpc_cruise_accel_max=mpc_cruise_accel_max, state=self.live.state, shadow_state=self.shadow.state, base_speed=base_speed, 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 93856b876f..fa95e3e709 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 @@ -6,8 +6,14 @@ import pytest from cereal import log from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import ( + ACCEL_LIMIT_JERK, + ACCEL_PROFILE_MAX_BP, + ACCEL_PROFILE_MAX_V, + LAUNCH_ACCEL_JERK, + LAUNCH_DELTA_V, AccelController, AccelControllerState, AccelProfile, @@ -40,12 +46,148 @@ def update(governor, radar_state=None, **overrides): "cruise_initialized": True, "previous_mpc_source": LongitudinalPlanSource.cruise, "planner_speed": 20.0, + "stock_accel_max": 2.0, + "planner_accel": 0.0, "previous_should_stop": False, } args.update(overrides) return governor.update(radar_state or make_radar(), **args) +class TestAccelProfileLimits: + @pytest.mark.parametrize("profile", list(AccelProfile)) + def test_profile_accel_max_matches_lookup_table(self, profile): + for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True): + assert AccelController.get_profile_accel_max(profile, speed) == expected + + @pytest.mark.parametrize("profile", list(AccelProfile)) + def test_profile_accel_max_interpolates_and_clamps(self, profile): + expected_midpoint = (ACCEL_PROFILE_MAX_V[profile][1] + ACCEL_PROFILE_MAX_V[profile][2]) / 2.0 + + assert AccelController.get_profile_accel_max(profile, 17.5) == pytest.approx(expected_midpoint) + assert AccelController.get_profile_accel_max(profile, -1.0) == ACCEL_PROFILE_MAX_V[profile][0] + assert AccelController.get_profile_accel_max(profile, 50.0) == ACCEL_PROFILE_MAX_V[profile][-1] + + @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP) + def test_profile_accel_max_order_is_distinct(self, speed): + limits = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile] + + assert limits[AccelProfile.eco] < limits[AccelProfile.normal] < limits[AccelProfile.sport] + + @pytest.mark.parametrize("profile", list(AccelProfile)) + def test_profile_table_never_exceeds_stock_speed_limit(self, profile): + for step in range(161): + speed = step * 0.25 + assert AccelController.get_profile_accel_max(profile, speed) <= get_max_accel(speed) + + def test_active_result_exposes_profile_accel_max(self): + governor = make_governor() + + result = update(governor, profile=AccelProfile.eco, v_ego=17.5, planner_speed=17.5) + + assert result.profile_accel_max == pytest.approx(0.60) + + def test_normal_active_limits_are_bounded_by_stock_and_profile(self): + governor = make_governor() + + result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40) + + assert result.profile_accel_max == 1.0 + assert result.effective_accel_max == 1.0 + assert result.output_accel_max == 1.0 + assert result.mpc_cruise_accel_max == 1.0 + + def test_first_enable_seeds_from_positive_planner_accel_within_stock(self): + governor = make_governor() + + first = update( + governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20 + ) + second = update( + governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20 + ) + + assert first.effective_accel_max == 1.20 + assert second.effective_accel_max == pytest.approx(first.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL) + + def test_first_enable_seed_preserves_current_plan_but_effective_limit_stays_stock_bounded(self): + governor = make_governor() + + result = update( + governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.10, planner_accel=1.80 + ) + + assert result.effective_accel_max == 1.10 + assert result.output_accel_max == 1.80 + + def test_profile_switch_slews_at_one_meter_per_second_cubed(self): + governor = make_governor() + sport = update(governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0) + + eco = update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0) + + assert sport.effective_accel_max == 1.15 + assert eco.effective_accel_max == pytest.approx(sport.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL) + assert eco.effective_accel_max > eco.profile_accel_max + + def test_dynamic_stock_tightening_does_not_enter_controller_comfort_state(self): + governor = make_governor() + update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40) + + tightened = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=0.40) + released = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40) + + assert tightened.effective_accel_max == 0.40 + assert tightened.output_accel_max == 1.0 + assert released.effective_accel_max == 1.0 + assert released.output_accel_max == 1.0 + + def test_negative_stock_max_never_becomes_positive(self): + governor = make_governor() + + result = update(governor, v_ego=10.0, planner_speed=10.0, stock_accel_max=-0.20, planner_accel=1.0) + + assert result.effective_accel_max == -0.20 + assert result.output_accel_max == 1.0 + + def test_profile_tightening_can_converge_below_positive_planner_accel(self): + governor = make_governor() + update( + governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15 + ) + + results = [ + update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15) + for _ in range(8) + ] + + assert results[-1].output_accel_max == pytest.approx(ACCEL_PROFILE_MAX_V[AccelProfile.eco][1]) + assert results[-1].output_accel_max < 1.15 + assert all(math.isfinite(result.output_accel_max) for result in results) + + @pytest.mark.parametrize("bypass", [{"enabled": False}, {"acc_selected": False}]) + def test_bypass_does_not_expose_an_active_accel_limit(self, bypass): + governor = make_governor() + + result = update(governor, **bypass) + + assert math.isinf(result.profile_accel_max) + assert math.isinf(result.effective_accel_max) + assert math.isinf(result.output_accel_max) + assert math.isinf(result.mpc_cruise_accel_max) + + @pytest.mark.parametrize("invalid", [{"stock_accel_max": math.nan}, {"planner_accel": math.nan}]) + def test_invalid_accel_input_bypasses_and_resets_limits(self, invalid): + governor = make_governor() + update(governor) + + result = update(governor, **invalid) + + assert not result.active + assert governor.live.accel_limit is None + assert math.isinf(result.effective_accel_max) + + class TestEnergyEnvelope: def test_correct_relative_energy_formula_and_lead_selection(self): governor = make_governor() @@ -189,7 +331,8 @@ class TestAccelControllerState: assert confirmation_updates < 20 assert confirmation_updates >= 6 - assert result.live_pace == pytest.approx(held_pace + PROFILE_CONFIGS[AccelProfile.normal].release_accel * DT_MDL) + expected_rate = AccelController.get_profile_accel_max(AccelProfile.normal, 20.0) + assert result.live_pace == pytest.approx(held_pace + expected_rate * DT_MDL) def test_live_state_never_adopts_shadow_history(self): governor = make_governor() @@ -229,17 +372,63 @@ class TestAccelControllerState: assert result.state == AccelControllerState.stopHold assert result.live_pace == 0.0 assert result.target_speed == stop_args["base_speed"] + assert result.effective_accel_max == 0.0 + assert result.output_accel_max == 0.0 + assert math.isinf(result.mpc_cruise_accel_max) - for _ in range(5): + for _ in range(3): result = update(governor, moving, **stop_args) assert result.state == AccelControllerState.stopHold + assert not result.launching assert result.live_pace == 0.0 assert result.target_speed == stop_args["base_speed"] + assert result.effective_accel_max == 0.0 + assert result.output_accel_max == 0.0 + assert math.isinf(result.mpc_cruise_accel_max) departed = update(governor, moving, **stop_args) assert departed.state == AccelControllerState.release - assert departed.live_pace == pytest.approx(PROFILE_CONFIGS[AccelProfile.normal].release_accel * DT_MDL) + assert departed.launching + assert departed.live_pace == pytest.approx(stop_args["v_ego"] + LAUNCH_DELTA_V) assert departed.target_speed == stop_args["base_speed"] + assert departed.effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL) + assert departed.output_accel_max == departed.effective_accel_max + assert math.isinf(departed.mpc_cruise_accel_max) + + def test_second_nearly_stopped_lead_blocks_departure_confirmation(self): + governor = make_governor() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + mixed = make_radar( + make_lead(status=True, d_rel=20.0, v_lead_k=5.0), + make_lead(status=True, d_rel=200.0, v_lead_k=0.0), + ) + args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0} + update(governor, stopped, **args) + + results = [update(governor, mixed, **args) for _ in range(5)] + + assert all(result.raw_energy_cap > 0.8 for result in results) + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(not result.launching for result in results) + assert governor.live.departure_frames == 0 + + @pytest.mark.parametrize("profile", list(AccelProfile)) + def test_confirmed_departure_launch_is_immediate_bounded_and_profiled(self, profile): + governor = make_governor() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) + args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0, "stock_accel_max": 2.0, "profile": profile} + + for _ in range(3): + update(governor, stopped, **args) + departure = [update(governor, moving, **args) for _ in range(4)] + + assert [result.target_speed for result in departure[:3]] == [args["base_speed"]] * 3 + expected_launch_pace = min(args["base_speed"], departure[-1].live_filtered_cap, args["v_ego"] + LAUNCH_DELTA_V) + assert departure[-1].live_pace == pytest.approx(expected_launch_pace) + assert departure[-1].target_speed == args["base_speed"] + assert departure[-1].effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL) + assert departure[-1].output_accel_max == departure[-1].effective_accel_max def test_stopped_lead_departure_releases_while_mpc_source_remains_lead(self): governor = make_governor() @@ -255,10 +444,11 @@ class TestAccelControllerState: for _ in range(3): update(governor, stopped, **lead_args) - departure = [update(governor, moving, **lead_args) for _ in range(6)] - assert [result.live_pace for result in departure[:5]] == [0.0] * 5 - assert departure[5].live_pace > 0.0 - assert all(result.target_speed == lead_args["base_speed"] for result in departure) + departure = [update(governor, moving, **lead_args) for _ in range(4)] + assert [result.live_pace for result in departure[:3]] == [0.0] * 3 + assert departure[3].live_pace > 0.0 + assert [result.target_speed for result in departure[:3]] == [lead_args["base_speed"]] * 3 + assert departure[-1].target_speed == lead_args["base_speed"] assert len(departure) * DT_MDL < 1.0 continued_release = update(governor, moving, **lead_args) @@ -280,9 +470,9 @@ class TestAccelControllerState: for _ in range(3): update(governor, stopped, **stale_stop_args) - departure = [update(governor, moving, **stale_stop_args) for _ in range(6)] - assert [result.live_pace for result in departure[:5]] == [0.0] * 5 - assert departure[5].live_pace > 0.0 + departure = [update(governor, moving, **stale_stop_args) for _ in range(4)] + assert [result.live_pace for result in departure[:3]] == [0.0] * 3 + assert departure[3].live_pace > 0.0 assert len(departure) * DT_MDL < 1.0 continued_paces = [update(governor, moving, **stale_stop_args).live_pace for _ in range(60)] @@ -311,9 +501,10 @@ class TestAccelControllerState: assert renewed_stop.state == AccelControllerState.stopHold assert renewed_stop.live_pace == 0.0 assert renewed_stop.target_speed == stale_stop_args["base_speed"] + assert renewed_stop.output_accel_max == 0.0 assert not governor.live.departing_from_stop - def test_low_speed_moving_lead_only_warms_mpc_on_first_frame(self): + def test_low_speed_moving_lead_never_bypasses_bounded_pace(self): governor = make_governor() noisy_moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5)) @@ -322,7 +513,7 @@ class TestAccelControllerState: assert first.selected_lead == 0 assert first.live_pace == 0.0 - assert first.target_speed == first.base_speed + assert first.target_speed == first.live_pace assert not governor.live.stopped_lead_hold assert second.target_speed == second.live_pace assert second.target_speed < second.base_speed @@ -337,10 +528,11 @@ class TestAccelControllerState: stopped_evidence = update(governor, stopped, **args) repeated_noise = update(governor, noisy_moving_lead, **args) - assert initial_noise.target_speed == initial_noise.base_speed + assert initial_noise.target_speed == initial_noise.live_pace assert stopped_evidence.state == AccelControllerState.stopHold assert governor.live.stopped_lead_hold - assert repeated_noise.target_speed == repeated_noise.base_speed + assert stopped_evidence.target_speed == stopped_evidence.base_speed + assert repeated_noise.target_speed == args["base_speed"] def test_later_continuously_moving_lead_does_not_latch_stopped_hold(self): governor = make_governor() @@ -366,19 +558,24 @@ class TestAccelControllerState: assert dropout.selected_lead == -1 assert dropout.state == AccelControllerState.stopHold assert dropout.live_pace == 0.0 - assert dropout.target_speed == dropout.live_pace + assert dropout.target_speed == dropout.base_speed + assert dropout.output_accel_max == 0.0 assert governor.live.stopped_lead_hold - def test_no_lead_start_retains_profile_ramp(self): + def test_no_lead_start_launches_immediately_with_profile_limit(self): governor = make_governor() result = update(governor, base_speed=5.0, v_ego=0.1, planner_speed=0.1) assert result.selected_lead == -1 - assert result.live_pace == 0.1 + assert result.launching + assert result.live_pace == pytest.approx(0.1 + LAUNCH_DELTA_V) assert result.target_speed == result.live_pace + assert result.effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL) + assert result.output_accel_max == result.effective_accel_max + assert math.isinf(result.mpc_cruise_accel_max) - def test_confirmed_departure_seeds_filtered_cap_at_start_threshold(self): + def test_confirmed_departure_has_no_later_pace_jump(self): governor = make_governor() stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0)) @@ -399,9 +596,11 @@ class TestAccelControllerState: assert not governor.live.departing_from_stop assert not governor.live.stopped_lead_hold - assert handed_back.live_pace == min(handed_back.base_speed, handed_back.live_filtered_cap) - assert handed_back.live_pace > departing.live_pace + expected_step = AccelController.get_profile_accel_max(AccelProfile.normal, 0.31) * DT_MDL + assert handed_back.live_pace == pytest.approx(departing.live_pace + expected_step) + assert handed_back.live_pace < min(handed_back.base_speed, handed_back.live_filtered_cap) assert handed_back.target_speed == handed_back.live_pace + assert handed_back.mpc_cruise_accel_max == handed_back.output_accel_max @pytest.mark.parametrize( "bypass", @@ -426,6 +625,9 @@ class TestAccelControllerState: assert not result.active assert result.state == AccelControllerState.inactive assert math.isinf(result.live_pace) + assert governor.live.accel_limit is None + assert math.isinf(result.effective_accel_max) + assert math.isinf(result.output_accel_max) def test_disabled_acc_mode_keeps_shadow_running(self): governor = make_governor() diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py index f31de0434b..146b07ae58 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py @@ -1,8 +1,11 @@ from types import SimpleNamespace +import numpy as np import pytest -from cereal import custom +from cereal import custom, messaging +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource @@ -20,6 +23,24 @@ def test_legacy_profile_enum_keeps_toyota_importable(): assert CarState.__module__ == "opendbc.car.toyota.carstate" +def test_mpc_profile_shapes_only_the_cruise_reference(): + radar_state = messaging.new_message('radarState').radarState + mpc = LongitudinalMpc() + mpc.set_cur_state(10.0, 0.0) + mpc.run = lambda: None + + mpc.update(radar_state, 30.0, cruise_accel_max=0.5) + shaped_params = mpc.params.copy() + mpc.update(radar_state, 30.0) + stock_params = mpc.params.copy() + mpc.update(radar_state, 30.0, cruise_accel_max=np.inf) + + np.testing.assert_array_equal(shaped_params[:, 0], ACCEL_MIN) + np.testing.assert_array_equal(shaped_params[:, 1], ACCEL_MAX) + assert np.any(shaped_params[:, 2] < stock_params[:, 2]) + np.testing.assert_array_equal(mpc.params, stock_params) + + def test_shadow_target_telemetry_publishes_filtered_cap(): planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) planner.source = LongitudinalPlanSource.cruise @@ -31,6 +52,7 @@ def test_shadow_target_telemetry_publishes_filtered_cap(): enabled=True, active=False, shadow_active=True, + launching=False, profile=AccelProfile.normal, state=AccelControllerState.inactive, shadow_state=AccelControllerState.restrict, @@ -43,6 +65,10 @@ def test_shadow_target_telemetry_publishes_filtered_cap(): usable_gap=30.0, closing_speed=5.0, required_decel=0.4, + profile_accel_max=1.0, + effective_accel_max=0.85, + output_accel_max=0.85, + mpc_cruise_accel_max=0.85, ) planner.scc = SimpleNamespace( vision=SimpleNamespace( @@ -84,3 +110,5 @@ def test_shadow_target_telemetry_publishes_filtered_cap(): telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController assert telemetry.vTargetShadow == pytest.approx(planner.accel_controller_result.shadow_filtered_cap) assert telemetry.vTargetShadow != pytest.approx(planner.accel_controller_result.shadow_pace) + assert telemetry.aMaxProfile == pytest.approx(planner.accel_controller_result.profile_accel_max) + assert telemetry.aMaxEffective == pytest.approx(planner.accel_controller_result.effective_accel_max) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 0d400b3bf2..117cff6c86 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -40,7 +40,7 @@ class LongitudinalPlannerSP: self.accel_controller = AccelController(CP, dt=dt) self.accel_controller_result = None - self._param_read_frames = max(1, int(round(1.0 / dt))) + self._param_read_frames = max(1, int(round(0.25 / dt))) self._param_frame = 0 self.accel_personality_enabled = False self.accel_personality = int(AccelProfile.normal) @@ -96,7 +96,7 @@ class LongitudinalPlannerSP: def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool, acc_selected: bool, planner_speed: float, previous_mpc_source, previous_should_stop: bool, - controller_fault: bool = False) -> float: + stock_accel_max: float, planner_accel: float, controller_fault: bool = False) -> float: self.accel_controller_result = self.accel_controller.update( sm['radarState'], base_speed=base_speed, @@ -110,6 +110,8 @@ class LongitudinalPlannerSP: cruise_initialized=cruise_initialized, previous_mpc_source=previous_mpc_source, planner_speed=planner_speed, + stock_accel_max=stock_accel_max, + planner_accel=planner_accel, previous_should_stop=previous_should_stop, controller_fault=controller_fault, ) @@ -155,6 +157,8 @@ class LongitudinalPlannerSP: accel_controller.usableGap = float(result.usable_gap) accel_controller.closingSpeed = float(result.closing_speed) accel_controller.requiredDecel = float(result.required_decel) + accel_controller.aMaxProfile = float(result.profile_accel_max) + accel_controller.aMaxEffective = float(result.effective_accel_max) # Smart Cruise Control smartCruiseControl = longitudinalPlanSP.smartCruiseControl diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 0ba3b4f907..661902a2a0 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -22,9 +22,13 @@ class ClosedLoopTrace: source: list active: np.ndarray shadow_active: np.ndarray + launching: np.ndarray pace: np.ndarray filtered_cap: np.ndarray selected_lead: np.ndarray + profile_accel_max: np.ndarray + effective_accel_max: np.ndarray + output_accel_max: np.ndarray solver_failures: int @@ -75,9 +79,13 @@ def _run( result["fcw"], controller.active, controller.shadow_active, + controller.launching, controller.live_pace, controller.live_filtered_cap, controller.selected_lead, + controller.profile_accel_max, + controller.effective_accel_max, + controller.output_accel_max, ) ) sources.append(result["mpc_source"]) @@ -95,9 +103,13 @@ def _run( source=sources, active=data[:, 8].astype(bool), shadow_active=data[:, 9].astype(bool), - pace=data[:, 10], - filtered_cap=data[:, 11], - selected_lead=data[:, 12].astype(int), + launching=data[:, 10].astype(bool), + pace=data[:, 11], + filtered_cap=data[:, 12], + selected_lead=data[:, 13].astype(int), + profile_accel_max=data[:, 14], + effective_accel_max=data[:, 15], + output_accel_max=data[:, 16], solver_failures=solver_failures, ) @@ -170,6 +182,19 @@ def test_non_actuating_modes_are_bit_exact(plant_kwargs, expect_shadow_active): assert not shadow.shadow_active.any() +def test_disabled_profiles_are_bit_exact_in_engaged_acc(): + common = dict(duration=2.0, controller_enabled=False, lead_relevancy=True, speed=20.0, distance_lead=70.0, v_lead=14.0) + traces = [_run(profile=profile, **common) for profile in range(3)] + + for trace in traces[1:]: + np.testing.assert_allclose(trace.a_target, traces[0].a_target, atol=1e-6, rtol=0.0) + np.testing.assert_array_equal(trace.should_stop, traces[0].should_stop) + np.testing.assert_array_equal(trace.fcw, traces[0].fcw) + assert trace.source == traces[0].source + assert all(not trace.active.any() for trace in traces) + assert all(np.isinf(trace.output_accel_max).all() for trace in traces) + + def test_dec_radar_lead_selects_acc_and_standstill_uses_shadow_only(): blended = _run( duration=2.0, @@ -295,10 +320,6 @@ def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal(): assert not np.any(disturbance[positive[0] + 1:] < -0.2) -@pytest.mark.xfail( - reason="opt-in validation: raw stock lead MPC dominates a zero cruise cap during repeated slow-lead stop/go", - strict=True, -) def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal(): def lead_speed(current_time: float) -> float: return float(0.1 * (1.0 - np.cos(np.pi * current_time))) @@ -318,7 +339,7 @@ def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal(): settled = trace.time >= 4.0 assert trace.active[settled].all() assert np.all(trace.pace[settled] == 0.0) - assert np.any(trace.a_target[settled] > 0.2) + assert np.max(trace.a_target[settled]) <= 0.2 assert not _has_propulsion_brake_reversal(trace, after=4.0) @@ -392,6 +413,7 @@ def test_stopped_lead_noise_requires_four_departure_frames_and_launches_within_o record_property("predeparture_peak_command", float(np.max(trace.a_target[before_departure]))) record_property("first_three_departure_frames_peak_command", float(np.max(trace.a_target[first_three_departure_frames]))) assert np.max(trace.speed[first_three_departure_frames]) < 1e-3 + assert not trace.launching[first_three_departure_frames].any() launched = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) assert len(launched) @@ -427,6 +449,43 @@ def test_stop_hold_two_frame_total_lead_dropout_cannot_launch(): assert not _has_propulsion_brake_reversal(trace, after=0.5) +def test_clear_road_launch_is_immediate_bounded_and_profiles_feel_distinct(): + common = dict( + duration=6.0, + controller_enabled=True, + lead_relevancy=False, + speed=0.0, + v_cruise=15.0, + actuator_delay=0.15, + actuator_lag=0.20, + ) + traces = [_run(profile=profile, **common) for profile in range(3)] + + onset_times = [] + movement_times = [] + for trace in traces: + positive = np.flatnonzero(trace.a_target > 0.05) + moving = np.flatnonzero(trace.speed > 0.01) + assert len(positive) + assert len(moving) + onset_times.append(float(trace.time[positive[0]])) + movement_times.append(float(trace.time[moving[0]])) + assert np.all(trace.a_target <= trace.output_accel_max + 1e-6) + assert trace.solver_failures == 0 + + assert max(onset_times) - min(onset_times) <= DT_MDL + assert max(onset_times) <= 4 * DT_MDL + assert max(movement_times) <= 1.0 + + for sample_time in (2.0,): + realized = [float(trace.acceleration[np.searchsorted(trace.time, sample_time)]) for trace in traces] + assert realized[0] < realized[1] < realized[2], (sample_time, realized) + final_speeds = [trace.speed[-1] for trace in traces] + assert final_speeds[0] < final_speeds[1] < final_speeds[2] + assert final_speeds[1] - final_speeds[0] > 0.5 + assert final_speeds[2] - final_speeds[1] > 0.4 + + @pytest.mark.parametrize( ("actuator_delay", "actuator_lag", "current_tn_jerk_p95"), [