diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 8b62808dc0..4deb13b378 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_model = 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, @@ -149,7 +154,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP): candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)] if is_e2e: - candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) + candidates.append((output_a_target_model, LongitudinalPlanSource.e2e, output_should_stop_e2e)) + elif self.model_accel_transition.active: + candidates[0] = (output_a_target_model, self.mpc.source, output_should_stop_mpc) output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) self.output_should_stop = any(should_stop for _, _, should_stop in candidates) 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..60840453d2 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,51 @@ class ModeTransitionManager: return self.current_mode +class ModelAccelTransition: + """Smooths acceleration while DEC changes modes.""" + + def __init__(self, dt: float = DT_MDL): + self._max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * dt + self._accel = 0.0 + self._active = False + self._blended = False + + def reset(self) -> None: + self._active = False + self._blended = False + + @property + def active(self) -> bool: + return self._active + + 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 blended != self._blended: + self._accel = previous_accel + self._active = True + self._blended = blended + + if urgent and selected_accel <= self._accel: + self._accel = selected_accel + self._active = True + return selected_accel + if not self._active: + return selected_accel + + transition_target = model_accel if blended else mpc_accel + preview_accel = max(self._accel - self._max_step, min(self._accel + self._max_step, transition_target)) + output_accel = min(mpc_accel, preview_accel) + self._accel = output_accel + if mpc_accel >= transition_target and math.isclose(preview_accel, transition_target, abs_tol=1e-9): + self._active = False + return output_accel + + class DynamicExperimentalController: def __init__(self, CP: structs.CarParams, mpc, params=None): self._CP = CP 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..8b86880690 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,56 @@ 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_entry_and_exit_are_rate_bounded(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") + + output = transition.update(0.0, -1.50, output, blended=True) + self.assertAlmostEqual(output, -1.50) + self.assertAlmostEqual(transition.update(0.0, -0.20, output, blended=False), -1.35) + + def test_harder_mpc_braking_bypasses_exit_ramp(self): + transition = ModelAccelTransition() + self.assertAlmostEqual(transition.update(2.0, 0.76, 0.76, blended=True), 0.76) + self.assertAlmostEqual(transition.update(-2.0, 0.76, 0.76, blended=False), -2.0) + + def test_urgent_release_remains_rate_bounded(self): + transition = ModelAccelTransition() + self.assertAlmostEqual(transition.update(-1.0, -1.0, -1.0, blended=True), -1.0) + self.assertAlmostEqual(transition.update(1.0, -1.0, -1.0, blended=False, urgent=True), -0.85) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f1e0c36416..125b81a908 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,7 +9,7 @@ from openpilot.cereal import messaging, custom from opendbc.car import structs from openpilot.common.constants import CV from openpilot.selfdrive.car.cruise import V_CRUISE_MAX -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 @@ -26,6 +26,7 @@ class LongitudinalPlannerSP: self.events_sp = EventsSP() self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) + self.model_accel_transition = ModelAccelTransition(mpc.dt) self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() self.sla = SpeedLimitAssist(CP, CP_SP) @@ -35,6 +36,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 @@ -43,6 +45,12 @@ 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: + return self.model_accel_transition.update( + mpc_accel, model_accel, self.previous_plan_accel, blended=blended, urgent=should_stop or fcw, reset=reset or not self.dec.active(), + ) + def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: CS = sm['carState'] v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)