diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py index 4586afbc9f..76d00d2cea 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -1,17 +1,48 @@ +from openpilot.common.realtime import DT_MDL + + class WMACConstants: - # Lead detection parameters - LEAD_WINDOW_SIZE = 6 # Stable detection window - LEAD_PROB = 0.45 # Balanced threshold for lead detection + TRAJECTORY_SIZE = 33 + PARAM_READ_FRAMES = max(1, int(round(1.0 / DT_MDL))) - # Slow down detection parameters - SLOW_DOWN_WINDOW_SIZE = 5 # Responsive but stable - SLOW_DOWN_PROB = 0.3 # Balanced threshold for slow down scenarios + EMERGENCY_HOLD_FRAMES = max(1, int(round(0.75 / DT_MDL))) + MIN_MODE_DURATION = {'acc': max(1, int(round(0.6 / DT_MDL))), 'blended': max(1, int(round(0.5 / DT_MDL)))} + ENTER_BLENDED_FRAMES = max(1, int(round(0.4 / DT_MDL))) + EXIT_BLENDED_FRAMES = max(1, int(round(0.35 / DT_MDL))) + STANDSTILL_FRAMES = max(1, int(round(0.2 / DT_MDL))) - # Optimized slow down distance curve - smooth and progressive + LEAD_PROB = 0.45 + LEAD_EXIT_PROB = 0.25 + LEAD_RISE_RATE = 1.0 + LEAD_FALL_RATE = 0.35 + RADAR_LEAD_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL))) + RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL))) + RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL))) + + SLOW_DOWN_PROB = 0.5 + SLOW_DOWN_EXIT_PROB = 0.4 + SLOW_DOWN_RISE_RATE = 0.65 + SLOW_DOWN_FALL_RATE = 0.15 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.] + URGENT_SLOW_DOWN_PROB = 0.85 - # 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 + MODEL_DECEL_START = -0.5 + MODEL_DECEL_RANGE = 2.0 + MODEL_DECEL_TREND_FRAMES = 4 + MODEL_DECEL_TREND_ACCEL = -0.075 + MODEL_DECEL_TREND_RATE = 0.35 + MODEL_DECEL_TREND_MAX_MPC_ACCEL = 0.075 + MODEL_DECEL_TREND_MAX_COMMAND_STEP = 0.15 + MODEL_DECEL_TREND_RELEASE_ACCEL = -0.02 + ENDPOINT_URGENCY_GAIN = 1.3 + CRITICAL_ENDPOINT_FACTOR = 0.3 + CRITICAL_URGENCY_GAIN = 1.5 + SPEED_URGENCY_MIN = 25.0 + SPEED_URGENCY_RANGE = 80.0 + + SLOWNESS_PROB = 0.55 + SLOWNESS_EXIT_PROB = 0.45 + SLOWNESS_RISE_RATE = 0.35 + SLOWNESS_FALL_RATE = 0.5 + SLOWNESS_CRUISE_OFFSET = 1.025 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py index fb854edae8..e01b9f9119 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -6,129 +6,119 @@ See the LICENSE.md file in the root directory for more details. """ # Version = 2025-6-30 +from collections import deque +import math +from typing import Literal + from openpilot.cereal import messaging -from opendbc.car import structs from numpy import interp +from opendbc.car import structs 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 clip01(value: float) -> float: + return max(0.0, min(1.0, float(value))) - 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) +class SmoothedSignal: + def __init__(self, rise_rate: float, fall_rate: float, initial_value: float = 0.0): + self.rise_rate = clip01(rise_rate) + self.fall_rate = clip01(fall_rate) + self.value = clip01(initial_value) - if not self.initialized: - self.x = measurement - self.initialized = True - self.confidence = 0.1 - return + def update(self, measurement: float) -> float: + measurement = clip01(measurement) + rate = self.rise_rate if measurement > self.value else self.fall_rate + self.value += (measurement - self.value) * rate + return self.value - self.P = self.alpha * self.P + self.Q + def reset(self, value: float = 0.0) -> None: + self.value = clip01(value) - 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 +class HysteresisSignal: + def __init__(self, enter_threshold: float, exit_threshold: float, rise_rate: float, fall_rate: float): + self.enter_threshold = clip01(enter_threshold) + self.exit_threshold = clip01(exit_threshold) + self.filter = SmoothedSignal(rise_rate, fall_rate) + self.active = False - 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 update(self, measurement: float) -> bool: + value = self.filter.update(measurement) + threshold = self.exit_threshold if self.active else self.enter_threshold + self.active = value > threshold + return self.active - def get_value(self): - return self.x if self.initialized else None + def reset(self) -> None: + self.filter.reset() + self.active = False - def get_confidence(self): - return self.confidence - - def reset_data(self): - self.initialized = False - self.history = [] - self.confidence = 0.0 + @property + def value(self) -> float: + return self.filter.value 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 + self._pending_mode: ModeType = 'acc' + self._pending_count = 0 + self._blended_hold_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 + def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None: + if immediate: + self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) if mode == 'blended' else 0 + self._pending_mode = mode + self._pending_count = 0 + self._switch_mode(mode) 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) + if cancel_hold and mode == 'acc': + self._blended_hold_frames = 0 - # Require minimum duration in current mode (unless emergency) - if self.mode_duration < self.min_mode_duration and not self.emergency_override: + if self._blended_hold_frames > 0: + mode = 'blended' + + if mode == self.current_mode: + self._pending_mode = mode + self._pending_count = 0 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 mode != self._pending_mode: + self._pending_mode = mode + self._pending_count = 1 + else: + self._pending_count += 1 - 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 + if self.mode_duration < WMACConstants.MIN_MODE_DURATION[self.current_mode]: + return - def update(self): - if self.transition_timeout > 0: - self.transition_timeout -= 1 + required_count = WMACConstants.ENTER_BLENDED_FRAMES if mode == 'blended' else WMACConstants.EXIT_BLENDED_FRAMES + if self._pending_count >= required_count: + self._switch_mode(mode) + + def update(self) -> None: + if self._blended_hold_frames > 0: + self._blended_hold_frames -= 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 _switch_mode(self, mode: ModeType) -> None: + if mode == self.current_mode: + return + + self.current_mode = mode + self.mode_duration = 0 + self._pending_mode = mode + self._pending_count = 0 + class DynamicExperimentalController: def __init__(self, CP: structs.CarParams, mpc, params=None): @@ -142,35 +132,32 @@ class DynamicExperimentalController: 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._lead_tracker = HysteresisSignal( + enter_threshold=WMACConstants.LEAD_PROB, + exit_threshold=WMACConstants.LEAD_EXIT_PROB, + rise_rate=WMACConstants.LEAD_RISE_RATE, + fall_rate=WMACConstants.LEAD_FALL_RATE, + ) + self._slow_down_tracker = HysteresisSignal( + enter_threshold=WMACConstants.SLOW_DOWN_PROB, + exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB, + rise_rate=WMACConstants.SLOW_DOWN_RISE_RATE, + fall_rate=WMACConstants.SLOW_DOWN_FALL_RATE, + ) + self._slowness_tracker = HysteresisSignal( + enter_threshold=WMACConstants.SLOWNESS_PROB, + exit_threshold=WMACConstants.SLOWNESS_EXIT_PROB, + rise_rate=WMACConstants.SLOWNESS_RISE_RATE, + fall_rate=WMACConstants.SLOWNESS_FALL_RATE, ) - 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_any_lead = False + self._has_current_radar_acc_lead = False + self._has_radar_acc_lead = False + self._radar_acc_lead_frames = 0 + self._radar_fresh = True + self._radar_stale_frames = 0 self._has_slow_down = False self._has_slowness = False self._has_mpc_fcw = False @@ -179,13 +166,18 @@ class DynamicExperimentalController: 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 + self._raw_urgency = 0.0 + self._model_accel_samples = deque(maxlen=WMACConstants.MODEL_DECEL_TREND_FRAMES) + self._model_decel_trending = False + self._model_decel_latched = False + self._planner_accel = math.nan def _read_params(self) -> None: - if self._frame % int(1. / DT_MDL) == 0: + if self._frame % WMACConstants.PARAM_READ_FRAMES == 0: self._enabled = self._params.get_bool("DynamicExperimentalControl") def mode(self) -> str: @@ -198,191 +190,221 @@ class DynamicExperimentalController: 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: + def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None: car_state = sm['carState'] - lead_one = sm['radarState'].leadOne + radar_state = sm['radarState'] + lead_one = radar_state.leadOne + lead_two = radar_state.leadTwo 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) + self._standstill_count = min(WMACConstants.STANDSTILL_FRAMES * 3, 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._radar_fresh = bool(radar_fresh) + if self._radar_fresh: + self._radar_stale_frames = 0 + self._has_lead_filtered = self._lead_tracker.update(float(lead_one.present)) + self._has_any_lead = bool(lead_one.present or lead_two.present) + self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two))) + self._update_radar_acc_lead() + else: + self._radar_stale_frames += 1 + self._has_current_radar_acc_lead = False + if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES: + self._update_radar_acc_lead() + else: + self._lead_tracker.reset() + self._has_lead_filtered = False + self._has_any_lead = False + self._has_radar_acc_lead = False + self._radar_acc_lead_frames = 0 + self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0 self._calculate_slow_down(md) - # Slowness detection - if not (self._standstill_count > 5) and not self._has_slow_down: + if self._standstill_count > WMACConstants.STANDSTILL_FRAMES or self._has_slow_down: + self._slowness_tracker.reset() + self._has_slowness = False + else: 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 + self._has_slowness = self._slowness_tracker.update(current_slowness) - # 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 + def _calculate_slow_down(self, md) -> None: self._endpoint_x = float('inf') + self._expected_distance = 0.0 self._trajectory_valid = False - #Require exact trajectory size - position_valid = len(md.position.x) == TRAJECTORY_SIZE - orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE + self._update_model_decel_trend(md) + urgency = self._model_action_urgency(md) + position_valid = len(md.position.x) == WMACConstants.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 + if position_valid: + self._trajectory_valid = True + self._endpoint_x = md.position.x[WMACConstants.TRAJECTORY_SIZE - 1] + self._expected_distance = interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST) + urgency = max(urgency, self._endpoint_urgency(self._endpoint_x, self._expected_distance)) - 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 + self._raw_urgency = clip01(urgency) + self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency) + self._urgency = self._slow_down_tracker.value + + def _update_model_decel_trend(self, md) -> None: + try: + desired_accel = float(md.action.desiredAcceleration) + except (AttributeError, OverflowError, TypeError, ValueError): + desired_accel = math.nan + if not math.isfinite(desired_accel): + self._reset_model_decel_trend() + else: + self._model_accel_samples.append(desired_accel) + history = tuple(self._model_accel_samples) + self._model_decel_trending = (len(history) == self._model_accel_samples.maxlen + and history[-1] <= WMACConstants.MODEL_DECEL_TREND_ACCEL + and (history[0] - history[-1]) / (DT_MDL * (len(history) - 1)) > WMACConstants.MODEL_DECEL_TREND_RATE + and all(after <= before for before, after in zip(history[:-1], history[1:], strict=True)) + and sum(after < before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2) + if len(history) == self._model_accel_samples.maxlen and all( + accel >= WMACConstants.MODEL_DECEL_TREND_RELEASE_ACCEL for accel in history + ): + self._model_decel_latched = False + + def _reset_model_decel_trend(self) -> None: + self._model_accel_samples.clear() + self._model_decel_trending = False + self._model_decel_latched = False + + def _radar_acc_lead_score(self, lead_one) -> float: + radar_track_id = int(getattr(lead_one, 'radarTrackId', -1)) + return float(lead_one.present and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0)) + + def _update_radar_acc_lead(self) -> None: + if self._has_current_radar_acc_lead: + self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + self._has_radar_acc_lead = True return - # We have a valid full trajectory - self._trajectory_valid = True + if not self._has_any_lead: + self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES) - # Use the exact endpoint (33rd point, index 32) - endpoint_x = md.position.x[TRAJECTORY_SIZE - 1] - self._endpoint_x = endpoint_x + self._has_radar_acc_lead = self._radar_acc_lead_frames > 0 + self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1) - # 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 + def _model_action_urgency(self, md) -> float: + action = getattr(md, 'action', None) + if action is None: + return 0.0 - # Calculate urgency based on trajectory shortage - if endpoint_x < expected_distance: - shortage = expected_distance - endpoint_x - shortage_ratio = shortage / expected_distance + urgency = 1.0 if getattr(action, 'shouldStop', False) else 0.0 + desired_accel = getattr(action, 'desiredAcceleration', 0.0) + if desired_accel < WMACConstants.MODEL_DECEL_START: + urgency = max(urgency, min(1.0, (WMACConstants.MODEL_DECEL_START - desired_accel) / WMACConstants.MODEL_DECEL_RANGE)) + return urgency - # Base urgency on shortage ratio - urgency = min(1.0, shortage_ratio * 2.0) + def _endpoint_urgency(self, endpoint_x: float, expected_distance: float) -> float: + if endpoint_x >= expected_distance: + return 0.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) + shortage_ratio = (expected_distance - endpoint_x) / expected_distance + urgency = min(1.0, shortage_ratio * WMACConstants.ENDPOINT_URGENCY_GAIN) - # 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) + if endpoint_x < expected_distance * WMACConstants.CRITICAL_ENDPOINT_FACTOR: + urgency = min(1.0, urgency * WMACConstants.CRITICAL_URGENCY_GAIN) - # 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 + if self._v_ego_kph > WMACConstants.SPEED_URGENCY_MIN: + speed_factor = 1.0 + (self._v_ego_kph - WMACConstants.SPEED_URGENCY_MIN) / WMACConstants.SPEED_URGENCY_RANGE + urgency = min(1.0, urgency * speed_factor) - # 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 + return urgency - def _radarless_mode(self) -> None: - """Radarless mode decision logic with emergency handling.""" + def _model_decel_handoff_ready(self) -> bool: + try: + mpc_accel = float(self._mpc.a_solution[1]) + return (math.isfinite(mpc_accel) and mpc_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL + and math.isfinite(self._planner_accel) and self._planner_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL + and self._planner_accel - self._model_accel_samples[-1] <= WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP) + except (AttributeError, IndexError, OverflowError, TypeError, ValueError): + return False + + def _lead_dropout_model_handoff_ready(self) -> bool: + if (not self._active or self._CP.radarUnavailable or not self._radar_fresh or self._has_any_lead or not self._has_radar_acc_lead + or self._mode_manager.get_mode() != 'acc' or self._mpc.last_solution_status != 0): + return False + try: + model_accel = float(self._model_accel_samples[-1]) + except (IndexError, OverflowError, TypeError, ValueError): + return False + return (math.isfinite(model_accel) and math.isfinite(self._planner_accel) + and self._planner_accel <= WMACConstants.MODEL_DECEL_START + and model_accel <= WMACConstants.MODEL_DECEL_START + and model_accel <= self._planner_accel + WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP) + + def _desired_mode(self) -> tuple[ModeType, bool]: + standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES + urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB + + if not self._CP.radarUnavailable and self._has_current_radar_acc_lead: + self._reset_model_decel_trend() + return 'acc', True + + radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1 + if (radar_stale or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down): + self._radar_acc_lead_frames = 0 + self._has_radar_acc_lead = False + return 'blended', True + + if self._lead_dropout_model_handoff_ready(): + self._radar_acc_lead_frames = 0 + self._has_radar_acc_lead = False + self._model_decel_latched = True + return 'blended', True + + if not self._CP.radarUnavailable and self._has_radar_acc_lead: + self._reset_model_decel_trend() + return 'acc', True + + entering_model_slowdown = self._model_decel_trending and self._model_decel_handoff_ready() and not self._model_decel_latched + self._model_decel_latched |= entering_model_slowdown + if self._model_decel_latched: + return 'blended', entering_model_slowdown - # 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) + return 'blended', True if self._CP.radarUnavailable: - self._radarless_mode() - else: - self._radar_mode() + if standstill or self._has_slow_down: + return 'blended', urgent_slow_down + return 'acc', False - self._mode_manager.update() + if standstill or self._has_slow_down: + return 'blended', urgent_slow_down + + return 'acc', False + + def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True, planner_accel: float | None = None) -> None: + self._read_params() + self.set_mpc_fcw_crash_cnt() + try: + self._planner_accel = float(planner_accel) + except (OverflowError, TypeError, ValueError): + self._planner_accel = math.nan + self._update_calculations(sm, radar_fresh) self._active = sm['selfdriveState'].experimentalMode and self._enabled + if not self._active: + model_decel_latched = self._model_decel_latched + self._reset_model_decel_trend() + if model_decel_latched: + self._mode_manager.request_mode('acc', immediate=True) + + mode, immediate = self._desired_mode() + self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES, + cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead) + self._mode_manager.update() + self._frame += 1 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py deleted file mode 100644 index 407ed0af3a..0000000000 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py +++ /dev/null @@ -1,94 +0,0 @@ -import pytest - -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController - -class MockLeadOne: - def __init__(self, status=0.0): - self.status = status - -class MockRadarState: - def __init__(self, status=0.0): - self.leadOne = MockLeadOne(status=status) - -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 - -@pytest.fixture -def default_sm(): - sm = { - 'carState': MockCarState(vEgo=10.0, vCruise=20.0), - 'radarState': MockRadarState(status=1.0), - 'modelV2': MockModelData(valid=True), - 'selfdriveState': MockSelfDriveState(experimentalMode=True), - } - return sm - -@pytest.fixture -def mock_cp(): - class CP: - radarUnavailable = False - return CP() - -@pytest.fixture -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 - -def test_initial_mode_is_acc(mock_cp, mock_mpc): - controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) - assert controller.mode() == "acc" - -def test_standstill_triggers_blended(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(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(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/dec/tests/test_dynamic_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py new file mode 100644 index 0000000000..24a32da652 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py @@ -0,0 +1,685 @@ +import pytest + +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal + + +class MockLeadOne: + def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1): + self.present = status + self.dRel = dRel + self.vRel = vRel + self.radar = radar + self.radarTrackId = radarTrackId + + +class MockRadarState: + def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1, leadTwo=None): + self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId) + self.leadTwo = leadTwo if leadTwo is not None else MockLeadOne() + + +class MockCarState: + def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False): + self.vEgo = vEgo + self.vCruise = vCruise + self.standstill = standstill + + +class MockAction: + def __init__(self, desiredAcceleration=0.0, shouldStop=False): + self.desiredAcceleration = desiredAcceleration + self.shouldStop = shouldStop + + +class MockModelData: + def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False): + position_size = 33 if valid else 10 + orientation_size = position_size if orientation_valid is None else (33 if orientation_valid else 10) + position_x = [0.0] * position_size + if position_x: + position_x[-1] = endpoint_x + self.position = type("Pos", (), {"x": position_x})() + self.orientation = type("Ori", (), {"x": [0.0] * orientation_size})() + self.acceleration = type("Accel", (), {"x": [0.0] * position_size})() + self.action = MockAction(desired_acceleration, should_stop) + + +class MockSelfDriveState: + def __init__(self, experimentalMode=False): + self.experimentalMode = experimentalMode + + +class MockParams: + def get_bool(self, name): + return True + + +@pytest.fixture +def default_sm(): + sm = { + 'carState': MockCarState(vEgo=10.0, vCruise=20.0), + 'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7), + 'modelV2': MockModelData(valid=True), + 'selfdriveState': MockSelfDriveState(experimentalMode=True), + } + return sm + + +@pytest.fixture +def mock_cp(): + class CP: + radarUnavailable = False + return CP() + + +@pytest.fixture +def mock_mpc(): + class MPC: + crash_cnt = 0 + a_solution = [0.0, 0.0] + last_solution_status = 0 + return MPC() + + +def test_initial_mode_is_acc(mock_cp, mock_mpc): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + assert controller.mode() == "acc" + + +def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['carState'].standstill = True + for _ in range(20): + controller.update(default_sm) + assert controller.mode() == "blended" + + +def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + mock_mpc.crash_cnt = 1 + controller.update(default_sm) + assert controller.mode() == "blended" + + +def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + + controller.update(default_sm) + + assert controller.mode() == "blended" + + +def test_valid_position_with_missing_orientation_can_trigger_slowdown(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, orientation_valid=False) + + controller.update(default_sm) + + assert controller._trajectory_valid + assert controller.mode() == "blended" + + +def test_incomplete_position_does_not_trigger_slowdown(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=False, endpoint_x=0.0) + + for _ in range(3): + controller.update(default_sm) + + assert not controller._trajectory_valid + assert not controller._has_slow_down + assert controller.mode() == "acc" + + +def test_slowdown_hysteresis_prevents_threshold_chatter(): + signal = HysteresisSignal(enter_threshold=0.5, exit_threshold=0.4, rise_rate=1.0, fall_rate=1.0) + + assert signal.update(0.55) + assert signal.update(0.45) + assert not signal.update(0.35) + + +def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=False, should_stop=True) + + controller.update(default_sm) + + assert not controller._trajectory_valid + assert controller.mode() == "blended" + + +def test_confirmed_model_decel_trend_enters_blended_before_a_large_command(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (-0.02, -0.05, -0.08): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + assert controller.mode() == "acc" + + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12) + controller.update(default_sm, planner_accel=0.0) + + assert controller._model_decel_trending + assert not controller._has_slow_down + assert controller.mode() == "blended" + + +def test_confirmed_model_decel_handoff_stays_latched_through_a_plateau(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + + for _ in range(WMACConstants.EMERGENCY_HOLD_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES + 1): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_decel_trending + assert controller._model_decel_latched + assert controller.mode() == "blended" + + for _ in range(WMACConstants.MODEL_DECEL_TREND_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_decel_latched + assert controller.mode() == "acc" + + +def test_model_decel_trend_never_overrides_a_radar_lead(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm) + + assert not controller._model_accel_samples + assert not controller._model_decel_latched + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_radar_acquisition_clears_a_latched_model_decel_handoff(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + assert controller._model_decel_latched + + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_accel_samples + assert not controller._model_decel_latched + assert controller.mode() == "acc" + + +def test_model_decel_trend_does_not_accumulate_while_dec_is_inactive(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['selfdriveState'].experimentalMode = False + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_accel_samples + assert not controller._model_decel_latched + + default_sm['selfdriveState'].experimentalMode = True + controller.update(default_sm, planner_accel=0.0) + assert not controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_disabling_dec_clears_a_latched_model_decel_mode(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + assert controller._model_decel_latched + assert controller.mode() == "blended" + + default_sm['selfdriveState'].experimentalMode = False + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0) + controller.update(default_sm, planner_accel=0.0) + + assert not controller._model_decel_latched + assert controller.mode() == "acc" + + +def test_model_decel_trend_waits_while_mpc_is_accelerating(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + mock_mpc.a_solution[1] = 0.5 + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.0) + + assert controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_steep_model_decel_trend_defers_to_the_existing_urgent_path(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (0.0, -0.2, -0.4, -0.6): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.05) + + assert controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_model_decel_trend_waits_while_the_planner_is_accelerating(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (-0.02, -0.05, -0.08, -0.12): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm, planner_accel=0.2) + + assert controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_alternating_model_accel_noise_does_not_trigger_an_early_handoff(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + + for desired_acceleration in (0.0, -0.2, 0.0, -0.2): + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration) + controller.update(default_sm) + + assert not controller._model_decel_trending + assert controller.mode() == "acc" + + +def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + + for _ in range(3): + controller.update(default_sm) + + assert controller._has_slow_down + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_far_radar_lead_always_uses_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0, radar=True) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + + controller.update(default_sm) + + assert controller._has_lead_filtered + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_radar_acquisition_immediately_returns_blended_to_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + assert controller.mode() == "blended" + + default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, radar=True, radarTrackId=7) + controller.update(default_sm) + + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=True) + for _ in range(20): + controller.update(default_sm) + assert controller.mode() == "acc" + + +def test_close_vision_only_lead_can_use_blended(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + +def test_second_radar_lead_forces_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + lead_two = MockLeadOne(status=1.0, dRel=120.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0, leadTwo=lead_two) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_second_vision_only_lead_does_not_force_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + lead_two = MockLeadOne(status=1.0, dRel=20.0, vRel=-10.0) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + +def test_inactive_lead_with_radar_marker_does_not_force_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + +def test_radarless_car_ignores_marked_radar_track(mock_cp, mock_mpc, default_sm): + mock_cp.radarUnavailable = True + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + assert controller._has_radar_acc_lead + assert controller.mode() == "blended" + + +def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + + for _ in range(20): + controller.update(default_sm) + + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['carState'].standstill = True + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True) + mock_mpc.crash_cnt = 1 + + for _ in range(10): + controller.update(default_sm) + + assert controller._has_lead_filtered + assert controller._has_mpc_fcw + assert controller.mode() == "acc" + + +def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0) + for _ in range(2): + controller.update(default_sm) + assert controller._has_slow_down + + default_sm['radarState'] = MockRadarState(status=0.0) + controller.update(default_sm) + + assert controller._has_lead_filtered + assert controller.mode() == "acc" + + +def test_braking_model_takes_over_on_the_first_fresh_full_lead_dropout(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1) + controller.update(default_sm, planner_accel=-1.2) + + default_sm['radarState'] = MockRadarState(status=0.0) + controller.update(default_sm, planner_accel=-1.2) + + assert not controller._has_radar_acc_lead + assert controller._model_decel_latched + assert controller.mode() == "blended" + + +@pytest.mark.parametrize(("model_accel", "planner_accel"), ((-0.75, -1.1), (-0.3, -1.1), (-1.1, -0.3))) +def test_lead_dropout_guard_stays_active_without_a_matching_brake(model_accel, planner_accel, mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm, planner_accel=-1.1) + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=model_accel) + default_sm['radarState'] = MockRadarState(status=0.0) + + controller.update(default_sm, planner_accel=planner_accel) + + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_failed_mpc_keeps_the_lead_dropout_guard(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm, planner_accel=-1.1) + mock_mpc.last_solution_status = 1 + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1) + default_sm['radarState'] = MockRadarState(status=0.0) + + controller.update(default_sm, planner_accel=-1.1) + + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_matching_brake_without_a_prior_radar_lead_does_not_use_dropout_handoff(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=0.0) + default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1) + + controller.update(default_sm, planner_accel=-1.1) + + assert not controller._has_radar_acc_lead + assert not controller._model_decel_latched + assert controller.mode() == "acc" + + +def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0) + for _ in range(2): + controller.update(default_sm) + assert controller._has_slow_down + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + controller.update(default_sm) + assert not controller._has_radar_acc_lead + assert controller.mode() == "acc" + + for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1): + controller.update(default_sm) + assert controller.mode() == "blended" + + +def test_radar_lead_short_dropout_guard_expires_without_any_lead(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=0.0) + for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + + controller.update(default_sm) + assert not controller._has_radar_acc_lead + + +def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + + controller.update(default_sm, radar_fresh=False) + + assert not controller._has_current_radar_acc_lead + assert controller._has_radar_acc_lead + assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1 + assert controller._radar_stale_frames == 1 + assert controller.mode() == "acc" + + +def test_one_stale_radar_frame_does_not_override_retained_lead_for_model_urgency(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + default_sm['modelV2'] = MockModelData(valid=False, should_stop=True) + + controller.update(default_sm, radar_fresh=False) + assert controller.mode() == "acc" + + controller.update(default_sm, radar_fresh=False) + assert controller.mode() == "blended" + + +def test_one_stale_radar_frame_does_not_delay_fcw(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + mock_mpc.crash_cnt = 1 + + controller.update(default_sm, radar_fresh=False) + + assert controller.mode() == "blended" + + +def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + + for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1): + controller.update(default_sm, radar_fresh=False) + assert controller._has_radar_acc_lead + + controller.update(default_sm, radar_fresh=False) + + assert not controller._has_current_radar_acc_lead + assert not controller._has_radar_acc_lead + assert not controller._has_any_lead + assert not controller._has_lead_filtered + + +def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + controller.update(default_sm) + for _ in range(WMACConstants.RADAR_STALE_FRAMES): + controller.update(default_sm, radar_fresh=False) + + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm, radar_fresh=False) + assert controller.mode() == "blended" + + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + controller.update(default_sm, radar_fresh=True) + + assert controller._radar_stale_frames == 0 + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + +@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"]) +def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=0.0) + if urgent_source == "fcw": + mock_mpc.crash_cnt = 1 + else: + default_sm['modelV2'] = MockModelData(valid=False, should_stop=True) + controller.update(default_sm) + + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + mock_mpc.crash_cnt = 0 + default_sm['modelV2'] = MockModelData(valid=True) + controller.update(default_sm) + assert controller.mode() == "blended" + + +def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES): + controller.update(default_sm) + assert controller._has_radar_acc_lead + assert controller.mode() == "acc" + + +def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + + for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2): + if frame % 2 == 0: + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0)) + else: + default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8)) + controller.update(default_sm) + + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" + + +def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(mock_cp, mock_mpc, default_sm): + controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams()) + default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7) + default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0) + controller.update(default_sm) + + default_sm['radarState'] = MockRadarState(status=1.0) + for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1): + controller.update(default_sm) + assert not controller._has_radar_acc_lead + assert controller.mode() == "blended" + + lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8) + default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two) + controller.update(default_sm) + + assert controller._has_current_radar_acc_lead + assert controller.mode() == "acc" diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 8fa0740d03..34be8b2747 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -136,7 +136,7 @@ class LongitudinalPlannerSP: self._radar_fresh_this_cycle = self._update_radar_freshness(sm) self.accel_controller.update_params() self.events_sp.clear() - self.dec.update(sm) + self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target) self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: 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 b07b6556a1..72c8d3c0cb 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 @@ -397,6 +397,61 @@ def test_positive_e2e_to_acc_handoff_starts_from_previous_plan(actuator_delay, a assert plant.planner.mpc.last_solution_status == 0 +def test_dec_retains_acc_through_route_like_radar_marker_dropout(): + dropout_start = 1.0 + reacquisition_time = 1.8 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation: + frame = round(current_time / DT_MDL) + if current_time < dropout_start: + marked_slot = "leadOne" if frame % 2 == 0 else "leadTwo" + return truth | {"radar": lead_name == marked_slot, "radarTrackId": 985 + frame if lead_name == marked_slot else -1} + if current_time < reacquisition_time: + return truth | {"radar": False, "radarTrackId": -1} + return truth | {"radar": lead_name == "leadOne", "radarTrackId": 1263 if lead_name == "leadOne" else -1} + + trace = _run( + duration=2.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=True, speed=20.0, + distance_lead=35.0, v_lead=18.0, v_cruise=30.0, lead_observation_fn=observe, + model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False), actuator_delay=0.15, actuator_lag=0.20, + ) + response = (trace.time >= dropout_start - DT_MDL) & (trace.time <= reacquisition_time + 0.5) + + assert all(mode == "acc" for mode in trace.dec_mode) + assert all(str(source) != "e2e" for source in trace.source) + assert not trace.fcw.any() + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.max(np.abs(np.diff(trace.a_target[response]) / DT_MDL)) < 3.0 + assert trace.solver_failures == 0 + + +def test_dec_uses_confirmed_model_slowdown_while_the_handoff_is_still_gentle(): + def model_action(current_time: float, _v_ego: float, _a_ego: float) -> tuple[float, bool]: + if current_time < 1.0: + return 0.0, False + if current_time < 1.75: + return -0.5 * (current_time - 1.0), False + if current_time < 3.25: + return -0.375, False + return -0.375 - 0.5 * (current_time - 3.25), False + + trace = _run( + duration=4.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=False, speed=22.0, + v_cruise=22.0, model_action_fn=model_action, actuator_delay=0.15, actuator_lag=0.20, + ) + mode_changes = np.flatnonzero(np.asarray(trace.dec_mode)[1:] != np.asarray(trace.dec_mode)[:-1]) + 1 + response = trace.time >= 0.5 + + assert len(mode_changes) == 1 + assert trace.dec_mode[mode_changes[0]] == "blended" + assert trace.time[mode_changes[0]] <= 1.20 + 1e-9 + assert -0.10 < trace.a_target[mode_changes[0]] < 0.0 + assert np.all(np.asarray(trace.dec_mode)[(trace.time >= 1.75) & (trace.time < 3.25)] == "blended") + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed(): traces = [ _run(