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 4586afbc9f..0000000000 --- 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 index fb854edae8..22f7bf2c14 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -4,192 +4,120 @@ 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 dataclasses import dataclass +from typing import Literal + +import numpy as np 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 +from openpilot.selfdrive.modeld.constants import ModelConstants -# d-e2e, from modeldata.h -TRAJECTORY_SIZE = 33 -SET_MODE_TIMEOUT = 15 - -# Define the valid mode types ModeType = Literal['acc', 'blended'] +_DECEL_LOOKAHEAD_MIN_T = 1.0 +_DECEL_LOOKAHEAD_MAX_T = 6.0 +_T_IDXS = np.array(ModelConstants.T_IDXS) +_DECEL_IDX = np.where((_T_IDXS >= _DECEL_LOOKAHEAD_MIN_T) & (_T_IDXS <= _DECEL_LOOKAHEAD_MAX_T))[0] +_DECEL_INV_T = 1.0 / _T_IDXS[_DECEL_IDX] -class SmoothKalmanFilter: - """Enhanced Kalman filter with smoothing for stable decision making.""" +DECEL_INTENT_A_HINT = 0.35 +DECEL_INTENT_A_FULL = 1.30 +DECEL_INTENT_TRIGGER = 0.5 +DECEL_INTENT_CURVE_OVERRIDE = 0.9 - 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 +CURVE_Y_MAX = 5.0 - def add_data(self, measurement): - if len(self.history) >= self.max_history: - self.history.pop(0) - self.history.append(measurement) +LEAD_FUTURE_PROB_VANISH = 0.35 +LEAD_VETO_CONFIRM_FRAMES = 4 - if not self.initialized: - self.x = measurement - self.initialized = True - self.confidence = 0.1 - return +MODEL_DROP_TRUST_FULL = 5.0 +MODEL_DROP_TRUST_NONE = 30.0 +MODEL_TRUST_MIN = 0.5 - self.P = self.alpha * self.P + self.Q +CREEP_SPEED_ENTER = 2.0 +CREEP_SPEED_EXIT = 3.0 - K = self.P / (self.P + self.R) - effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1 +ENTER_FRAMES = 3 +EXIT_FRAMES = 16 +MIN_BLENDED_FRAMES = 20 - 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 +PARAM_READ_FRAMES = 5 -class ModeTransitionManager: - """Manages smooth transitions between driving modes with hysteresis.""" +@dataclass +class DecSignals: + decel_intent: float = 0.0 + curve_detected: bool = False + model_trust: float = 1.0 + creeping: bool = False + +def should_blend(s: DecSignals) -> bool: + degraded = s.model_trust < MODEL_TRUST_MIN + curve_gate = s.decel_intent >= DECEL_INTENT_CURVE_OVERRIDE or not s.curve_detected + slowdown_detected = not degraded and s.decel_intent >= DECEL_INTENT_TRIGGER and curve_gate + return slowdown_detected or s.creeping + + +class ModeHysteresis: 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 + self.mode: ModeType = 'acc' + self.above = 0 + self.below = 0 + self.blended_frames = 0 - 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 + def update(self, want_blended: bool, override: bool, veto: bool) -> ModeType: + self.above = self.above + 1 if want_blended else 0 + self.below = 0 if want_blended else self.below + 1 - 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) + if override: + self.mode, self.blended_frames = 'blended', 0 + elif veto: + self.mode = 'acc' + elif self.mode == 'acc': + if self.above >= ENTER_FRAMES: + self.mode, self.blended_frames = 'blended', 0 + else: + self.blended_frames += 1 + if self.blended_frames >= MIN_BLENDED_FRAMES and self.below >= EXIT_FRAMES: + self.mode = 'acc' + return self.mode - # 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 + def reset(self) -> None: + self.mode = 'acc' + self.above = 0 + self.below = 0 + self.blended_frames = 0 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() + self._hysteresis = ModeHysteresis() + self._creeping = False + self._lead_veto_frames = 0 - # 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.signals = DecSignals() + self.want_blended = False + self.lead_veto = False - 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 _update_creeping(self, v_ego: float) -> bool: + self._creeping = v_ego < CREEP_SPEED_EXIT if self._creeping else v_ego <= CREEP_SPEED_ENTER + return self._creeping def _read_params(self) -> None: - if self._frame % int(1. / DT_MDL) == 0: + if self._frame % PARAM_READ_FRAMES == 0: self._enabled = self._params.get_bool("DynamicExperimentalControl") def mode(self) -> str: - return self._mode_manager.get_mode() + return self._hysteresis.mode def enabled(self) -> bool: return self._enabled @@ -197,192 +125,77 @@ class DynamicExperimentalController: 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 + @staticmethod + def _decel_intent(md) -> float: + v = np.asarray(md.velocity.x) + if len(v) != len(_T_IDXS): + return 0.0 + a_req = float(np.min((v[_DECEL_IDX] - v[0]) * _DECEL_INV_T)) + return float(np.interp(-a_req, [DECEL_INTENT_A_HINT, DECEL_INTENT_A_FULL], [0.0, 1.0])) - def _update_calculations(self, sm: messaging.SubMaster) -> None: - car_state = sm['carState'] - lead_one = sm['radarState'].leadOne - md = sm['modelV2'] + @staticmethod + def _curve_detected(md) -> bool: + y = md.position.y + if len(y) < 1: + return False + return abs(y[-1]) >= CURVE_Y_MAX - self._v_ego_kph = car_state.vEgo * 3.6 - self._v_cruise_kph = car_state.vCruise - self._has_standstill = car_state.standstill + @staticmethod + def _model_trust(md) -> float: + if len(md.velocity.x) != len(_T_IDXS): + return 0.0 + return float(np.interp(md.frameDropPerc, [MODEL_DROP_TRUST_FULL, MODEL_DROP_TRUST_NONE], [1.0, 0.0])) - # 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) + @staticmethod + def _lead_veto(radar_state, md) -> bool: + lead_one, lead_two = radar_state.leadOne, radar_state.leadTwo + lead_now = lead_one.present or lead_two.present + probs = md.leadsV3 + future = min(probs[1].prob, probs[2].prob) if len(probs) >= 3 else 1.0 + return bool(lead_now and future > LEAD_FUTURE_PROB_VANISH) - # 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 + def _update_lead_veto(self, raw_veto: bool, lead_present: bool, urgent_override: bool) -> bool: + if raw_veto: + self._lead_veto_frames = min(self._lead_veto_frames + 1, LEAD_VETO_CONFIRM_FRAMES) + return self.lead_veto or self._lead_veto_frames >= LEAD_VETO_CONFIRM_FRAMES - # 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 + if not lead_present or urgent_override or not self.lead_veto: + self._lead_veto_frames = 0 + return False - # 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) + self._lead_veto_frames = max(self._lead_veto_frames - 1, 0) + return self._lead_veto_frames > 0 def update(self, sm: messaging.SubMaster) -> None: self._read_params() - self.set_mpc_fcw_crash_cnt() + car_state = sm['carState'] + md = sm['modelV2'] + radar_state = sm['radarState'] - self._update_calculations(sm) + is_creeping = self._update_creeping(car_state.vEgo) + lead_present = radar_state.leadOne.present or radar_state.leadTwo.present + self.signals = DecSignals( + decel_intent=self._decel_intent(md), + curve_detected=self._curve_detected(md), + model_trust=self._model_trust(md), + creeping=is_creeping and not lead_present, + ) + self.want_blended = should_blend(self.signals) - if self._CP.radarUnavailable: - self._radarless_mode() + crash_override = self._mpc.crash_cnt >= 1 + hard_brake_override = bool(md.meta.hardBrakePredicted) + strong_stop = self.signals.model_trust >= MODEL_TRUST_MIN and self.signals.decel_intent >= DECEL_INTENT_CURVE_OVERRIDE + raw_lead_veto = self._lead_veto(radar_state, md) + urgent_release = (crash_override or hard_brake_override or strong_stop) and not raw_lead_veto + self.lead_veto = self._update_lead_veto(raw_lead_veto, lead_present, urgent_release) + + override = (crash_override or hard_brake_override) and not self.lead_veto + + if self._enabled: + self._hysteresis.update(self.want_blended, override, self.lead_veto) else: - self._radar_mode() + self._hysteresis.reset() - 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/test_dynamic_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py index 4fec6eaa52..f275075799 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,91 +1,465 @@ +import numpy as np + +from openpilot.cereal import messaging +from opendbc.car import structs from openpilot.common.test import OpenpilotTestCase -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ( + DecSignals, + DynamicExperimentalController, + ModeHysteresis, + should_blend, + ENTER_FRAMES, + LEAD_VETO_CONFIRM_FRAMES, + MIN_BLENDED_FRAMES, +) -class MockLeadOne: - def __init__(self, present=0.0): - self.present = present +T_IDXS = np.array(ModelConstants.T_IDXS) -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 __init__(self, enabled=True): + self._enabled = enabled + def get_bool(self, name): - return True + return self._enabled -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), + +class MockMpc: + def __init__(self, crash_cnt=0): + self.crash_cnt = crash_cnt + + +def flat_velocity(v): + return [float(v)] * len(T_IDXS) + + +def decel_velocity(v0, a): + return [float(max(0.0, v0 + a * t)) for t in T_IDXS] + + +def make_car_state(v_ego=10.0, v_cruise=20.0): + msg = messaging.new_message('carState') + msg.carState.vEgo = v_ego + msg.carState.vCruise = v_cruise + return msg.carState.as_reader() + + +def make_selfdrive_state(experimental_mode=True): + msg = messaging.new_message('selfdriveState') + msg.selfdriveState.experimentalMode = experimental_mode + return msg.selfdriveState.as_reader() + + +def make_radar_state(lead_present=False, lead_radar=False, lead_two_present=False): + msg = messaging.new_message('radarState') + msg.radarState.leadOne.present = lead_present + msg.radarState.leadOne.radar = lead_radar + msg.radarState.leadTwo.present = lead_two_present + return msg.radarState.as_reader() + + +def make_model_v2(velocity=None, position_y=None, hard_brake=False, lead_probs=None, frame_drop_perc=0.0): + msg = messaging.new_message('modelV2') + msg.modelV2.velocity.x = velocity if velocity is not None else flat_velocity(0.0) + msg.modelV2.position.y = position_y if position_y is not None else [0.0] * len(T_IDXS) + msg.modelV2.frameDropPerc = frame_drop_perc + msg.modelV2.meta.hardBrakePredicted = hard_brake + if lead_probs is not None: + msg.modelV2.init('leadsV3', 3) + for i, (prob, prob_time) in enumerate(zip(lead_probs, (0.0, 2.0, 4.0), strict=True)): + msg.modelV2.leadsV3[i].prob = prob + msg.modelV2.leadsV3[i].probTime = prob_time + return msg.modelV2.as_reader() + + +def make_sm(v_ego=10.0, v_cruise=20.0, velocity=None, position_y=None, hard_brake=False, + lead_present=False, lead_radar=False, lead_two_present=False, lead_probs=None, + frame_drop_perc=0.0, experimental_mode=True): + return { + 'carState': make_car_state(v_ego, v_cruise), + 'radarState': make_radar_state(lead_present, lead_radar, lead_two_present), + 'modelV2': make_model_v2(velocity, position_y, hard_brake, lead_probs, frame_drop_perc), + 'selfdriveState': make_selfdrive_state(experimental_mode), } - return sm -def mock_cp(): - class CP: - radarUnavailable = False - return CP() -def mock_mpc(): - class MPC: - crash_cnt = 0 - return MPC() +def make_controller(cp=None, mpc=None, enabled=True): + return DynamicExperimentalController(cp or structs.CarParams(), mpc or MockMpc(), params=MockParams(enabled)) -# 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()) + def test_initial_mode_is_acc(self): + controller = make_controller() 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 + def test_flat_plan_never_blends_at_any_speed(self): + for v_ego in (2.5, 5.6, 8.3, 13.9, 22.2, 30.6): + controller = make_controller() + sm = make_sm(v_ego=v_ego, velocity=flat_velocity(v_ego)) + for _ in range(100): + controller.update(sm) + assert controller.mode() == "acc", f"false blend on a flat plan at v_ego={v_ego}" + + def test_highway_slowdown_without_lead_blends(self): + v0 = 110 / 3.6 + a = (70 / 3.6 - v0) / 6.0 + controller = make_controller() + sm = make_sm(v_ego=v0, velocity=decel_velocity(v0, a)) for _ in range(10): - controller.update(default_sm) + controller.update(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) + def test_curve_exclusion_prevents_false_blend(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0), position_y=[6.0] * len(T_IDXS)) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + def test_curve_does_not_override_saturated_decel_intent(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), position_y=[6.0] * len(T_IDXS)) + for _ in range(10): + controller.update(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) + def test_persistent_lead_forces_acc_even_with_strong_model_signal(self): + for lead_radar in (True, False): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_radar=lead_radar, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1): + controller.update(sm) + assert not controller.lead_veto + for _ in range(60): + controller.update(sm) + assert controller.lead_veto + assert controller.mode() == "acc" + def test_single_frame_lead_veto_pulse_does_not_leave_blended(self): + controller = make_controller() + no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0)) + lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(ENTER_FRAMES): + controller.update(no_lead_sm) assert controller.mode() == "blended" + + controller.update(lead_sm) + assert controller.mode() == "blended" + assert not controller.lead_veto + + controller.update(no_lead_sm) + assert controller.mode() == "blended" + assert not controller.lead_veto + + def test_three_frame_lead_veto_pulse_does_not_leave_blended(self): + controller = make_controller() + no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0)) + lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(ENTER_FRAMES): + controller.update(no_lead_sm) + assert controller.mode() == "blended" + + for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1): + controller.update(lead_sm) + assert controller.mode() == "blended" + assert not controller.lead_veto + + controller.update(no_lead_sm) + assert controller.mode() == "blended" + assert not controller.lead_veto + + def test_persistent_lead_veto_forces_acc_after_confirmation(self): + controller = make_controller() + no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0)) + lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(ENTER_FRAMES): + controller.update(no_lead_sm) + assert controller.mode() == "blended" + + for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1): + controller.update(lead_sm) + assert controller.mode() == "blended" + assert not controller.lead_veto + + controller.update(lead_sm) + assert controller.mode() == "acc" + assert controller.lead_veto + + controller.update(no_lead_sm) + assert controller.mode() == "blended" + assert not controller.lead_veto + + def test_veto_releases_without_rebuild_lag(self): + controller = make_controller() + lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(30): + controller.update(lead_sm) + assert controller.mode() == "acc" + assert controller.lead_veto + + no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=False) + for _ in range(ENTER_FRAMES + 2): + controller.update(no_lead_sm) + if controller.mode() == "blended": + break + assert controller.mode() == "blended" + + def test_lead_gone_with_no_underlying_slowdown_stays_acc(self): + controller = make_controller() + lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(30): + controller.update(lead_sm) + assert controller.mode() == "acc" + + no_lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False) + for _ in range(20): + controller.update(no_lead_sm) + assert controller.mode() == "acc" + + def test_creep_does_not_release_lead_veto(self): + controller = make_controller() + sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(10): + controller.update(sm) + assert controller.mode() == "acc" + assert controller.lead_veto + + def test_lead_prevents_creep_only_blending_when_model_probability_drops(self): + controller = make_controller() + confirmed_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES): + controller.update(confirmed_sm) + assert controller.lead_veto + + low_probability_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 0.1, 0.1]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1): + controller.update(low_probability_sm) + assert controller.lead_veto + assert controller.mode() == "acc" + + for _ in range(20): + controller.update(low_probability_sm) + assert not controller.lead_veto + assert not controller.signals.creeping + assert controller.mode() == "acc" + + no_lead_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=False, lead_probs=[1.0, 0.1, 0.1]) + for _ in range(ENTER_FRAMES): + controller.update(no_lead_sm) + assert controller.mode() == "blended" + + def test_confirmed_lead_veto_ignores_short_future_probability_dropout(self): + controller = make_controller() + confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES): + controller.update(confirmed_sm) + assert controller.lead_veto + + dropout_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0), + lead_present=True, lead_probs=[1.0, 0.1, 0.1]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1): + controller.update(dropout_sm) + assert controller.lead_veto + assert controller.mode() == "acc" + + controller.update(confirmed_sm) + assert controller.lead_veto + assert controller.mode() == "acc" + + def test_urgent_override_bypasses_confirmed_veto_release(self): + controller = make_controller() + confirmed_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES): + controller.update(confirmed_sm) + assert controller.lead_veto + + hard_brake_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), hard_brake=True, + lead_present=True, lead_probs=[1.0, 0.1, 0.1]) + controller.update(hard_brake_sm) + assert not controller.lead_veto + assert controller.mode() == "blended" + + def test_trusted_strong_stop_bypasses_confirmed_veto_release(self): + controller = make_controller() + confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES): + controller.update(confirmed_sm) + assert controller.lead_veto + assert controller.mode() == "acc" + + departing_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 0.1, 0.1]) + controller.update(departing_lead_sm) + assert not controller.lead_veto + assert controller.mode() == "blended" + + def test_degraded_strong_stop_does_not_bypass_confirmed_veto_release(self): + controller = make_controller() + confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES): + controller.update(confirmed_sm) + assert controller.lead_veto + + degraded_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=True, + lead_probs=[1.0, 0.1, 0.1], frame_drop_perc=60.0) + for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1): + controller.update(degraded_sm) + assert controller.lead_veto + assert controller.mode() == "acc" + + def test_model_slowdown_still_blends_while_creeping_with_a_lead(self): + controller = make_controller() + sm = make_sm(v_ego=1.0, velocity=decel_velocity(1.0, -1.0), lead_present=True, lead_probs=[1.0, 0.1, 0.1]) + for _ in range(ENTER_FRAMES): + controller.update(sm) + assert not controller.lead_veto + assert controller.mode() == "blended" + + def test_hard_brake_still_blends_while_creeping_with_a_lead(self): + controller = make_controller() + sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), hard_brake=True, + lead_present=True, lead_probs=[1.0, 0.1, 0.1]) + controller.update(sm) + assert not controller.lead_veto + assert controller.mode() == "blended" + + def test_creep_hysteresis_band_without_lead(self): + controller = make_controller() + controller.update(make_sm(v_ego=1.5, velocity=flat_velocity(1.5))) + assert controller.signals.creeping + + controller.update(make_sm(v_ego=2.5, velocity=flat_velocity(2.5))) + assert controller.signals.creeping, "a small excursion above CREEP_SPEED_ENTER should not exit creeping" + + controller.update(make_sm(v_ego=5.0, velocity=flat_velocity(5.0))) + assert not controller.signals.creeping, "should exit creeping once genuinely above CREEP_SPEED_EXIT" + + def test_crash_cnt_override_inert_while_lead_present(self): + mpc = MockMpc(crash_cnt=0) + controller = make_controller(mpc=mpc) + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + mpc.crash_cnt = 1 + controller.update(sm) + assert controller.mode() == "acc" + + def test_crash_cnt_blends_within_one_frame_without_lead(self): + mpc = MockMpc(crash_cnt=1) + controller = make_controller(mpc=mpc) + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False) + controller.update(sm) + assert controller.mode() == "blended" + + def test_hard_brake_predicted_blends_within_one_frame_without_lead(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True, lead_present=False) + controller.update(sm) + assert controller.mode() == "blended" + + def test_confirmed_lead_veto_suppresses_hard_brake_override(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True, + lead_present=True, lead_probs=[1.0, 1.0, 1.0]) + for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1): + controller.update(sm) + assert controller.mode() == "blended" + assert not controller.lead_veto + controller.update(sm) + assert controller.mode() == "acc" + assert controller.lead_veto + + def test_degraded_model_does_not_blend(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0), frame_drop_perc=60.0) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + def test_short_plan_arrays_do_not_blend(self): + controller = make_controller() + sm = make_sm(v_ego=20.0, velocity=[20.0] * 5) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + def test_disabled_param_holds_acc(self): + controller = make_controller(enabled=False) + sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0)) + for _ in range(30): + controller.update(sm) + assert controller.mode() == "acc" + + +class TestModeHysteresis(OpenpilotTestCase): + def test_entry_requires_enter_frames(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES - 1): + assert h.update(want_blended=True, override=False, veto=False) == "acc" + assert h.update(want_blended=True, override=False, veto=False) == "blended" + + def test_override_beats_veto(self): + h = ModeHysteresis() + assert h.update(want_blended=False, override=True, veto=True) == "blended" + + def test_veto_forces_acc_even_when_reason_active(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES + 5): + assert h.update(want_blended=True, override=False, veto=True) == "acc" + + def test_counter_accumulates_under_veto_then_releases_instantly(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES + 5): + h.update(want_blended=True, override=False, veto=True) + assert h.mode == "acc" + assert h.update(want_blended=True, override=False, veto=False) == "blended" + + def test_exit_requires_min_dwell_and_sustained_absence(self): + h = ModeHysteresis() + for _ in range(ENTER_FRAMES): + h.update(want_blended=True, override=False, veto=False) + assert h.mode == "blended" + for _ in range(MIN_BLENDED_FRAMES - 1): + assert h.update(want_blended=False, override=False, veto=False) == "blended" + assert h.update(want_blended=False, override=False, veto=False) == "acc" + + def test_no_flapping_on_alternating_reason(self): + h = ModeHysteresis() + changes = 0 + prev = h.mode + for i in range(200): + mode = h.update(want_blended=i % 2 == 0, override=False, veto=False) + changes += mode != prev + prev = mode + assert changes == 0 + + +class TestShouldBlend(OpenpilotTestCase): + def test_slowdown_detected_triggers(self): + assert should_blend(DecSignals(decel_intent=1.0)) + assert not should_blend(DecSignals(decel_intent=0.0)) + + def test_curve_exclusion_suppresses_slowdown(self): + assert not should_blend(DecSignals(decel_intent=0.7, curve_detected=True)) + + def test_curve_exclusion_does_not_override_saturated_decel_intent(self): + assert should_blend(DecSignals(decel_intent=1.0, curve_detected=True)) + + def test_degraded_model_suppresses_model_based_reasons(self): + s = DecSignals(decel_intent=1.0, model_trust=0.0) + assert not should_blend(s) + + def test_creep_bypasses_everything(self): + assert should_blend(DecSignals(model_trust=0.0, creeping=True))