diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index cfbbbe472..3da899919 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -196,7 +196,6 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { } struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { - dec @0 :DynamicExperimentalControl; longitudinalPlanSource @1 :LongitudinalPlanSource; smartCruiseControl @2 :SmartCruiseControl; speedLimit @3 :SpeedLimit; @@ -205,7 +204,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { events @6 :List(OnroadEventSP.Event); e2eAlerts @7 :E2eAlerts; - struct DynamicExperimentalControl { + deprecated :group { + dec @0 :DynamicExperimentalControlDEPRECATED; + } + + struct DynamicExperimentalControlDEPRECATED { state @0 :DynamicExperimentalControlState; enabled @1 :Bool; active @2 :Bool; diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 9f5f05ceb..d94ab17ab 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -243,7 +243,6 @@ inline static std::unordered_map keys = { {"ToyotaAftermarketBsm", {PERSISTENT | BACKUP, BOOL, "0"}}, {"VagAvoidEPSLockout", {PERSISTENT | BACKUP, BOOL, "0"}}, - {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}}, // sunnypilot model params diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index ddfe383d6..2e45b1821 100755 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -127,9 +127,6 @@ class Car: set_alternative_experience(self.CP, self.CP_SP, self.params) set_car_specific_params(self.CP, self.CP_SP, self.params) - # Dynamic Experimental Control - self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl") - openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly self.CP.passive = not controller_available or self.CP.dashcamOnly @@ -214,7 +211,7 @@ class Car: self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric) if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on - self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.dynamic_experimental_control) + self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode) # TODO: mirror the carState.cruiseState struct? CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) @@ -302,7 +299,6 @@ class Car: self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl # sunnypilot - self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl") self.v_cruise_helper.read_custom_set_speed_params() time.sleep(0.1) diff --git a/openpilot/selfdrive/car/cruise.py b/openpilot/selfdrive/car/cruise.py index b554c1a97..9aed674e3 100644 --- a/openpilot/selfdrive/car/cruise.py +++ b/openpilot/selfdrive/car/cruise.py @@ -138,13 +138,12 @@ class VCruiseHelper(VCruiseHelperSP): self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool, dynamic_experimental_control: bool) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return - initial_experimental_mode = experimental_mode and not dynamic_experimental_control - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_experimental_mode else V_CRUISE_INITIAL + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized: self.v_cruise_kph = self.v_cruise_kph_last diff --git a/openpilot/selfdrive/car/tests/test_cruise_speed.py b/openpilot/selfdrive/car/tests/test_cruise_speed.py index 60af34683..c38c7c717 100644 --- a/openpilot/selfdrive/car/tests/test_cruise_speed.py +++ b/openpilot/selfdrive/car/tests/test_cruise_speed.py @@ -59,16 +59,16 @@ class TestVCruiseHelper(OpenpilotTestCase): for _ in range(2): self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) - def enable(self, v_ego, experimental_mode, dynamic_experimental_control): + def enable(self, v_ego, experimental_mode): # Simulates user pressing set with a current speed - self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode, dynamic_experimental_control) + self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode) def test_adjust_speed(self): """ Asserts speed changes on falling edges of buttons. """ - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) for btn in (ButtonType.accelCruise, ButtonType.decelCruise): for pressed in (True, False): @@ -92,7 +92,7 @@ class TestVCruiseHelper(OpenpilotTestCase): CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) if pressed: - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) # Expected diff on enabling. Speed should not change on falling edge of pressed assert (not pressed) == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) @@ -102,7 +102,7 @@ class TestVCruiseHelper(OpenpilotTestCase): Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. """ - self.enable(0, False, False) + self.enable(0, False) for standstill in (True, False): for pressed in (True, False): @@ -122,7 +122,7 @@ class TestVCruiseHelper(OpenpilotTestCase): for v_ego in np.linspace(0, 100, 101): self.reset_cruise_speed_state() - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) # first decrement speed, then perform gas pressed logic expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT @@ -144,11 +144,10 @@ class TestVCruiseHelper(OpenpilotTestCase): """ for experimental_mode in (True, False): - for dynamic_experimental_control in (True, False): - for v_ego in np.linspace(0, 100, 101): - self.reset_cruise_speed_state() - assert not self.v_cruise_helper.v_cruise_initialized + for v_ego in np.linspace(0, 100, 101): + self.reset_cruise_speed_state() + assert not self.v_cruise_helper.v_cruise_initialized - self.enable(float(v_ego), experimental_mode, dynamic_experimental_control) - assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX - assert self.v_cruise_helper.v_cruise_initialized + self.enable(float(v_ego), experimental_mode) + assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX + assert self.v_cruise_helper.v_cruise_initialized diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index bf91c5e1f..74dbfc0dd 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -138,16 +138,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - is_e2e = self.is_e2e(sm) + experimental_mode = sm['selfdriveState'].experimentalMode - self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, + self.a_cruise = get_cruise_accel(experimental_mode, v_cruise, v_ego, self.a_cruise, steer_angle_without_offset, self.CP, self.dt, accel_coast, self.allow_throttle) cruise_should_stop = should_stop(v_ego, self.a_cruise) candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)] - if is_e2e: + if experimental_mode: candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py index c2c4b0597..1b78200d3 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py @@ -84,16 +84,10 @@ class CruiseLayout(Widget): callback=lambda: self._set_current_panel(PanelType.SLA) ) - self.dec_toggle = toggle_item_sp( - title=lambda: tr("Enable Dynamic Experimental Control"), - description=lambda: tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."), - param="DynamicExperimentalControl") - items = [ self._toggles_layout._toggles["ExperimentalMode"], self._toggles_layout._toggles["LongitudinalPersonality"], self._toggles_layout._toggles["IsMetric"], - self.dec_toggle, self.scc_v_toggle, self.custom_acc_toggle, self.custom_acc_short_increment, @@ -149,16 +143,13 @@ class CruiseLayout(Widget): if has_long or has_icbm: self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad()) - self.dec_toggle.action_item.set_enabled(has_long) self.scc_v_toggle.action_item.set_enabled(True) self.scc_m_toggle.action_item.set_enabled(True) else: ui_state.params.remove("CustomAccIncrementsEnabled") - ui_state.params.remove("DynamicExperimentalControl") ui_state.params.remove("SmartCruiseControlVision") ui_state.params.remove("SmartCruiseControlMap") self.custom_acc_toggle.action_item.set_enabled(False) - self.dec_toggle.action_item.set_enabled(False) self.scc_v_toggle.action_item.set_enabled(False) self.scc_m_toggle.action_item.set_enabled(False) diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index d130c3862..f70da2b48 100644 --- a/openpilot/selfdrive/ui/sunnypilot/ui_state.py +++ b/openpilot/selfdrive/ui/sunnypilot/ui_state.py @@ -221,10 +221,9 @@ class UIStateSP: self.params.remove("LateralJerkTorqueController") self.params.remove("AlphaLongitudinalEnabled") - # No longitudinal control: no experimental mode or DEC + # No longitudinal control: no experimental mode if not has_long: self.params.remove("ExperimentalMode") - self.params.remove("DynamicExperimentalControl") # ICBM: clear if not available or if full longitudinal control is active if self.CP_SP is not None: diff --git a/openpilot/sunnypilot/selfdrive/car/interfaces.py b/openpilot/sunnypilot/selfdrive/car/interfaces.py index edc918da5..7df77688d 100644 --- a/openpilot/sunnypilot/selfdrive/car/interfaces.py +++ b/openpilot/sunnypilot/selfdrive/car/interfaces.py @@ -81,7 +81,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed: cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params") - params.remove("DynamicExperimentalControl") params.remove("CustomAccIncrementsEnabled") params.remove("SmartCruiseControlVision") params.remove("SmartCruiseControlMap") diff --git a/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py b/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py index 56491e351..257f12151 100644 --- a/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py +++ b/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py @@ -57,7 +57,7 @@ class TestCustomAccIncrements(TestVCruiseHelper): def test_default_behavior_when_disabled(self): """Test that default increments are used when custom ACC is disabled""" self.set_custom_increments(enabled=False, short_inc=5, long_inc=10) - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) initial_speed = self.v_cruise_helper.v_cruise_kph @@ -69,7 +69,7 @@ class TestCustomAccIncrements(TestVCruiseHelper): def test_custom_short_press_increments(self, increment): """Test custom short press increments (1-10)""" self.set_custom_increments(enabled=True, short_inc=increment, long_inc=5) - self.enable(50 * CV.KPH_TO_MS, False, False) + self.enable(50 * CV.KPH_TO_MS, False) initial_speed = self.v_cruise_helper.v_cruise_kph self.press_button_short(ButtonType.accelCruise) @@ -86,7 +86,7 @@ class TestCustomAccIncrements(TestVCruiseHelper): def test_custom_long_press_increments(self, increment): """Test custom long press increments (1, 5, 10)""" self.set_custom_increments(enabled=True, short_inc=1, long_inc=increment) - self.enable(50 * CV.KPH_TO_MS, False, False) + self.enable(50 * CV.KPH_TO_MS, False) initial_speed = self.v_cruise_helper.v_cruise_kph self.press_button_long(ButtonType.accelCruise) @@ -103,7 +103,7 @@ class TestCustomAccIncrements(TestVCruiseHelper): def test_accel_decel_symmetry(self, button_type): """Test that acceleration and deceleration work symmetrically""" self.set_custom_increments(enabled=True, short_inc=3, long_inc=5) - self.enable(50 * CV.KPH_TO_MS, False, False) + self.enable(50 * CV.KPH_TO_MS, False) initial_speed = self.v_cruise_helper.v_cruise_kph self.press_button_short(button_type) @@ -123,7 +123,7 @@ class TestCustomAccIncrements(TestVCruiseHelper): for initial, increment, expected in test_cases: self.set_custom_increments(enabled=True, short_inc=increment, long_inc=increment) self.reset_cruise_speed_state() - self.enable(initial * CV.KPH_TO_MS, False, False) + self.enable(initial * CV.KPH_TO_MS, False) self.press_button_short(ButtonType.accelCruise) assert self.v_cruise_helper.v_cruise_kph == expected @@ -132,7 +132,7 @@ class TestCustomAccIncrements(TestVCruiseHelper): """Test that invalid values fallback to safe defaults""" # Test invalid short increment self.set_custom_increments(enabled=True, short_inc=-1, long_inc=5) - self.enable(50 * CV.KPH_TO_MS, False, False) + self.enable(50 * CV.KPH_TO_MS, False) initial_speed = self.v_cruise_helper.v_cruise_kph self.press_button_short(ButtonType.accelCruise) @@ -141,7 +141,7 @@ class TestCustomAccIncrements(TestVCruiseHelper): # Test invalid long increment self.reset_cruise_speed_state() self.set_custom_increments(enabled=True, short_inc=1, long_inc=99) - self.enable(50 * CV.KPH_TO_MS, False, False) + self.enable(50 * CV.KPH_TO_MS, False) initial_speed = self.v_cruise_helper.v_cruise_kph self.press_button_long(ButtonType.accelCruise) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/__init__.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py deleted file mode 100644 index 4586afbc9..000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ /dev/null @@ -1,17 +0,0 @@ -class WMACConstants: - # Lead detection parameters - LEAD_WINDOW_SIZE = 6 # Stable detection window - LEAD_PROB = 0.45 # Balanced threshold for lead detection - - # Slow down detection parameters - SLOW_DOWN_WINDOW_SIZE = 5 # Responsive but stable - SLOW_DOWN_PROB = 0.3 # Balanced threshold for slow down scenarios - - # Optimized slow down distance curve - smooth and progressive - SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] - SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.] - - # Slowness detection parameters - SLOWNESS_WINDOW_SIZE = 10 # Stable slowness detection - SLOWNESS_PROB = 0.55 # Clear threshold for slowness - SLOWNESS_CRUISE_OFFSET = 1.025 # Conservative cruise speed offset diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py deleted file mode 100644 index fb854edae..000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ /dev/null @@ -1,388 +0,0 @@ -""" -Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -# Version = 2025-6-30 - -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 -SET_MODE_TIMEOUT = 15 - -# Define the valid mode types -ModeType = Literal['acc', 'blended'] - - -class SmoothKalmanFilter: - """Enhanced Kalman filter with smoothing for stable decision making.""" - - def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01, - alpha=1.0, smoothing_factor=0.85): - self.x = initial_value - self.P = 1.0 - self.R = measurement_noise - self.Q = process_noise - self.alpha = alpha - self.smoothing_factor = smoothing_factor - self.initialized = False - self.history = [] - self.max_history = 10 - self.confidence = 0.0 - - def add_data(self, measurement): - if len(self.history) >= self.max_history: - self.history.pop(0) - self.history.append(measurement) - - if not self.initialized: - self.x = measurement - self.initialized = True - self.confidence = 0.1 - return - - self.P = self.alpha * self.P + self.Q - - K = self.P / (self.P + self.R) - effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1 - - innovation = measurement - self.x - self.x = self.x + effective_K * innovation - self.P = (1 - effective_K) * self.P - - if abs(innovation) < 0.1: - self.confidence = min(1.0, self.confidence + 0.05) - else: - self.confidence = max(0.1, self.confidence - 0.02) - - def get_value(self): - return self.x if self.initialized else None - - def get_confidence(self): - return self.confidence - - def reset_data(self): - self.initialized = False - self.history = [] - self.confidence = 0.0 - - -class ModeTransitionManager: - """Manages smooth transitions between driving modes with hysteresis.""" - - def __init__(self): - self.current_mode: ModeType = 'acc' - self.mode_confidence = {'acc': 1.0, 'blended': 0.0} - self.transition_timeout = 0 - self.min_mode_duration = 10 - self.mode_duration = 0 - self.emergency_override = False - - def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False): - # Emergency override for critical situations (stops, collisions) - if emergency: - self.emergency_override = True - self.current_mode = mode - self.transition_timeout = SET_MODE_TIMEOUT - self.mode_duration = 0 - return - - self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence) - for m in self.mode_confidence: - if m != mode: - self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05) - - # Require minimum duration in current mode (unless emergency) - if self.mode_duration < self.min_mode_duration and not self.emergency_override: - return - - # Hysteresis: higher threshold for mode changes - confidence_threshold = 0.6 if mode != self.current_mode else 0.3 # Lower threshold for faster response - - if self.mode_confidence[mode] > confidence_threshold: - if mode != self.current_mode and self.transition_timeout == 0: - self.transition_timeout = SET_MODE_TIMEOUT - self.current_mode = mode - self.mode_duration = 0 - - def update(self): - if self.transition_timeout > 0: - self.transition_timeout -= 1 - self.mode_duration += 1 - - # Reset emergency override after some time - if self.emergency_override and self.mode_duration > 20: - self.emergency_override = False - - # Gradual confidence decay - for mode in self.mode_confidence: - self.mode_confidence[mode] *= 0.98 - - def get_mode(self) -> ModeType: - return self.current_mode - - -class DynamicExperimentalController: - def __init__(self, CP: structs.CarParams, mpc, params=None): - self._CP = CP - self._mpc = mpc - self._params = params or Params() - self._enabled: bool = self._params.get_bool("DynamicExperimentalControl") - self._active: bool = False - self._frame: int = 0 - self._urgency = 0.0 - - self._mode_manager = ModeTransitionManager() - - # Smooth filters for stable decision making with faster response for critical scenarios - self._lead_filter = SmoothKalmanFilter( - measurement_noise=0.15, - process_noise=0.05, - alpha=1.02, - smoothing_factor=0.8 - ) - - self._slow_down_filter = SmoothKalmanFilter( - measurement_noise=0.1, - process_noise=0.1, - alpha=1.05, - smoothing_factor=0.7 - ) - - self._slowness_filter = SmoothKalmanFilter( - measurement_noise=0.1, - process_noise=0.06, - alpha=1.015, - smoothing_factor=0.92 - ) - - self._mpc_fcw_filter = SmoothKalmanFilter( - measurement_noise=0.2, - process_noise=0.1, - alpha=1.1, - smoothing_factor=0.5 - ) - self._has_lead_filtered = False - self._has_slow_down = False - self._has_slowness = False - self._has_mpc_fcw = False - self._v_ego_kph = 0.0 - self._v_cruise_kph = 0.0 - self._has_standstill = False - self._mpc_fcw_crash_cnt = 0 - self._standstill_count = 0 - # debug - self._endpoint_x = float('inf') - self._expected_distance = 0.0 - self._trajectory_valid = False - - def _read_params(self) -> None: - if self._frame % int(1. / DT_MDL) == 0: - self._enabled = self._params.get_bool("DynamicExperimentalControl") - - def mode(self) -> str: - return self._mode_manager.get_mode() - - def enabled(self) -> bool: - return self._enabled - - def active(self) -> bool: - return self._active - - def set_mpc_fcw_crash_cnt(self) -> None: - """Set MPC FCW crash count""" - self._mpc_fcw_crash_cnt = self._mpc.crash_cnt - - def _update_calculations(self, sm: messaging.SubMaster) -> None: - car_state = sm['carState'] - lead_one = sm['radarState'].leadOne - md = sm['modelV2'] - - self._v_ego_kph = car_state.vEgo * 3.6 - self._v_cruise_kph = car_state.vCruise - self._has_standstill = car_state.standstill - - # standstill detection - if self._has_standstill: - self._standstill_count = min(20, self._standstill_count + 1) - else: - self._standstill_count = max(0, self._standstill_count - 1) - - # Lead detection - self._lead_filter.add_data(float(lead_one.present)) - lead_value = self._lead_filter.get_value() or 0.0 - self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB - - # MPC FCW detection - fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0 - self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0)) - self._has_mpc_fcw = fcw_filtered_value > 0.5 - - # Slow down detection - self._calculate_slow_down(md) - - # Slowness detection - if not (self._standstill_count > 5) and not self._has_slow_down: - current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET)) - self._slowness_filter.add_data(current_slowness) - slowness_value = self._slowness_filter.get_value() or 0.0 - - # Hysteresis for slowness - threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1) - self._has_slowness = slowness_value > threshold - - def _calculate_slow_down(self, md): - """Calculate urgency based on trajectory endpoint vs expected distance.""" - - # Reset to safe defaults - urgency = 0.0 - self._endpoint_x = float('inf') - self._trajectory_valid = False - - #Require exact trajectory size - position_valid = len(md.position.x) == TRAJECTORY_SIZE - orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE - - if not (position_valid and orientation_valid): - # Invalid trajectory - this itself might indicate a stop scenario - # Apply moderate urgency for incomplete trajectories at speed - if self._v_ego_kph > 20.0: - urgency = 0.3 - - self._slow_down_filter.add_data(urgency) - urgency_filtered = self._slow_down_filter.get_value() or 0.0 - self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB - self._urgency = urgency_filtered - return - - # We have a valid full trajectory - self._trajectory_valid = True - - # Use the exact endpoint (33rd point, index 32) - endpoint_x = md.position.x[TRAJECTORY_SIZE - 1] - self._endpoint_x = endpoint_x - - # Get expected distance based on current speed using tuned constants - expected_distance = interp(self._v_ego_kph, - WMACConstants.SLOW_DOWN_BP, - WMACConstants.SLOW_DOWN_DIST) - self._expected_distance = expected_distance - - # Calculate urgency based on trajectory shortage - if endpoint_x < expected_distance: - shortage = expected_distance - endpoint_x - shortage_ratio = shortage / expected_distance - - # Base urgency on shortage ratio - urgency = min(1.0, shortage_ratio * 2.0) - - # Increase urgency for very short trajectories (imminent stops) - critical_distance = expected_distance * 0.3 - if endpoint_x < critical_distance: - urgency = min(1.0, urgency * 2.0) - - # Speed-based urgency adjustment - if self._v_ego_kph > 25.0: - speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0 - urgency = min(1.0, urgency * speed_factor) - - # Apply filtering but with less smoothing for stops - self._slow_down_filter.add_data(urgency) - urgency_filtered = self._slow_down_filter.get_value() or 0.0 - - # Update state with lower threshold for better stop detection - self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8) - self._urgency = urgency_filtered - - def _radarless_mode(self) -> None: - """Radarless mode decision logic with emergency handling.""" - - # EMERGENCY: MPC FCW - immediate blended mode - if self._has_mpc_fcw: - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - return - - # Standstill: use blended - if self._standstill_count > 3: - self._mode_manager.request_mode('blended', confidence=0.9) - return - - # Slow down scenarios: emergency for high urgency, normal for lower urgency - if self._has_slow_down: - if self._urgency > 0.7: - # Emergency: immediate blended mode for high urgency stops - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - else: - # Normal: blended with urgency-based confidence - confidence = min(1.0, self._urgency * 1.5) - self._mode_manager.request_mode('blended', confidence=confidence) - return - - # Driving slow: use ACC (but not if actively slowing down) - if self._has_slowness and not self._has_slow_down: - self._mode_manager.request_mode('acc', confidence=0.8) - return - - # Default: ACC - self._mode_manager.request_mode('acc', confidence=0.7) - - def _radar_mode(self) -> None: - """Radar mode with emergency handling.""" - - # EMERGENCY: MPC FCW - immediate blended mode - if self._has_mpc_fcw: - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - return - - # If lead detected and not in standstill: always use ACC - if self._has_lead_filtered and not (self._standstill_count > 3): - self._mode_manager.request_mode('acc', confidence=1.0) - return - - # Slow down scenarios: emergency for high urgency, normal for lower urgency - if self._has_slow_down: - if self._urgency > 0.7: - # Emergency: immediate blended mode for high urgency stops - self._mode_manager.request_mode('blended', confidence=1.0, emergency=True) - else: - # Normal: blended with urgency-based confidence - confidence = min(1.0, self._urgency * 1.3) - self._mode_manager.request_mode('blended', confidence=confidence) - return - - # Standstill: use blended - if self._standstill_count > 3: - self._mode_manager.request_mode('blended', confidence=0.9) - return - - # Driving slow: use ACC (but not if actively slowing down) - if self._has_slowness and not self._has_slow_down: - self._mode_manager.request_mode('acc', confidence=0.8) - return - - # Default: ACC - self._mode_manager.request_mode('acc', confidence=0.7) - - def update(self, sm: messaging.SubMaster) -> None: - self._read_params() - - self.set_mpc_fcw_crash_cnt() - - self._update_calculations(sm) - - if self._CP.radarUnavailable: - self._radarless_mode() - else: - self._radar_mode() - - self._mode_manager.update() - self._active = sm['selfdriveState'].experimentalMode and self._enabled - self._frame += 1 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py deleted file mode 100644 index e69de29bb..000000000 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 deleted file mode 100644 index 1f5c57702..000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py +++ /dev/null @@ -1,112 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -from typing import cast - -from openpilot.cereal import custom, messaging -from opendbc.car import structs -from openpilot.common.test import OpenpilotTestCase -from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, LongitudinalPlanSource -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController - -V_EGO = 20.0 -E2E_ACCEL = -3.0 # low enough that e2e wins the min() whenever it is a candidate - - -class MockDec: - def __init__(self, active: bool, mode: str): - self._active = active - self._mode = mode - - def update(self, sm): - pass - - def active(self) -> bool: - return self._active - - def mode(self) -> str: - return self._mode - - def enabled(self) -> bool: - return True - - -class MockSubMaster(dict): - def __init__(self, services: dict): - super().__init__(services) - self.valid = dict.fromkeys(services, True) - self.logMonoTime = dict.fromkeys(services, 0) - self.updated = dict.fromkeys(services, True) - self.recv_frame = dict.fromkeys(services, 1) - - def all_checks(self, service_list=None) -> bool: - return True - - -def build_sm(experimental_mode: bool) -> MockSubMaster: - services = {} - for service in ("radarState", "controlsState", "vehicleParameters", "carStateSP", - "liveMapDataSP", "gpsLocationExternal", "gpsLocation"): - services[service] = getattr(messaging.new_message(service), service) - - car_state = messaging.new_message('carState') - car_state.carState.vEgo = V_EGO - car_state.carState.vCruise = 100.0 - car_state.carState.vCruiseCluster = 100.0 - services['carState'] = car_state.carState.as_reader() - - selfdrive_state = messaging.new_message('selfdriveState') - selfdrive_state.selfdriveState.experimentalMode = experimental_mode - selfdrive_state.selfdriveState.enabled = True - services['selfdriveState'] = selfdrive_state.selfdriveState.as_reader() - - car_control = messaging.new_message('carControl') - car_control.carControl.enabled = True - services['carControl'] = car_control.carControl.as_reader() - - model = messaging.new_message('modelV2') - model.modelV2.orientationRate.z = [0.01] * 33 # nonzero: a straight path divides by zero in SCC vision - model.modelV2.velocity.x = [V_EGO] * 33 - model.modelV2.position.x = [float(i) for i in range(33)] - model.modelV2.action.desiredAcceleration = E2E_ACCEL - services['modelV2'] = model.modelV2.as_reader() - - return MockSubMaster(services) - - -def build_planner(dec_active: bool, dec_mode: str) -> LongitudinalPlanner: - CP = structs.CarParams() - CP.steerRatio = 15.0 - CP.wheelbase = 2.7 - CP.longitudinalActuatorDelay = 0.2 - CP_SP = custom.CarParamsSP.new_message().as_reader() - - planner = LongitudinalPlanner(CP, CP_SP, init_v=V_EGO) - planner.dec = cast(DynamicExperimentalController, MockDec(dec_active, dec_mode)) - return planner - - -class TestDecPlannerGate(OpenpilotTestCase): - """The e2e candidate must be gated on is_e2e(), not raw experimentalMode.""" - - def _source(self, experimental_mode: bool, dec_active: bool, dec_mode: str) -> LongitudinalPlanSource: - planner = build_planner(dec_active, dec_mode) - planner.update(build_sm(experimental_mode)) - return planner.mpc.source - - def test_no_e2e_when_experimental_mode_off(self): - assert self._source(False, False, 'acc') != LongitudinalPlanSource.e2e - - def test_e2e_when_dec_inactive(self): - # DEC off: behavior must match upstream - assert self._source(True, False, 'acc') == LongitudinalPlanSource.e2e - - def test_e2e_when_dec_blended(self): - assert self._source(True, True, 'blended') == LongitudinalPlanSource.e2e - - def test_no_e2e_when_dec_holds_acc(self): - # the regression - assert self._source(True, True, 'acc') != LongitudinalPlanSource.e2e 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 deleted file mode 100644 index 4fec6eaa5..000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py +++ /dev/null @@ -1,91 +0,0 @@ -from openpilot.common.test import OpenpilotTestCase -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController - -class MockLeadOne: - def __init__(self, present=0.0): - self.present = present - -class MockRadarState: - def __init__(self, present=0.0): - self.leadOne = MockLeadOne(present=present) - -class MockCarState: - def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False): - self.vEgo = vEgo - self.vCruise = vCruise - self.standstill = standstill - -class MockModelData: - def __init__(self, valid=True): - size = 33 if valid else 10 # incomplete if invalid - self.position = type("Pos", (), {"x": [0.0] * size})() - self.orientation = type("Ori", (), {"x": [0.0] * size})() - -class MockSelfDriveState: - def __init__(self, experimentalMode=False): - self.experimentalMode = experimentalMode - -class MockParams: - def get_bool(self, name): - return True - -def default_sm(): - sm = { - 'carState': MockCarState(vEgo=10.0, vCruise=20.0), - 'radarState': MockRadarState(present=1.0), - 'modelV2': MockModelData(valid=True), - 'selfdriveState': MockSelfDriveState(experimentalMode=True), - } - return sm - -def mock_cp(): - class CP: - radarUnavailable = False - return CP() - -def mock_mpc(): - class MPC: - crash_cnt = 0 - return MPC() - -# Fake Kalman Filter that always returns a given value -class FakeKalman: - def __init__(self, value=1.0): - self.value = value - def add_data(self, v): pass - def get_value(self): return self.value - def get_confidence(self): return 1.0 - def reset_data(self): pass - -class TestDynamicExperimentalController(OpenpilotTestCase): - def test_initial_mode_is_acc(self, mock_cp, mock_mpc): - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - assert controller.mode() == "acc" - - def test_standstill_triggers_blended(self, mock_cp, mock_mpc, default_sm): - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - default_sm['carState'].standstill = True - for _ in range(10): - controller.update(default_sm) - assert controller.mode() == "blended" - - def test_emergency_blended_on_fcw(self, mock_cp, mock_mpc, default_sm): - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - mock_mpc.crash_cnt = 1 # simulate FCW - for _ in range(2): - controller.update(default_sm) - assert controller.mode() == "blended" - - def test_radarless_slowdown_triggers_blended(self, mock_cp, mock_mpc, default_sm): - mock_cp.radarUnavailable = True - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - - # Force conditions to simulate slowdown - controller._slow_down_filter = FakeKalman(value=1.0) # ty: ignore[invalid-assignment] - controller._v_ego_kph = 35.0 - default_sm['modelV2'] = MockModelData(valid=False) # Incomplete trajectory - - for _ in range(3): - controller.update(default_sm) - - assert controller.mode() == "blended" diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f1e0c3641..a579e61c5 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,7 +9,6 @@ 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.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 @@ -17,7 +16,6 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle -DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource @@ -25,7 +23,6 @@ class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc): self.events_sp = EventsSP() self.resolver = SpeedLimitResolver() - self.dec = DynamicExperimentalController(CP, mpc) self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() self.sla = SpeedLimitAssist(CP, CP_SP) @@ -36,13 +33,6 @@ class LongitudinalPlannerSP: self.output_v_target = 0. self.output_a_target = 0. - def is_e2e(self, sm: messaging.SubMaster) -> bool: - experimental_mode = sm['selfdriveState'].experimentalMode - if not self.dec.active(): - return experimental_mode - - return experimental_mode and self.dec.mode() == "blended" - 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) @@ -75,7 +65,6 @@ class LongitudinalPlannerSP: def update(self, sm: messaging.SubMaster) -> None: self.events_sp.clear() - self.dec.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: @@ -89,12 +78,6 @@ class LongitudinalPlannerSP: longitudinalPlanSP.aTarget = float(self.output_a_target) longitudinalPlanSP.events = self.events_sp.to_msg() - # Dynamic Experimental Control - dec = longitudinalPlanSP.dec - dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc - dec.enabled = self.dec.enabled() - dec.active = self.dec.active() - # Smart Cruise Control smartCruiseControl = longitudinalPlanSP.smartCruiseControl # Vision Control diff --git a/openpilot/sunnypilot/sunnylink/docs/README.md b/openpilot/sunnypilot/sunnylink/docs/README.md index 62d46677e..17320073f 100644 --- a/openpilot/sunnypilot/sunnylink/docs/README.md +++ b/openpilot/sunnypilot/sunnylink/docs/README.md @@ -367,8 +367,8 @@ enablement: enablement: - type: any conditions: - - {type: param, key: ExperimentalMode, equals: true} - - {type: param, key: DynamicExperimentalControl, equals: true} + - {type: param, key: SmartCruiseControlVision, equals: true} + - {type: param, key: SmartCruiseControlMap, equals: true} ``` **Mixed: capability AND param** (only on longitudinal cars when ShowAdvancedControls is on): diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 63bf342fc..e03858814 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -599,26 +599,6 @@ } ] }, - { - "key": "DynamicExperimentalControl", - "widget": "toggle", - "title": "Dynamic Experimental Control", - "description": "Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal.", - "visibility": [ - { - "type": "capability", - "field": "has_longitudinal_control", - "equals": true - } - ], - "enablement": [ - { - "type": "capability", - "field": "has_longitudinal_control", - "equals": true - } - ] - }, { "key": "DisengageOnAccelerator", "widget": "toggle", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 3ef73e0fb..662e0205a 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -16,14 +16,6 @@ sections: title: Experimental Mode enablement: - $ref: '#/macros/longitudinal' - - key: DynamicExperimentalControl - widget: toggle - title: Dynamic Experimental Control - description: Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - visibility: - - $ref: '#/macros/longitudinal' - enablement: - - $ref: '#/macros/longitudinal' - key: DisengageOnAccelerator widget: toggle title: Disengage Cruise on Accelerator Pedal diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py index d9b1267a1..8c0be866e 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py @@ -201,11 +201,6 @@ class TestSpuriousOffroadGatesDropped(OpenpilotTestCase): assert item is not None assert "offroad_only" not in _flatten_rule_types(item.get("enablement")) - def test_dynamic_experimental_has_no_offroad_only(self, schema): - item = _find_item(schema, "DynamicExperimentalControl") - assert item is not None - assert "offroad_only" not in _flatten_rule_types(item.get("enablement")) - class TestNotEngagedReplacement(OpenpilotTestCase): @parameterized.expand([