diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index e89e4b0574..172bd52ce6 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -77,6 +77,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): def update(self, sm): LongitudinalPlannerSP.update(self, sm) + self.previous_plan_accel = self.output_a_target if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) @@ -140,6 +141,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_should_stop_e2e = sm['modelV2'].action.shouldStop is_e2e = self.is_e2e(sm) + output_a_target_e2e = self.select_model_accel( + output_a_target_mpc, output_a_target_e2e, blended=is_e2e, + should_stop=output_should_stop_e2e or output_should_stop_mpc, fcw=self.fcw, reset=reset_state, + ) self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, self.a_cruise, steer_angle_without_offset, self.CP, self.dt, diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index 4fdd8b86f7..5f7d4b0cb7 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -80,9 +80,10 @@ class AccelController: comfort_decel = COMFORT_DECEL[self.profile] return max(speed_error / EARLY_DECEL_RESPONSE_TIME, -comfort_decel) - def _update_early_decel(self, raw_target: float) -> None: + def _update_early_decel(self, raw_target: float, previous_plan_accel: float) -> None: raw_target = min(float(raw_target), 0.0) - previous = self._early_decel if self._early_decel is not None else 0.0 + plan_accel = float(previous_plan_accel) if math.isfinite(previous_plan_accel) else 0.0 + previous = self._early_decel if self._early_decel is not None else max(plan_accel, 0.0) if raw_target < previous - EARLY_DECEL_EPSILON: updated = max(raw_target, previous - EARLY_DECEL_TIGHTEN_RATE * self.dt) @@ -94,7 +95,7 @@ class AccelController: updated = raw_target state = AccelControllerState.hold if updated < -EARLY_DECEL_EPSILON else AccelControllerState.free - if updated >= -EARLY_DECEL_EPSILON: + if raw_target >= -EARLY_DECEL_EPSILON and updated >= -EARLY_DECEL_EPSILON: self._early_decel = None self.early_decel = None self.state = AccelControllerState.free @@ -105,7 +106,7 @@ class AccelController: def update(self, radar_state, *, v_ego: float, a_ego: float, v_cruise: float, follow_personality, engaged: bool, cruise_initialized: bool, acc_selected: bool, stock_accel_max: float, - radar_fresh: bool = True, force_decel: bool = False) -> AccelDecision: + radar_fresh: bool = True, force_decel: bool = False, previous_plan_accel: float = 0.0) -> AccelDecision: self.profile = sanitize_profile(self.profile) valid_context = self._valid_context( v_ego=v_ego, a_ego=a_ego, v_cruise=v_cruise, stock_accel_max=stock_accel_max, @@ -131,7 +132,7 @@ class AccelController: self.selected_lead = lead_plan.selected_lead self.selected_lead_track_id = lead_plan.selected_lead_track_id self.required_decel = lead_plan.required_decel - self._update_early_decel(self._raw_early_decel(lead_plan)) + self._update_early_decel(self._raw_early_decel(lead_plan), previous_plan_accel) profile_binding = self.cruise_accel_max < positive_stock_max - EARLY_DECEL_EPSILON self.is_active = profile_binding or self.early_decel is not None diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 859ff0dcf0..270adc3a25 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -119,6 +119,13 @@ class TestAccelDecision(OpenpilotTestCase): class TestEarlyDecel(OpenpilotTestCase): + def test_early_decel_starts_from_the_previous_positive_plan(self): + instance = controller() + decision = update(instance, restrictive_radar(), previous_plan_accel=0.48) + + self.assertIsNotNone(decision.early_decel) + self.assertAlmostEqual(decision.early_decel, 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL) + def test_early_decel_is_nonpositive_and_tightens_at_bound(self): instance = controller() samples = [0.0] diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py index 397e0b4b31..b9a6457ee8 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py @@ -9,7 +9,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel, get_max_accel from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( - ACCEL_PROFILES, AccelProfile, profile_accel_scale, + ACCEL_PROFILES, EARLY_DECEL_TIGHTEN_RATE, AccelProfile, profile_accel_scale, ) from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource @@ -57,6 +57,7 @@ def planner_for_hook(*, enabled=True, profile=AccelProfile.normal): dynamic_planner.accel_controller = accel_controller(enabled=enabled, profile=profile) dynamic_planner.dec = SimpleNamespace(active=lambda: False) dynamic_planner.output_v_target = 30.0 + dynamic_planner.previous_plan_accel = 0.0 dynamic_planner._radar_fresh_this_cycle = True return planner @@ -119,6 +120,18 @@ class TestPlannerHook(OpenpilotTestCase): self.assertEqual(result[-1][1:], (MpcSource.cruise, False)) self.assertLessEqual(result[-1][0], 0.0) + def test_early_decel_enters_from_the_previous_positive_command(self): + planner = planner_for_hook(profile=AccelProfile.sport) + planner.previous_plan_accel = 0.48 + sm = PlannerSM(radar_state=radar(lead(present=True, distance=25.0, speed=5.0))) + candidates = [(-0.02, MpcSource.lead0, False), (0.60, MpcSource.cruise, False)] + + result = planner.update_accel_controller(sm, candidates) + + self.assertEqual(result[:2], candidates) + self.assertAlmostEqual(result[-1][0], 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL) + self.assertEqual(result[-1][1:], (MpcSource.cruise, False)) + def test_radar_freshness_requires_a_healthy_advanced_message(self): planner = planner_for_hook() planner._radar_log_mono_time = None diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py index 4586afbc9f..e8afd79e7a 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -1,4 +1,6 @@ class WMACConstants: + MODEL_ACCEL_TRANSITION_RATE = 3.0 + # Lead detection parameters LEAD_WINDOW_SIZE = 6 # Stable detection window LEAD_PROB = 0.45 # Balanced threshold for lead detection diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py index fb854edae8..dc3551768e 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -6,13 +6,15 @@ See the LICENSE.md file in the root directory for more details. """ # Version = 2025-6-30 +import math +from typing import Literal + from openpilot.cereal import messaging from opendbc.car import structs from numpy import interp from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants -from typing import Literal # d-e2e, from modeldata.h TRAJECTORY_SIZE = 33 @@ -130,6 +132,49 @@ class ModeTransitionManager: return self.current_mode +class ModelAccelTransition: + """Smooths model acceleration while DEC enters blended mode.""" + + def __init__(self, dt: float = DT_MDL): + self._max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * dt + self._accel = 0.0 + self._active = False + self._was_blended = False + + def reset(self) -> None: + self._active = False + self._was_blended = False + + def update(self, mpc_accel: float, model_accel: float, previous_accel: float, *, blended: bool, + urgent: bool = False, reset: bool = False) -> float: + selected_accel = min(mpc_accel, model_accel) if blended else mpc_accel + if reset or not all(math.isfinite(accel) for accel in (mpc_accel, model_accel, previous_accel)): + self.reset() + return selected_accel + if not blended: + self.reset() + return selected_accel + + if not self._was_blended: + self._accel = previous_accel + self._active = True + self._was_blended = True + + if urgent: + self._accel = selected_accel + self._active = True + return selected_accel + if not self._active: + return selected_accel + + preview_accel = max(self._accel - self._max_step, min(self._accel + self._max_step, model_accel)) + output_accel = min(mpc_accel, preview_accel) + self._accel = output_accel + if mpc_accel >= model_accel and math.isclose(preview_accel, model_accel, abs_tol=1e-9): + self._active = False + return output_accel + + class DynamicExperimentalController: def __init__(self, CP: structs.CarParams, mpc, params=None): self._CP = CP @@ -197,6 +242,9 @@ class DynamicExperimentalController: def active(self) -> bool: return self._active + def accel_transition_urgent(self) -> bool: + return self._has_mpc_fcw or (self._has_slow_down and self._urgency > 0.7) + def set_mpc_fcw_crash_cnt(self) -> None: """Set MPC FCW crash count""" self._mpc_fcw_crash_cnt = self._mpc.crash_cnt diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py index 0e83ef7c99..86746b4ce9 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py @@ -33,6 +33,9 @@ class MockDec: def enabled(self) -> bool: return True + def accel_transition_urgent(self) -> bool: + return False + class MockSubMaster(dict): def __init__(self, services: dict): diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index 4fec6eaa52..1f73ec1e1d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -1,5 +1,7 @@ +from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition class MockLeadOne: def __init__(self, present=0.0): @@ -89,3 +91,45 @@ class TestDynamicExperimentalController(OpenpilotTestCase): controller.update(default_sm) assert controller.mode() == "blended" + + +class TestModelAccelTransition(OpenpilotTestCase): + def test_blended_entry_is_rate_bounded(self): + transition = ModelAccelTransition() + previous = 0.30 + outputs = [] + for _ in range(10): + previous = transition.update(0.50, -0.88, previous, blended=True) + outputs.append(previous) + + max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * DT_MDL + self.assertAlmostEqual(outputs[0], 0.15) + assert min(b - a for a, b in zip([0.30, *outputs[:-1]], outputs, strict=True)) >= -max_step - 1e-9 + self.assertAlmostEqual(outputs[-1], -0.88) + + def test_harder_mpc_braking_is_immediate(self): + transition = ModelAccelTransition() + self.assertAlmostEqual(transition.update(-2.0, -0.88, 0.30, blended=True), -2.0) + + def test_urgent_model_braking_is_immediate(self): + transition = ModelAccelTransition() + self.assertAlmostEqual(transition.update(0.0, -2.0, 0.30, blended=True, urgent=True), -2.0) + self.assertAlmostEqual(transition.update(0.0, 0.0, -2.0, blended=True), -1.85) + + def test_harder_mpc_release_is_rate_bounded(self): + transition = ModelAccelTransition() + self.assertAlmostEqual(transition.update(-2.0, -0.5, 0.30, blended=True), -2.0) + self.assertAlmostEqual(transition.update(0.0, -0.5, -2.0, blended=True), -1.85) + + def test_only_entry_is_shaped(self): + transition = ModelAccelTransition() + output = 0.30 + for _ in range(20): + output = transition.update(0.0, -0.88, output, blended=True) + if output <= -0.88: + break + else: + self.fail("transition did not converge") + + self.assertAlmostEqual(transition.update(0.0, -1.50, output, blended=True), -1.50) + self.assertAlmostEqual(transition.update(0.0, -0.20, output, blended=False), 0.0) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index e4ad668606..62ca1e625c 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -12,7 +12,7 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist @@ -29,6 +29,7 @@ class LongitudinalPlannerSP: self.accel_controller = AccelController(CP, dt=mpc.dt) self.events_sp = EventsSP() self.dec = DynamicExperimentalController(CP, mpc) + self.model_accel_transition = ModelAccelTransition(mpc.dt) self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() self.sla = SpeedLimitAssist(CP, CP_SP) @@ -40,6 +41,7 @@ class LongitudinalPlannerSP: self.output_v_target = 0. self.output_a_target = 0. + self.previous_plan_accel = 0. def is_e2e(self, sm: messaging.SubMaster) -> bool: experimental_mode = sm['selfdriveState'].experimentalMode @@ -48,6 +50,13 @@ class LongitudinalPlannerSP: return experimental_mode and self.dec.mode() == "blended" + def select_model_accel(self, mpc_accel: float, model_accel: float, *, blended: bool, + should_stop: bool, fcw: bool, reset: bool) -> float: + urgent = should_stop or fcw or self.dec.accel_transition_urgent() + return self.model_accel_transition.update( + mpc_accel, model_accel, self.previous_plan_accel, blended=blended, urgent=urgent, reset=reset or not self.dec.active(), + ) + def update_accel_controller(self, sm: messaging.SubMaster, candidates): cruise_index = next((i for i, candidate in enumerate(candidates) if candidate[1] == MpcSource.cruise), -1) if cruise_index < 0: @@ -63,7 +72,7 @@ class LongitudinalPlannerSP: follow_personality=sm['selfdriveState'].personality, engaged=not reset_state, cruise_initialized=CS.vCruise != V_CRUISE_UNSET, acc_selected=not self.is_e2e(sm), stock_accel_max=max(cruise_accel, 0.0), radar_fresh=self._radar_fresh_this_cycle, - force_decel=sm['controlsState'].forceDecel, + force_decel=sm['controlsState'].forceDecel, previous_plan_accel=self.previous_plan_accel, ) if decision.cruise_accel_max is None and decision.early_decel is None: return candidates diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 98ccb00f5e..d25de9d161 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -92,3 +92,38 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase): self.assertTrue(any(candidate[2] for candidate in augmented)) self.assertTrue(result["should_stop"]) self.assertAlmostEqual(result["a_target"], min(augmented, key=lambda candidate: candidate[0])[0]) + + def test_dec_blended_entry_limits_the_first_model_brake_step(self): + class DecStub: + mode_name = "acc" + + def update(self, _sm): + pass + + def active(self): + return True + + def mode(self): + return self.mode_name + + def accel_transition_urgent(self): + return False + + plant = Plant( + enabled=True, e2e=True, speed=22.0, + model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False), + actuator_delay=0.15, actuator_lag=0.20, + ) + configure(plant) + dec = DecStub() + plant.planner.dec = dec + + outputs = [] + for frame in range(20): + if frame == 10: + dec.mode_name = "blended" + result = plant.step(v_lead=0.0, v_cruise=22.0) + outputs.append(result["a_target"]) + + self.assertGreaterEqual(outputs[10] - outputs[9], -0.15 - 1e-9) + self.assertFalse(result["fcw"])