diff --git a/common/libcommon.a b/common/libcommon.a index 3462a0fae..36b710704 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index 980c9865f..21d8cede7 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -44,6 +44,8 @@ inline static std::unordered_map keys = { {"ExperimentalLongitudinalEnabled", {PERSISTENT, BOOL}}, {"ExperimentalMode", {PERSISTENT, BOOL}}, {"ExperimentalModeConfirmed", {PERSISTENT, BOOL}}, + {"LongitudinalModelPreference", {PERSISTENT, INT, "0", "0", 1, SETTINGS_SIMPLE}}, + {"LongitudinalModelPreferenceOverride", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "-1", "-1"}}, {"PersistChillState", {PERSISTENT, BOOL, "0", "0", 1}}, {"PersistExperimentalState", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, {"PersistedCCStatus", {PERSISTENT, INT, "0", "0"}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 96abcb939..248061871 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0738d3fbc..42c91329f 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -427,7 +427,6 @@ class Controls: # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) - self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode) actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, self.starpilot_toggles, has_lead=long_plan.hasLead, traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled, diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py deleted file mode 100644 index 7068a10e0..000000000 --- a/selfdrive/controls/lib/lead_behavior.py +++ /dev/null @@ -1,176 +0,0 @@ -#!/usr/bin/env python3 -from openpilot.common.constants import CV - - -HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED = 45. * CV.MPH_TO_MS -TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED = 52. * CV.MPH_TO_MS -TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL = 1.5 -VISION_LEAD_TRACK_MIN_DISTANCE = 25.0 -VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75 -VISION_LEAD_TRACK_CLOSING_GAIN = 0.20 -VISION_LEAD_TRACK_CLOSING_CAP = 2.50 -VISION_LEAD_TRACK_EXIT_TIME_GAP = 2.30 -VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET = 1.6 -VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB = 0.70 -VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB = 0.95 -VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET = 1.1 -VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN = 0.55 -VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED = 20.0 -VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED = 25.0 -TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.40 -TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.70 -TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75 -TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN = 1.05 -TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START = 2.75 -TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END = 3.10 -TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET = 0.90 -TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET = 1.60 -TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.45 -TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.55 -RADARLESS_MATCHED_FOLLOW_MIN_SPEED = 22.0 -RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED = 2.0 -RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY = 0.95 -RADARLESS_MATCHED_FOLLOW_HEADWAY_BELOW_TARGET = 0.35 -RADARLESS_MATCHED_FOLLOW_HEADWAY_ABOVE_TARGET = 0.90 -RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE = 0.35 -RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB = 0.70 - - -def _smoothstep(value: float, start: float, end: float) -> float: - factor = min(1.0, max(0.0, (float(value) - float(start)) / max(float(end) - float(start), 1e-3))) - return factor * factor * (3.0 - 2.0 * factor) - - -def should_track_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float, - v_ego: float, *, v_lead: float | None = None, radar: bool = False) -> bool: - if not lead_status: - return False - - tracking_buffer = max(float(stop_distance), 4.0) - model_limit = float(model_length) + tracking_buffer - if radar: - return float(lead_distance) < model_limit - - closing_speed = max(0.0, float(v_ego) - float(v_lead if v_lead is not None else v_ego)) - vision_time_gap = VISION_LEAD_TRACK_BASE_TIME_GAP + min(closing_speed * VISION_LEAD_TRACK_CLOSING_GAIN, - VISION_LEAD_TRACK_CLOSING_CAP) - vision_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE, float(v_ego) * vision_time_gap + tracking_buffer) - return float(lead_distance) < min(model_limit, vision_limit) - - -def should_hold_tracked_vision_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float, - v_ego: float, *, model_prob: float, - y_rel: float, path_y: float = 0.0, radar: bool = False) -> bool: - if not lead_status or radar or float(model_prob) < VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB: - return False - if abs(float(y_rel) + float(path_y)) > VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET: - return False - - tracking_buffer = max(float(stop_distance), 4.0) - model_limit = float(model_length) + tracking_buffer - vision_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE, - float(v_ego) * VISION_LEAD_TRACK_EXIT_TIME_GAP + tracking_buffer) - if float(lead_distance) < min(model_limit, vision_exit_limit): - return True - - path_relative_offset = abs(float(y_rel) + float(path_y)) - if (float(model_prob) < VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB or - path_relative_offset > VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET): - return False - - speed_factor = 1.0 - _smoothstep( - v_ego, - VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED, - VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED, - ) - continuity_time_gap = VISION_LEAD_TRACK_EXIT_TIME_GAP + VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN * speed_factor - continuity_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE, - float(v_ego) * continuity_time_gap + tracking_buffer) - return float(lead_distance) < continuity_exit_limit - - -def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lead: float, t_follow: float, *, - radar: bool = False, lead_brake: float = 0.0, - lead_prob: float = 0.0, - min_speed: float = RADARLESS_MATCHED_FOLLOW_MIN_SPEED) -> bool: - if radar or float(t_follow) <= 0.0 or float(v_ego) < float(min_speed): - return False - if float(lead_prob) < RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB: - return False - if float(lead_brake) > RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE: - return False - - relative_speed = float(v_ego) - float(v_lead) - if abs(relative_speed) > RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED: - return False - - actual_headway = float(lead_distance) / max(float(v_ego), 1e-3) - min_headway = max(RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY, - float(t_follow) - RADARLESS_MATCHED_FOLLOW_HEADWAY_BELOW_TARGET) - max_headway = float(t_follow) + RADARLESS_MATCHED_FOLLOW_HEADWAY_ABOVE_TARGET - return min_headway <= actual_headway <= max_headway - - -def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float, - v_cruise: float | None = None, y_rel: float | None = None) -> float: - gap_error = lead_distance - desired_gap - actual_hw = lead_distance / max(v_ego, 1e-3) - desired_hw = desired_gap / max(v_ego, 1e-3) - headway_margin = actual_hw - desired_hw - - if gap_error <= 0.0: - return 0.0 - - speed_factor = _smoothstep(v_ego, HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED, TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED) - cruise_factor = 1.0 - if v_cruise is not None: - cruise_factor = _smoothstep(v_cruise - v_ego, 0.0, TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL) - if speed_factor == 0.0 or cruise_factor == 0.0: - return 0.0 - - # Encourage ACC to treat a tracked lead as the active constraint when we're - # hanging far above the requested time gap, but don't override cruise for a - # truly distant lead or one we're already closing on decisively. - fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN, - TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw) - fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN, - TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw) - entry_factor = _smoothstep(headway_margin, - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, - TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN) - exit_factor = 1.0 - _smoothstep(headway_margin, fade_start_margin, fade_end_margin) - - closing_fade_end = max(2.5, 0.12 * v_ego) - closing_fade_start = max(1.75, 0.08 * v_ego) - closing_factor = 1.0 - _smoothstep(closing_speed, closing_fade_start, closing_fade_end) - - lateral_factor = 1.0 - if y_rel is not None: - lateral_offset = abs(float(y_rel)) - lateral_factor = 1.0 - _smoothstep(lateral_offset, - TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET, - TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET) - - bias_cap = max(10.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego) - return (min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * speed_factor * cruise_factor * - entry_factor * exit_factor * closing_factor * lateral_factor) - - -def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float, - closing_speed: float, following_lead: bool) -> bool: - actual_hw = lead_distance / max(v_ego, 1e-3) - desired_hw = desired_gap / max(v_ego, 1e-3) - - if following_lead or v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED: - return False - - # Don't coast if we're already materially above the requested headway. - if actual_hw > max(desired_hw + 0.15, 1.75): - return False - - coast_window_open = lead_distance > desired_gap + max(4.0, 0.15 * v_ego) - coast_window_far = lead_distance < desired_gap + max(12.0, 0.60 * v_ego) - gentle_closing = 0.35 < closing_speed < max(1.35, 0.05 * v_ego) - ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6 - - return coast_window_open and coast_window_far and gentle_closing and ttc > 7.5 and lead_distance > desired_gap + 7.0 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 67990576e..d130763db 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,7 +4,6 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.common.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes import LongControlVehicleTuning CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -17,7 +16,6 @@ LEAD_GAP_SETTLE_MAX_START_ACCEL = 0.25 MOVING_STOP_FOLLOW_MIN_GAP = 0.25 NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35 NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40 -MODE_TRANSITION_MAX_DECEL = 4.0 LongCtrlState = car.CarControl.Actuators.LongControlState @@ -114,7 +112,6 @@ class LongControl: def __init__(self, CP): self.CP = CP self.long_control_state = LongCtrlState.off - self.experimental_mode = False self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=1 / DT_CTRL) @@ -122,38 +119,10 @@ class LongControl: kf = getattr(CP.longitudinalTuning, 'kfDEPRECATED', 0.0) self.feedforward_gain = kf if kf != 0.0 else 1.0 self.v_pid = 0.0 - self._mode_setup() self.last_output_accel = 0.0 self.stop_release_counter = 0 self.vehicle_tuning = LongControlVehicleTuning(CP) - def update_mpc_mode(self, experimental_mode): - new_mode = 'blended' if experimental_mode else 'acc' - - if self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc': - self.mode_transition_timer = 0.0 - - if new_mode != self.current_mode: - self.prev_mode = self.current_mode - self.transitioning = True - self.mode_transition_timer = 0.0 - self.mode_transition_filter.x = self.last_output_accel - - self.current_mode = new_mode - - if self.transitioning: - self.mode_transition_timer += DT_CTRL - if self.mode_transition_timer >= self.mode_transition_duration: - self.transitioning = False - - def _mode_setup(self): - self.prev_mode = 'acc' - self.current_mode = 'acc' - self.mode_transition_filter = FirstOrderFilter(0.0, 0.5, DT_CTRL) - self.mode_transition_timer = 0.0 - self.mode_transition_duration = 1.0 - self.transitioning = False - def reset(self, preserve_stop_release=False): self.pid.reset() self.vehicle_tuning.reset() @@ -271,7 +240,6 @@ class LongControl: else: # LongCtrlState.pid a_target = self.vehicle_tuning.shape_gm_truck_accel_target(a_target, CS.vEgo, should_stop) error = a_target - CS.aEgo - self.update_mpc_mode(self.experimental_mode) self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo) self._trim_positive_overshoot_integrator(a_target, error, CS) self.vehicle_tuning.trim_gm_truck_positive_hold_integrator( @@ -292,19 +260,7 @@ class LongControl: raw_output_accel = self.vehicle_tuning.apply_pedal_long_brake_bias(raw_output_accel, a_target, CS) - if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': - if raw_output_accel < 0 and raw_output_accel < self.last_output_accel: - progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration) - # Soften transition at low urgency, but keep sharp for high decel - # 20% smoother for chill decel (lower exponent) - urgency = abs(raw_output_accel / -MODE_TRANSITION_MAX_DECEL) - urgency_smooth = min(1.0, urgency ** 0.4) # 20% smoother for chill decel - blend_factor = 1.0 - (1.0 - progress) * (1.0 - urgency_smooth) - output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * blend_factor - else: - output_accel = raw_output_accel - else: - output_accel = raw_output_accel + output_accel = raw_output_accel self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py old mode 100755 new mode 100644 index 2e806b88a..3cdf2b47b --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -1,35 +1,34 @@ #!/usr/bin/env python3 import os import time + import numpy as np +from casadi import SX, vertcat from cereal import log + try: from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX except Exception: - # Build-time fallback for generated-code steps before full python extension availability. + # Generated-code builds run before every opendbc extension is available. ACCEL_MIN = -3.5 ACCEL_MAX = 2.0 -from openpilot.common.constants import CV -from openpilot.common.filter_simple import FirstOrderFilter + from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias, is_radarless_matched_follow_window -# WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function -if __name__ == '__main__': # generating code +if __name__ == "__main__": from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython -from casadi import SX, vertcat -MODEL_NAME = 'long' +MODEL_NAME = "long" LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") -SOURCES = ['lead0', 'lead1', 'cruise', 'e2e'] +SOURCES = ("lead0", "lead1", "cruise") X_DIM = 3 U_DIM = 1 @@ -38,987 +37,376 @@ COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 -# ===== VOACC SPEED-BASED TUNING PARAMETERS ===== -# City: Emergency-responsive | Highway: Rubber-banding prevention -# Speed ranges: [0-35, 35-55, 55-70, 70+ mph] - -# SPEED BREAKPOINTS (mph) -SPEED_BREAKPOINTS = [0, 35, 55, 70] # 4 ranges: 0-35, 35-55, 55-70, 70+ - -# ===== CHANGE THESE VALUES FOR DIFFERENT SPEEDS ===== - -# RESPONSIVENESS TO LEAD CARS (Lower = More responsive, Higher = More stable) -# [City Emergency, Urban Hwy, Rural Hwy, High Speed] -X_EGO_OBSTACLE_COSTS = [3.0, 3.0, 2.5, 2.0] # Less aggressive at low speeds, closer to original - -# JERK CONTROL (Lower = More jerky/responsive, Higher = Smoother/conservative) -# [City Emergency, Urban Hwy, Rural Hwy, High Speed] -J_EGO_COSTS = [5.0, 4.75, 4.5, 4.0] # Reverted to original 5.0 at low speeds - -# ACCELERATION CHANGE PENALTIES (Lower = More responsive, Higher = Smoother) -# [City Emergency, Urban Hwy, Rural Hwy, High Speed] -A_CHANGE_COSTS = [200, 195, 180, 170] # Reverted to original 200 at low speeds - -# SMOOTHING FILTERS - Speed-adaptive for optimal responsiveness -# Lower = More responsive, Higher = Smoother -LEAD_FILTER_TIME_LOW = 0.8 # Under 40 mph: Fast response for city emergency braking -LEAD_FILTER_TIME_HIGH = 1.2 # Over 40 mph: Faster response to prevent highway gaps -SPEED_FILTER_THRESHOLD = 40 * CV.MPH_TO_MS # 40 mph threshold -DUPLICATE_VISION_LEAD_FILTER_TIME = 0.15 - -# DISTANCE ADAPTATION STRENGTH (How much penalties increase when close to lead) -# [City, Urban Hwy, Rural Hwy, High Speed] -DIST_ADAPTS = [0.04, 0.06, 0.06, 0.05] # Balanced across speeds - -# ===== END TUNING PARAMETERS ===== - -FAR_RADAR_LEAD_ACCEL_TAPER_MAX = 1.0 -FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING = 2.5 -FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS = 8.0 -FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN = 0.25 -FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS = 25.0 -FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN = 0.9 -STABLE_FOLLOW_CRUISE_MIN_SPEED = 12.0 -STABLE_FOLLOW_CRUISE_HYSTERESIS_MIN = 4.0 -STABLE_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.14 -STABLE_FOLLOW_CRUISE_MAX_REL_SPEED = 2.5 -STABLE_FOLLOW_CRUISE_MIN_HEADWAY = 0.95 -STABLE_FOLLOW_CRUISE_HEADWAY_BELOW_TARGET = 0.35 -STABLE_FOLLOW_CRUISE_HEADWAY_ABOVE_TARGET = 0.90 -STABLE_FOLLOW_CRUISE_MAX_LEAD_BRAKE = 0.35 -STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_REL_SPEED = 2.0 -STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_HEADWAY_MARGIN = 0.35 -STABLE_FOLLOW_CRUISE_PULLAWAY_MIN_HEADWAY_MARGIN = -0.10 -STABLE_FOLLOW_CRUISE_PULLAWAY_HYSTERESIS_MAX = 1.75 -VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB = 0.95 -VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 2.0 -NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 20.0 -NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_MIN_SPEED = 10.0 -NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB = 0.9 -NEAR_DUPLICATE_LEAD_SOURCE_MAX_LEAD_BRAKE = 0.35 -NEAR_DUPLICATE_LEAD_SOURCE_MAX_DREL_DIFF = 1.5 -NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF = 0.35 -NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MIN = 1.25 -NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MAX = 2.25 -NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_KEEP_MARGIN = 0.35 -IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MIN_SPEED = 10.0 -IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_HEADWAY_ABOVE_TARGET = 0.40 -IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_LEAD_BRAKE = 0.25 -IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_PULLAWAY_SPEED = 1.5 -IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 10.0 -IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED = 15.0 -IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET = 0.55 -IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET = -0.15 -IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_LEAD_BRAKE = 0.25 -IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_PULLAWAY_SPEED = 0.75 -IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX = 10.0 - -# Function to get parameter value based on current speed -def get_speed_based_param(speed_mph, param_array): - """Get parameter value based on current speed using smooth interpolation""" - return float(np.interp(speed_mph, SPEED_BREAKPOINTS, param_array)) - -# Current active values (set based on speed) -X_EGO_OBSTACLE_COST = 2.75 -J_EGO_COST = 5.5 -A_CHANGE_COST = 250.0 -LEAD_FILTER_TIME = 2.0 -DIST_ADAPT = 0.06 - -X_EGO_COST = 0. -V_EGO_COST = 0. -A_EGO_COST = 0. -DANGER_ZONE_COST = 100. -CRASH_DISTANCE = .25 +X_EGO_OBSTACLE_COST = 3.0 +J_EGO_COST = 5.0 +A_CHANGE_COST = 200.0 +DANGER_ZONE_COST = 100.0 LEAD_DANGER_FACTOR = 0.75 LIMIT_COST = 1e6 -ACADOS_SOLVER_TYPE = 'SQP_RTI' -# Default lead acceleration decay set to 50% at 1s +CRASH_DISTANCE = 0.25 +ACADOS_SOLVER_TYPE = "SQP_RTI" + +N = 12 +MAX_T = 10.0 +T_IDXS = np.array([index_function(i, max_val=MAX_T, max_idx=N) for i in range(N + 1)]) +T_DIFFS = np.diff(T_IDXS, prepend=[0.0]) +FCW_IDXS = T_IDXS < 5.0 + +COMFORT_BRAKE = 2.5 +STOP_DISTANCE = 6.0 +CRUISE_MIN_ACCEL = -1.2 +CRUISE_MAX_ACCEL = 1.6 LEAD_ACCEL_TAU = 1.5 +LEAD_FILTER_TAU = 0.45 +LEAD_FILTER_RESET_DISTANCE = 8.0 + FCW_MIN_MODEL_PROB = 0.9 FCW_MIN_CLOSING_SPEED = 0.5 FCW_MAX_TTC = 4.0 -# Fewer timestamps don't hurt performance and lead to -# much better convergence of the MPC with low iterations -N = 12 -MAX_T = 10.0 -T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)] - -T_IDXS = np.array(T_IDXS_LST) -FCW_IDXS = T_IDXS < 5.0 -T_DIFFS = np.diff(T_IDXS, prepend=[0.]) -COMFORT_BRAKE = 2.5 -STOP_DISTANCE = 6.0 +def _personality_value(aggressive, standard, relaxed, personality): + return { + log.LongitudinalPersonality.aggressive: aggressive, + log.LongitudinalPersonality.standard: standard, + log.LongitudinalPersonality.relaxed: relaxed, + }[personality] -def should_trigger_planner_fcw(lead, v_ego: float) -> bool: - if lead is None or not lead.status or float(getattr(lead, "modelProb", 0.0)) <= FCW_MIN_MODEL_PROB: - return False +def get_jerk_factor(aggressive_accel=0.5, aggressive_danger=0.5, aggressive_speed=0.5, + standard_accel=1.0, standard_danger=1.0, standard_speed=1.0, + relaxed_accel=1.0, relaxed_danger=1.0, relaxed_speed=1.0, + custom_personalities=False, + personality=log.LongitudinalPersonality.standard): + if not custom_personalities: + factor = 0.5 if personality == log.LongitudinalPersonality.aggressive else 1.0 + return factor, factor, factor - closing_speed = max(0.0, float(v_ego) - float(getattr(lead, "vLead", 0.0))) - if closing_speed < FCW_MIN_CLOSING_SPEED: - return False + return ( + _personality_value(aggressive_accel, standard_accel, relaxed_accel, personality), + _personality_value(aggressive_danger, standard_danger, relaxed_danger, personality), + _personality_value(aggressive_speed, standard_speed, relaxed_speed, personality), + ) - ttc = max(0.0, float(getattr(lead, "dRel", 0.0))) / max(closing_speed, 1e-3) - return ttc < FCW_MAX_TTC -def get_jerk_factor(aggressive_jerk_acceleration=0.5, aggressive_jerk_danger=0.5, aggressive_jerk_speed=0.5, - standard_jerk_acceleration=1.0, standard_jerk_danger=1.0, standard_jerk_speed=1.0, - relaxed_jerk_acceleration=1.0, relaxed_jerk_danger=1.0, relaxed_jerk_speed=1.0, - custom_personalities=False, personality=log.LongitudinalPersonality.standard): +def get_T_FOLLOW(aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, + custom_personalities=False, + personality=log.LongitudinalPersonality.standard): if custom_personalities: - if personality==log.LongitudinalPersonality.relaxed: - return relaxed_jerk_acceleration, relaxed_jerk_danger, relaxed_jerk_speed - elif personality==log.LongitudinalPersonality.standard: - return standard_jerk_acceleration, standard_jerk_danger, standard_jerk_speed - elif personality==log.LongitudinalPersonality.aggressive: - return aggressive_jerk_acceleration, aggressive_jerk_danger, aggressive_jerk_speed - else: - raise NotImplementedError("Longitudinal personality not supported") - else: - if personality==log.LongitudinalPersonality.relaxed: - return 1.0, 1.0, 1.0 - elif personality==log.LongitudinalPersonality.standard: - return 1.0, 1.0, 1.0 - elif personality==log.LongitudinalPersonality.aggressive: - return 0.5, 0.5, 0.5 - else: - raise NotImplementedError("Longitudinal personality not supported") + return _personality_value(aggressive_follow, standard_follow, relaxed_follow, personality) + return _personality_value(1.25, 1.45, 1.75, personality) -def get_T_FOLLOW(aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, custom_personalities=False, personality=log.LongitudinalPersonality.standard): - if custom_personalities: - if personality==log.LongitudinalPersonality.relaxed: - return relaxed_follow - elif personality==log.LongitudinalPersonality.standard: - return standard_follow - elif personality==log.LongitudinalPersonality.aggressive: - return aggressive_follow - else: - raise NotImplementedError("Longitudinal personality not supported") - else: - if personality==log.LongitudinalPersonality.relaxed: - return 1.75 - elif personality==log.LongitudinalPersonality.standard: - return 1.45 - elif personality==log.LongitudinalPersonality.aggressive: - return 1.25 - else: - raise NotImplementedError("Longitudinal personality not supported") - def get_stopped_equivalence_factor(v_lead): - return (v_lead**2) / (2 * COMFORT_BRAKE) + return (v_lead ** 2) / (2.0 * COMFORT_BRAKE) + def get_safe_obstacle_distance(v_ego, t_follow): - return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + return (v_ego ** 2) / (2.0 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + def desired_follow_distance(v_ego, v_lead, t_follow=None): - if t_follow is None: - t_follow = get_T_FOLLOW() + t_follow = get_T_FOLLOW() if t_follow is None else t_follow return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) -def soften_far_radar_lead_accel(d_rel, v_lead, a_lead, v_ego, t_follow, *, radar=True): - if not radar or a_lead >= 0.0: - return float(a_lead) +def should_trigger_planner_fcw(lead, v_ego): + if lead is None or not bool(getattr(lead, "status", False)): + return False + if float(getattr(lead, "modelProb", 0.0)) <= FCW_MIN_MODEL_PROB: + return False - desired_gap = float(desired_follow_distance(v_ego, v_lead, t_follow)) - closing_speed = max(0.0, float(v_ego) - float(v_lead)) - gap_excess = float(d_rel) - desired_gap - - taper_start = max(FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS, - FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN * float(v_ego)) - if gap_excess <= taper_start or closing_speed >= FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING: - return float(a_lead) - - taper_scale = max(FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS, - FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN * float(v_ego)) - distance_factor = float(np.clip((gap_excess - taper_start) / taper_scale, 0.0, 1.0)) - closing_factor = float(np.clip((FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING - closing_speed) / - FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING, 0.0, 1.0)) - taper = FAR_RADAR_LEAD_ACCEL_TAPER_MAX * distance_factor * closing_factor - return float(a_lead * (1.0 - taper)) + closing_speed = max(0.0, float(v_ego) - float(getattr(lead, "vLead", 0.0))) + ttc = max(0.0, float(getattr(lead, "dRel", 0.0))) / max(closing_speed, 1e-3) + return closing_speed >= FCW_MIN_CLOSING_SPEED and ttc < FCW_MAX_TTC def gen_long_model(): model = AcadosModel() model.name = MODEL_NAME - # set up states & controls - x_ego = SX.sym('x_ego') - v_ego = SX.sym('v_ego') - a_ego = SX.sym('a_ego') + x_ego = SX.sym("x_ego") + v_ego = SX.sym("v_ego") + a_ego = SX.sym("a_ego") model.x = vertcat(x_ego, v_ego, a_ego) - # controls - j_ego = SX.sym('j_ego') + j_ego = SX.sym("j_ego") model.u = vertcat(j_ego) - # xdot - x_ego_dot = SX.sym('x_ego_dot') - v_ego_dot = SX.sym('v_ego_dot') - a_ego_dot = SX.sym('a_ego_dot') + x_ego_dot = SX.sym("x_ego_dot") + v_ego_dot = SX.sym("v_ego_dot") + a_ego_dot = SX.sym("a_ego_dot") model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot) - # live parameters - a_min = SX.sym('a_min') - a_max = SX.sym('a_max') - x_obstacle = SX.sym('x_obstacle') - prev_a = SX.sym('prev_a') - lead_t_follow = SX.sym('lead_t_follow') - lead_danger_factor = SX.sym('lead_danger_factor') + a_min = SX.sym("a_min") + a_max = SX.sym("a_max") + x_obstacle = SX.sym("x_obstacle") + prev_a = SX.sym("prev_a") + lead_t_follow = SX.sym("lead_t_follow") + lead_danger_factor = SX.sym("lead_danger_factor") model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor) - # dynamics model - f_expl = vertcat(v_ego, a_ego, j_ego) - model.f_impl_expr = model.xdot - f_expl - model.f_expl_expr = f_expl + dynamics = vertcat(v_ego, a_ego, j_ego) + model.f_impl_expr = model.xdot - dynamics + model.f_expl_expr = dynamics return model def gen_long_ocp(): ocp = AcadosOcp() ocp.model = gen_long_model() - - Tf = T_IDXS[-1] - - # set dimensions ocp.dims.N = N - # set cost module - ocp.cost.cost_type = 'NONLINEAR_LS' - ocp.cost.cost_type_e = 'NONLINEAR_LS' - - QR = np.zeros((COST_DIM, COST_DIM)) - Q = np.zeros((COST_E_DIM, COST_E_DIM)) - - ocp.cost.W = QR - ocp.cost.W_e = Q + ocp.cost.cost_type = "NONLINEAR_LS" + ocp.cost.cost_type_e = "NONLINEAR_LS" + ocp.cost.W = np.zeros((COST_DIM, COST_DIM)) + ocp.cost.W_e = np.zeros((COST_E_DIM, COST_E_DIM)) x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2] j_ego = ocp.model.u[0] - - a_min, a_max = ocp.model.p[0], ocp.model.p[1] + a_min = ocp.model.p[0] + a_max = ocp.model.p[1] x_obstacle = ocp.model.p[2] prev_a = ocp.model.p[3] - lead_t_follow = ocp.model.p[4] - lead_danger_factor = ocp.model.p[5] + t_follow = ocp.model.p[4] + danger_factor = ocp.model.p[5] - ocp.cost.yref = np.zeros((COST_DIM, )) - ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - - desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow) - - # The main cost in normal operation is how close you are to the "desired" distance - # from an obstacle at every timestep. This obstacle can be a lead car - # or other object. In e2e mode we can use x_position targets as a cost - # instead. - accel_change = a_ego - prev_a - costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.), - x_ego, - v_ego, - a_ego, - accel_change, - j_ego] + desired_distance = get_safe_obstacle_distance(v_ego, t_follow) + costs = [ + ((x_obstacle - x_ego) - desired_distance) / (v_ego + 10.0), + x_ego, + v_ego, + a_ego, + a_ego - prev_a, + j_ego, + ] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) + ocp.cost.yref = np.zeros(COST_DIM) + ocp.cost.yref_e = np.zeros(COST_E_DIM) - # Constraints on speed, acceleration and desired distance to - # the obstacle, which is treated as a slack constraint so it - # behaves like an asymmetrical cost. - constraints = vertcat(v_ego, - (a_ego - a_min), - (a_max - a_ego), - ((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.)) - ocp.model.con_h_expr = constraints + ocp.model.con_h_expr = vertcat( + v_ego, + a_ego - a_min, + a_max - a_ego, + ((x_obstacle - x_ego) - danger_factor * desired_distance) / (v_ego + 10.0), + ) - x0 = np.zeros(X_DIM) - ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR]) + ocp.constraints.x0 = np.zeros(X_DIM) + ocp.parameter_values = np.array([ + CRUISE_MIN_ACCEL, + CRUISE_MAX_ACCEL, + 0.0, + 0.0, + get_T_FOLLOW(), + LEAD_DANGER_FACTOR, + ]) - - # We put all constraint cost weights to 0 and only set them at runtime - cost_weights = np.zeros(CONSTR_DIM) - ocp.cost.zl = cost_weights - ocp.cost.Zl = cost_weights - ocp.cost.Zu = cost_weights - ocp.cost.zu = cost_weights - - ocp.constraints.lh = np.zeros(CONSTR_DIM) - ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM) + zero_constraints = np.zeros(CONSTR_DIM) + ocp.cost.zl = zero_constraints + ocp.cost.Zl = zero_constraints + ocp.cost.Zu = zero_constraints + ocp.cost.zu = zero_constraints + ocp.constraints.lh = zero_constraints + ocp.constraints.uh = 1e4 * np.ones(CONSTR_DIM) ocp.constraints.idxsh = np.arange(CONSTR_DIM) - # The HPIPM solver can give decent solutions even when it is stopped early - # Which is critical for our purpose where compute time is strictly bounded - # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This - # does not cause issues since the problem is well bounded. - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' - ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' - ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" + ocp.solver_options.hessian_approx = "GAUSS_NEWTON" + ocp.solver_options.integrator_type = "ERK" ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE ocp.solver_options.qp_solver_cond_N = 1 - - # More iterations take too much time and less lead to inaccurate convergence in - # some situations. Ideally we would run just 1 iteration to ensure fixed runtime. ocp.solver_options.qp_solver_iter_max = 10 ocp.solver_options.qp_tol = 1e-3 - - # set prediction horizon - ocp.solver_options.tf = Tf + ocp.solver_options.tf = T_IDXS[-1] ocp.solver_options.shooting_nodes = T_IDXS - ocp.code_export_directory = EXPORT_DIR return ocp class LongitudinalMpc: - def __init__(self, mode='acc', dt=DT_MDL): + def __init__(self, mode="acc", dt=DT_MDL): self.mode = mode self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) - self.source = SOURCES[2] - # Initialize smoothing filters with default time constants - self.current_filter_time = LEAD_FILTER_TIME_LOW - self.lead_a_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt) - self.lead_v_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt) - self.duplicate_lead_a_filters = [FirstOrderFilter(0.0, 0.0, self.dt, initialized=False) for _ in range(2)] - self.duplicate_lead_v_filters = [FirstOrderFilter(0.0, 0.0, self.dt, initialized=False) for _ in range(2)] - # Slew-limited filter factor to avoid abrupt 0.50↔1.00 jumps - self.filter_time_factor = 1.0 - self.prev_filter_time_factor = 1.0 - self.slew_per_sec = 1.0 - # Instance variables to avoid global modifications - self.current_x_ego_cost = X_EGO_OBSTACLE_COSTS[0] - self.current_j_ego_cost = J_EGO_COSTS[0] - self.current_a_change_cost = A_CHANGE_COSTS[0] - self.current_dist_adapt = DIST_ADAPTS[0] - # Initialize acceleration limits to prevent AttributeError - self.cruise_min_a = ACCEL_MIN - self.max_a = min(ACCEL_MAX, 1.2) + self.cruise_min_a = CRUISE_MIN_ACCEL + self.max_a = CRUISE_MAX_ACCEL + self.lead_filter_tau = LEAD_FILTER_TAU + self.source = "cruise" + self._lead_filter_state = [None, None] self.reset() def reset(self): - # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.solver.reset() - # self.solver.options_set('print_level', 2) - self.v_solution = np.zeros(N+1) - self.a_solution = np.zeros(N+1) - self.prev_a = np.array(self.a_solution) + self.x_sol = np.zeros((N + 1, X_DIM)) + self.u_sol = np.zeros((N, U_DIM)) + self.v_solution = np.zeros(N + 1) + self.a_solution = np.zeros(N + 1) self.j_solution = np.zeros(N) - self.yref = np.zeros((N+1, COST_DIM)) + self.prev_a = np.zeros(N + 1) + self.yref = np.zeros((N + 1, COST_DIM)) + self.params = np.zeros((N + 1, PARAM_DIM)) + self.x0 = np.zeros(X_DIM) + self.crash_cnt = 0 + self.solution_status = 0 + self.solve_time = 0.0 + self.last_cloudlog_t = 0.0 + self._lead_filter_state = [None, None] + for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - self.x_sol = np.zeros((N+1, X_DIM)) - self.u_sol = np.zeros((N,1)) - self.params = np.zeros((N+1, PARAM_DIM)) - for i in range(N+1): - self.solver.set(i, 'x', np.zeros(X_DIM)) - self.last_cloudlog_t = 0 - self.status = False - self.crash_cnt = 0.0 - self.solution_status = 0 - # timers - self.solve_time = 0.0 - self.time_qp_solution = 0.0 - self.time_linearization = 0.0 - self.time_integrator = 0.0 - self.x0 = np.zeros(X_DIM) - for lead_filter in (*self.duplicate_lead_a_filters, *self.duplicate_lead_v_filters): - lead_filter.x = 0.0 - lead_filter.initialized = False + for i in range(N + 1): + self.solver.set(i, "x", np.zeros(X_DIM)) self.set_weights() def set_cost_weights(self, cost_weights, constraint_cost_weights): - W = np.asfortranarray(np.diag(cost_weights)) + weights = np.asfortranarray(np.diag(cost_weights)) for i in range(N): - # TODO don't hardcode A_CHANGE_COST idx - # reduce the cost on (a-a_prev) later in the horizon. - W[4,4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) - self.solver.cost_set(i, 'W', W) - # Setting the slice without the copy make the array not contiguous, - # causing issues with the C interface. - self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) + weights[4, 4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) + self.solver.cost_set(i, "W", weights) + self.solver.cost_set(N, "W", np.copy(weights[:COST_E_DIM, :COST_E_DIM])) - # Set L2 slack cost on lower bound constraints - Zl = np.array(constraint_cost_weights) + constraints = np.asarray(constraint_cost_weights) for i in range(N): - self.solver.cost_set(i, 'Zl', Zl) + self.solver.cost_set(i, "Zl", constraints) - def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, - personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0, - uncertainty=0.0, accel_reengage=False, panic_bypass=False, - filter_time_factor_floor=0.0): - # Update parameters based on current speed with interpolation for smooth scaling - speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph + def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, + prev_accel_constraint=True, **_): + accel_change_cost = acceleration_jerk * A_CHANGE_COST if prev_accel_constraint else 0.0 + costs = [X_EGO_OBSTACLE_COST, 0.0, 0.0, 0.0, accel_change_cost, speed_jerk * J_EGO_COST] + constraints = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk * DANGER_ZONE_COST] + self.set_cost_weights(costs, constraints) - # Use speed-based parameters for smooth scaling across all breakpoints - self.current_x_ego_cost = get_speed_based_param(speed_mph, X_EGO_OBSTACLE_COSTS) - self.current_j_ego_cost = get_speed_based_param(speed_mph, J_EGO_COSTS) - self.current_a_change_cost = get_speed_based_param(speed_mph, A_CHANGE_COSTS) + def set_cur_state(self, v_ego, a_ego): + previous_v = self.x0[1] + self.x0[1] = v_ego + self.x0[2] = a_ego + if abs(previous_v - v_ego) > 2.0: + for i in range(N + 1): + self.solver.set(i, "x", self.x0) - # For dist_adapt, start from 0.0 under low speeds while enabling full smooth transitions - dist_adapt_array = [0.0, DIST_ADAPTS[1], DIST_ADAPTS[2], DIST_ADAPTS[3]] - self.current_dist_adapt = get_speed_based_param(speed_mph, dist_adapt_array) - - # Update filter time constants with interp and recreate filters if needed - if speed_mph < 47: - self.current_filter_time = 0.0 - else: - self.current_filter_time = np.interp(speed_mph, [47, 65], [0.0, LEAD_FILTER_TIME_HIGH]) - if abs(self.current_filter_time - getattr(self, 'prev_filter_time', 0)) > 0.1: # Only update if significant change - # Recreate filters with new time constant while preserving current values - current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0 - current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0 - self.lead_a_filter = FirstOrderFilter(current_a, self.current_filter_time, self.dt) - self.lead_v_filter = FirstOrderFilter(current_v, self.current_filter_time, self.dt) - self.prev_filter_time = self.current_filter_time - # Adaptive jerk factors for distance with interp scaling - dist_factor = 1.0 + self.current_dist_adapt * (20.0 / max(lead_dist, 5.0)) - acceleration_jerk *= dist_factor - danger_jerk *= dist_factor - speed_jerk *= dist_factor - - # Scene complexity adjustment based on model uncertainty - # Target factor from uncertainty - if uncertainty <= 0.45: - tgt_factor = 1.0 - elif uncertainty >= 0.70: - tgt_factor = 0.0 - else: - tgt_factor = float(np.interp(uncertainty, [0.45, 0.70], [1.0, 0.30])) - - if accel_reengage: - tgt_factor = min(tgt_factor, 0.5) - - # Hard bypass of smoothing when approaching fast or magnitude trips - if panic_bypass: - tgt_factor = 0.0 - else: - tgt_factor = max(tgt_factor, float(filter_time_factor_floor)) - - # Slew-limit changes to avoid step-wise filter jumps - if panic_bypass: - # A real closing hazard must never wait for the comfort filter to unwind. - self.filter_time_factor = 0.0 - else: - max_step = self.slew_per_sec * self.dt - delta = np.clip(tgt_factor - self.filter_time_factor, -max_step, max_step) - self.filter_time_factor += float(delta) - - # When uncertainty is moderately elevated, allow accel but cap jerk by increasing jerk cost - if 0.45 <= uncertainty < 0.60: - scale = float(np.interp(uncertainty, [0.45, 0.60], [1.2, 1.5])) - speed_jerk *= scale - - if self.mode == 'acc': - a_change_cost = acceleration_jerk if prev_accel_constraint else 0 - cost_weights = [self.current_x_ego_cost, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk] - elif self.mode == 'blended': - a_change_cost = 40.0 if prev_accel_constraint else 0 - cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk] - else: - raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') - self.set_cost_weights(cost_weights, constraint_cost_weights) - - # Adjust filter time constants for complex scenes - filter_time_factor = float(self.filter_time_factor) - if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.05: - new_filter_time = self.current_filter_time * filter_time_factor - current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0 - current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0 - self.lead_a_filter = FirstOrderFilter(current_a, new_filter_time, self.dt) - self.lead_v_filter = FirstOrderFilter(current_v, new_filter_time, self.dt) - self.prev_filter_time_factor = filter_time_factor - - def set_cur_state(self, v, a): - v_prev = self.x0[1] - self.x0[1] = v - self.x0[2] = a - if abs(v_prev - v) > 2.: # probably only helps if v < v_prev - for i in range(N+1): - self.solver.set(i, 'x', self.x0) + def set_accel_limits(self, min_a, max_a): + self.cruise_min_a = float(min_a) + self.max_a = float(max_a) @staticmethod - def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego=0.0): - speed_mph = v_ego * CV.MS_TO_MPH - bp = [0, 20, 35] - exp_weight = np.interp(speed_mph, bp, [1.0, 1.0, 0.0]) # Full exp at <20, blend to constant at 35 + def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): + a_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS ** 2) / 2.0) + v_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_traj), 0.0, 1e8) + x_traj = x_lead + np.cumsum(T_DIFFS * v_traj) + return np.column_stack((x_traj, v_traj)) - if exp_weight > 0: - # Exponential decay component - a_lead_traj_exp = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) - v_lead_traj_exp = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj_exp), 0.0, 1e8) - x_lead_traj_exp = x_lead + np.cumsum(T_DIFFS * v_lead_traj_exp) + def _filter_lead_motion(self, index, lead): + raw_distance = float(lead.dRel) + raw_velocity = float(lead.vLead) + raw_accel = float(lead.aLeadK) + state = self._lead_filter_state[index] + + reset = state is None or abs(raw_distance - state[2]) > LEAD_FILTER_RESET_DISTANCE + if reset: + filtered_velocity = raw_velocity + filtered_accel = raw_accel else: - x_lead_traj_exp = np.zeros_like(T_IDXS) - v_lead_traj_exp = np.zeros_like(T_IDXS) + alpha = self.dt / (self.lead_filter_tau + self.dt) + filtered_velocity = state[0] + alpha * (raw_velocity - state[0]) + filtered_accel = state[1] + alpha * (raw_accel - state[1]) - # Constant acceleration component - v_lead_traj_const = np.clip(v_lead + a_lead * T_IDXS, 0.0, 1e8) - x_lead_traj_const = x_lead + v_lead * T_IDXS + 0.5 * a_lead * T_IDXS**2 + self._lead_filter_state[index] = (filtered_velocity, filtered_accel, raw_distance) + return filtered_velocity, filtered_accel - # Blend based on weight - v_lead_traj = exp_weight * v_lead_traj_exp + (1 - exp_weight) * v_lead_traj_const - x_lead_traj = exp_weight * x_lead_traj_exp + (1 - exp_weight) * x_lead_traj_const - - lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) - return lead_xv - - def process_lead(self, lead, tracking_lead=True, t_follow=None, *, lead_index=0, - smooth_duplicate_vision=False): + def process_lead(self, lead, index): v_ego = self.x0[1] - lead_active = lead is not None and lead.status and tracking_lead - if lead_active: - x_lead = lead.dRel - v_lead = lead.vLead - a_lead = lead.aLeadK - a_lead_tau = lead.aLeadTau - a_lead = soften_far_radar_lead_accel(x_lead, v_lead, a_lead, v_ego, - get_T_FOLLOW() if t_follow is None else t_follow, - radar=bool(getattr(lead, "radar", False))) + present = lead is not None and bool(getattr(lead, "status", False)) + if present: + x_lead = float(lead.dRel) + v_lead, a_lead = self._filter_lead_motion(index, lead) + a_lead_tau = max(float(getattr(lead, "aLeadTau", LEAD_ACCEL_TAU)), 0.1) else: - # Fake a fast lead car, so mpc can keep running in the same mode + self._lead_filter_state[index] = None x_lead = 50.0 v_lead = v_ego + 10.0 a_lead = 0.0 a_lead_tau = LEAD_ACCEL_TAU - # MPC will not converge if immediate crash is expected. - # Bound this by physical hard-brake capability, not cruise comfort decel. - min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) - x_lead = np.clip(x_lead, min_x_lead, 1e8) - v_lead = np.clip(v_lead, 0.0, 1e8) - a_lead = np.clip(a_lead, -10., 5.) - if lead_active and smooth_duplicate_vision and not bool(getattr(lead, "radar", False)): - # Keep the baseline filter synchronized so leaving this narrow comfort - # path cannot introduce a state discontinuity. - self.lead_a_filter.update(a_lead) - self.lead_v_filter.update(v_lead) + min_x_lead = ((v_ego + v_lead) / 2.0) * (v_ego - v_lead) / (-ACCEL_MIN * 2.0) + x_lead = float(np.clip(x_lead, min_x_lead, 1e8)) + v_lead = float(np.clip(v_lead, 0.0, 1e8)) + a_lead = float(np.clip(a_lead, -10.0, 5.0)) + return self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) - filter_time = self.current_filter_time - filter_time = max(filter_time, DUPLICATE_VISION_LEAD_FILTER_TIME) - filter_time *= self.filter_time_factor - - a_filter = self.duplicate_lead_a_filters[lead_index] - v_filter = self.duplicate_lead_v_filters[lead_index] - a_filter.update_alpha(filter_time) - v_filter.update_alpha(filter_time) - a_lead = a_filter.update(a_lead) - v_lead = v_filter.update(v_lead) - else: - # Preserve the historical planner path outside the qualified comfort scene. - self.lead_a_filter.update(a_lead) - self.lead_v_filter.update(v_lead) - a_lead = self.lead_a_filter.x - v_lead = self.lead_v_filter.x - self.duplicate_lead_a_filters[lead_index].initialized = False - self.duplicate_lead_v_filters[lead_index].initialized = False - lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego) - return lead_xv - - @staticmethod - def get_stable_follow_cruise_hysteresis(lead, v_ego, t_follow): - if lead is None or not lead.status: - return 0.0 - - lead_radar = bool(getattr(lead, "radar", False)) - relative_speed = float(v_ego) - float(lead.vLead) - actual_headway = None - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > STABLE_FOLLOW_CRUISE_MAX_LEAD_BRAKE: - return 0.0 - - if lead_radar: - if float(t_follow) <= 0.0 or float(v_ego) < STABLE_FOLLOW_CRUISE_MIN_SPEED: - return 0.0 - if abs(relative_speed) > STABLE_FOLLOW_CRUISE_MAX_REL_SPEED: - return 0.0 - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - min_headway = max(STABLE_FOLLOW_CRUISE_MIN_HEADWAY, - float(t_follow) - STABLE_FOLLOW_CRUISE_HEADWAY_BELOW_TARGET) - max_headway = float(t_follow) + STABLE_FOLLOW_CRUISE_HEADWAY_ABOVE_TARGET - if not (min_headway <= actual_headway <= max_headway): - return 0.0 - elif not is_radarless_matched_follow_window( - v_ego, - lead.dRel, - lead.vLead, - t_follow, - radar=lead_radar, - lead_brake=lead_brake, - lead_prob=float(getattr(lead, "modelProb", 0.0)), - min_speed=STABLE_FOLLOW_CRUISE_MIN_SPEED, - ): - return 0.0 - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - - hysteresis = max(STABLE_FOLLOW_CRUISE_HYSTERESIS_MIN, - STABLE_FOLLOW_CRUISE_HYSTERESIS_GAIN * float(v_ego)) - - if relative_speed < 0.0: - headway_margin = actual_headway - float(t_follow) - if headway_margin <= STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_HEADWAY_MARGIN: - rel_speed_factor = float(np.clip((-relative_speed) / STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_REL_SPEED, 0.0, 1.0)) - headway_factor = float(np.clip( - (STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_HEADWAY_MARGIN - headway_margin) / - max(STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_HEADWAY_MARGIN - STABLE_FOLLOW_CRUISE_PULLAWAY_MIN_HEADWAY_MARGIN, 1e-3), - 0.0, 1.0, - )) - hysteresis += STABLE_FOLLOW_CRUISE_PULLAWAY_HYSTERESIS_MAX * rel_speed_factor * headway_factor - - return hysteresis - - @staticmethod - def leads_share_identical_radar_track(lead_one, lead_two): - if lead_one is None or lead_two is None or not lead_one.status or not lead_two.status: - return False - if not (bool(getattr(lead_one, "radar", False)) and bool(getattr(lead_two, "radar", False))): - return False - track_one = int(getattr(lead_one, "radarTrackId", -1)) - track_two = int(getattr(lead_two, "radarTrackId", -1)) - return track_one >= 0 and track_one == track_two - - @staticmethod - def leads_are_near_duplicates(lead_one, lead_two, v_ego, *, vision_min_speed=None): - if lead_one is None or lead_two is None or not lead_one.status or not lead_two.status: - return False - if LongitudinalMpc.leads_share_identical_radar_track(lead_one, lead_two): - if float(v_ego) < NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_MIN_SPEED: - return False - return ( - abs(float(lead_one.dRel) - float(lead_two.dRel)) <= NEAR_DUPLICATE_LEAD_SOURCE_MAX_DREL_DIFF and - abs(float(lead_one.vRel) - float(lead_two.vRel)) <= max(1.0, NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF) - ) - min_vision_speed = NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED if vision_min_speed is None else float(vision_min_speed) - if float(v_ego) < min_vision_speed: - return False - lead_one_radar = bool(getattr(lead_one, "radar", False)) - lead_two_radar = bool(getattr(lead_two, "radar", False)) - if lead_one_radar or lead_two_radar: - return False - if float(getattr(lead_one, "modelProb", 0.0)) < NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB: - return False - if float(getattr(lead_two, "modelProb", 0.0)) < NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB: - return False - if max(0.0, -float(getattr(lead_one, "aLeadK", 0.0))) > NEAR_DUPLICATE_LEAD_SOURCE_MAX_LEAD_BRAKE: - return False - if max(0.0, -float(getattr(lead_two, "aLeadK", 0.0))) > NEAR_DUPLICATE_LEAD_SOURCE_MAX_LEAD_BRAKE: - return False - - return ( - abs(float(lead_one.dRel) - float(lead_two.dRel)) <= NEAR_DUPLICATE_LEAD_SOURCE_MAX_DREL_DIFF and - abs(float(lead_one.vRel) - float(lead_two.vRel)) <= NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF - ) - - def get_near_duplicate_lead_source_hysteresis(self, prev_source, lead_one, lead_two, v_ego): - if prev_source not in ("lead0", "lead1"): - return 0.0, 0.0 - if not self.leads_are_near_duplicates(lead_one, lead_two, v_ego): - return 0.0, 0.0 - - hysteresis = float(np.interp( - float(v_ego), - [NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED, 35.0], - [NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MIN, NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MAX], - )) - if prev_source == "lead0": - return 0.0, hysteresis - return hysteresis, 0.0 - - def get_identical_radar_duplicate_source_hold(self, prev_source, lead_one, lead_two, lead_0_obstacle, lead_1_obstacle): - if prev_source not in ("lead0", "lead1"): - return None - if not self.leads_share_identical_radar_track(lead_one, lead_two): - return None - if abs(float(lead_0_obstacle) - float(lead_1_obstacle)) > NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_KEEP_MARGIN: - return None - return prev_source - - def get_identical_radar_duplicate_cruise_hold(self, prev_source, lead_one, lead_two, - lead_0_obstacle, lead_1_obstacle, cruise_obstacle, - v_ego, t_follow): - if prev_source not in ("lead0", "lead1"): - return None - if float(v_ego) < IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MIN_SPEED: - return None - if not self.leads_share_identical_radar_track(lead_one, lead_two): - return None - if abs(float(lead_0_obstacle) - float(lead_1_obstacle)) > NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_KEEP_MARGIN: - return None - - prev_lead = lead_one if prev_source == "lead0" else lead_two - if prev_lead is None or not prev_lead.status: - return None - - actual_headway = float(prev_lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway > float(t_follow) + IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_HEADWAY_ABOVE_TARGET: - return None - - lead_brake = max(0.0, -float(getattr(prev_lead, "aLeadK", 0.0))) - if lead_brake > IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_LEAD_BRAKE: - return None - - lead_delta = float(prev_lead.vLead) - float(v_ego) - if lead_delta > IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_PULLAWAY_SPEED: - return None - - prev_lead_obstacle = float(lead_0_obstacle if prev_source == "lead0" else lead_1_obstacle) - cruise_advantage = prev_lead_obstacle - float(cruise_obstacle) - if cruise_advantage > IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE: - return None - - return prev_source - - def get_vision_follow_cruise_hold(self, prev_source, lead_one, lead_two, - lead_0_obstacle, lead_1_obstacle, cruise_obstacle, - v_ego, t_follow, tracking_lead): - if not tracking_lead or prev_source not in ("lead0", "lead1"): - return None - - prev_lead = lead_one if prev_source == "lead0" else lead_two - if prev_lead is None or not prev_lead.status or bool(getattr(prev_lead, "radar", False)): - return None - if float(getattr(prev_lead, "modelProb", 0.0)) < VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB: - return None - if self.get_stable_follow_cruise_hysteresis(prev_lead, v_ego, t_follow) <= 0.0: - return None - - prev_lead_obstacle = float(lead_0_obstacle if prev_source == "lead0" else lead_1_obstacle) - cruise_advantage = prev_lead_obstacle - float(cruise_obstacle) - if cruise_advantage > VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE: - return None - - return prev_source - - def get_identical_radar_duplicate_cruise_bias(self, lead_one, lead_two, v_ego, t_follow): - if float(v_ego) < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED: - return 0.0 - if not self.leads_share_identical_radar_track(lead_one, lead_two): - return 0.0 - - lead = lead_one if lead_one.status else lead_two - if lead is None or not lead.status: - return 0.0 - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(t_follow) - if headway_margin < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET: - return 0.0 - if headway_margin > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET: - return 0.0 - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_LEAD_BRAKE: - return 0.0 - - lead_delta = float(lead.vLead) - float(v_ego) - if lead_delta > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_PULLAWAY_SPEED: - return 0.0 - - return float(np.interp( - headway_margin, - [IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET, 0.0, IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET], - [IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX, IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX * 0.85, 0.0], - )) - - def set_accel_limits(self, min_a, max_a): - # TODO this sets a max accel limit, but the minimum limit is only for cruise decel - # needs refactor - self.cruise_min_a = min_a - self.max_a = max_a - - def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow, - personality=log.LongitudinalPersonality.standard, tracking_lead=True, - optional_far_lead_comfort=True, smooth_duplicate_vision=False): + def update(self, radarstate, v_cruise, x=None, v=None, a=None, j=None, + danger_factor=LEAD_DANGER_FACTOR, t_follow=None, + personality=log.LongitudinalPersonality.standard, **_): + t_follow = get_T_FOLLOW(personality=personality) if t_follow is None else float(t_follow) v_ego = self.x0[1] - lead_one = radarstate.leadOne - lead_two = radarstate.leadTwo - self.status = tracking_lead and (lead_one.status or lead_two.status) - lead_xv_0 = self.process_lead(lead_one, tracking_lead, t_follow=t_follow, lead_index=0, - smooth_duplicate_vision=smooth_duplicate_vision) - lead_xv_1 = self.process_lead(lead_two, tracking_lead, t_follow=t_follow, lead_index=1, - smooth_duplicate_vision=smooth_duplicate_vision) - # To estimate a safe distance from a moving lead, we calculate how much stopping - # distance that lead needs as a minimum. We can add that to the current distance - # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + lead_trajectories = [ + self.process_lead(radarstate.leadOne, 0), + self.process_lead(radarstate.leadTwo, 1), + ] + lead_obstacles = [ + trajectory[:, 0] + get_stopped_equivalence_factor(trajectory[:, 1]) + for trajectory in lead_trajectories + ] - self.params[:,0] = ACCEL_MIN - self.params[:,1] = max(0.0, self.max_a) + v_lower = v_ego + T_IDXS * self.cruise_min_a * 1.05 + v_upper = v_ego + T_IDXS * self.max_a * 1.05 + v_cruise_clipped = np.clip(np.full(N + 1, v_cruise), v_lower, v_upper) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) - # Update in ACC mode or ACC/e2e blend - if self.mode == 'acc': - self.params[:,5] = LEAD_DANGER_FACTOR + obstacles = np.column_stack((*lead_obstacles, cruise_obstacle)) + self.source = SOURCES[int(np.argmin(obstacles[0]))] - # Fake an obstacle for cruise, this ensures smooth acceleration to set speed - # when the leads are no factor. - v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) - # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * self.max_a * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), - v_lower, - v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) - prev_source = self.source - if optional_far_lead_comfort: - if prev_source == 'lead0': - cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_one, v_ego, t_follow) - elif prev_source == 'lead1': - cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_two, v_ego, t_follow) - if optional_far_lead_comfort and tracking_lead and lead_one.status: - desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow) - closing_speed = max(0.0, v_ego - lead_one.vLead) - cruise_obstacle += get_tracked_lead_catchup_bias( - v_ego, - lead_one.dRel, - desired_gap, - closing_speed, - v_cruise=v_cruise, - y_rel=float(getattr(lead_one, "yRel", 0.0)), - ) - if optional_far_lead_comfort: - cruise_obstacle += self.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) - if optional_far_lead_comfort: - lead_0_bias, lead_1_bias = self.get_near_duplicate_lead_source_hysteresis(prev_source, lead_one, lead_two, v_ego) - lead_0_obstacle = lead_0_obstacle + lead_0_bias - lead_1_obstacle = lead_1_obstacle + lead_1_bias - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) - candidate_source = SOURCES[np.argmin(x_obstacles[0])] - sticky_source = None - if optional_far_lead_comfort: - if candidate_source in ("lead0", "lead1"): - sticky_source = self.get_identical_radar_duplicate_source_hold( - prev_source, - lead_one, - lead_two, - lead_0_obstacle[0], - lead_1_obstacle[0], - ) - elif candidate_source == "cruise": - sticky_source = self.get_identical_radar_duplicate_cruise_hold( - prev_source, - lead_one, - lead_two, - lead_0_obstacle[0], - lead_1_obstacle[0], - cruise_obstacle[0], - v_ego, - t_follow, - ) - if sticky_source is None: - sticky_source = self.get_vision_follow_cruise_hold( - prev_source, - lead_one, - lead_two, - lead_0_obstacle[0], - lead_1_obstacle[0], - cruise_obstacle[0], - v_ego, - t_follow, - tracking_lead, - ) - self.source = sticky_source or candidate_source - - # These are not used in ACC mode - x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0 - - elif self.mode == 'blended': - self.params[:,5] = 1.0 - - x_obstacles = np.column_stack([lead_0_obstacle, - lead_1_obstacle]) - cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] - xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) - x = np.cumsum(np.insert(xforward, 0, x[0])) - - x_and_cruise = np.column_stack([x, cruise_target]) - x = np.min(x_and_cruise, axis=1) - - self.source = 'e2e' if x_and_cruise[1,0] < x_and_cruise[1,1] else 'cruise' - - else: - raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update') - - self.yref[:,1] = x - self.yref[:,2] = v - self.yref[:,3] = a - self.yref[:,5] = j + self.yref.fill(0.0) for i in range(N): self.solver.set(i, "yref", self.yref[i]) self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) - self.params[:,2] = np.min(x_obstacles, axis=1) - self.params[:,3] = np.copy(self.prev_a) - self.params[:,4] = t_follow + self.params[:, 0] = self.cruise_min_a + self.params[:, 1] = max(0.0, self.max_a) + self.params[:, 2] = np.min(obstacles, axis=1) + self.params[:, 3] = self.prev_a + self.params[:, 4] = t_follow + self.params[:, 5] = float(danger_factor) self.run() - if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and - should_trigger_planner_fcw(lead_one, v_ego)): - self.crash_cnt += 1 - else: - self.crash_cnt = 0 - # Check if it got within lead comfort range - # TODO This should be done cleaner - if self.mode == 'blended': - if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0): - self.source = 'lead0' - if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0) and \ - (lead_1_obstacle[0] - lead_0_obstacle[0]): - self.source = 'lead1' + crash_risk = False + for lead, trajectory in zip((radarstate.leadOne, radarstate.leadTwo), lead_trajectories, strict=True): + crash_risk |= bool( + should_trigger_planner_fcw(lead, v_ego) and + np.any(trajectory[FCW_IDXS, 0] - self.x_sol[FCW_IDXS, 0] < CRASH_DISTANCE) + ) + self.crash_cnt = self.crash_cnt + 1 if crash_risk else 0 def run(self): - # t0 = time.monotonic() - # reset = 0 - for i in range(N+1): - self.solver.set(i, 'p', self.params[i]) + for i in range(N + 1): + self.solver.set(i, "p", self.params[i]) self.solver.constraints_set(0, "lbx", self.x0) self.solver.constraints_set(0, "ubx", self.x0) self.solution_status = self.solver.solve() - self.solve_time = float(self.solver.get_stats('time_tot')[0]) - self.time_qp_solution = float(self.solver.get_stats('time_qp')[0]) - self.time_linearization = float(self.solver.get_stats('time_lin')[0]) - self.time_integrator = float(self.solver.get_stats('time_sim')[0]) + self.solve_time = float(self.solver.get_stats("time_tot")[0]) - # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific - # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, \ - # integrator {self.time_integrator:.2e}, qp_iter {qp_iter}") - # res = self.solver.get_residuals() - # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}") - # self.solver.print_statistics() - - for i in range(N+1): - self.x_sol[i] = self.solver.get(i, 'x') + for i in range(N + 1): + self.x_sol[i] = self.solver.get(i, "x") for i in range(N): - self.u_sol[i] = self.solver.get(i, 'u') - - self.v_solution = self.x_sol[:,1] - self.a_solution = self.x_sol[:,2] - self.j_solution = self.u_sol[:,0] + self.u_sol[i] = self.solver.get(i, "u") + self.v_solution = self.x_sol[:, 1] + self.a_solution = self.x_sol[:, 2] + self.j_solution = self.u_sol[:, 0] self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution) - t = time.monotonic() if self.solution_status != 0: - if t > self.last_cloudlog_t + 5.0: - self.last_cloudlog_t = t - cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}") + now = time.monotonic() + if now > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = now + cloudlog.warning(f"Long MPC reset, solution_status: {self.solution_status}") self.reset() - # reset = 1 - # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \ - # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}") if __name__ == "__main__": ocp = gen_long_ocp() AcadosOcpSolver.generate(ocp, json_file=JSON_FILE) - # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py old mode 100755 new mode 100644 index 02a4a115e..0709403e8 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,3881 +1,498 @@ #!/usr/bin/env python3 +from enum import IntEnum import math + import numpy as np -import time import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX + from openpilot.common.constants import CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.starpilot.common.model_versions import is_tinygrad_model_version -from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import should_trigger_planner_fcw -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window -from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_far_follow_output_slew_rates, get_untracked_slow_lead_decel_scale -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( + STOP_DISTANCE, + T_IDXS as T_IDXS_MPC, + LongitudinalMpc, + should_trigger_planner_fcw, +) +from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_longitudinal_planner_tune +from openpilot.selfdrive.modeld.constants import ModelConstants + -LON_MPC_STEP = 0.2 # first step is 0.2s -A_CRUISE_MIN = -1.0 -A_CRUISE_MAX_BP = [0.0, 5., 10., 15., 20., 25., 40.] -A_CRUISE_MAX_VALS = [1.125, 1.125, 1.125, 1.125, 1.25, 1.25, 1.5] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] -ALLOW_THROTTLE_THRESHOLD = 0.4 +MIN_PLAN_HORIZON = 0.30 +A_CRUISE_MAX_BP = [0.0, 5.0, 10.0, 15.0, 20.0, 25.0, 40.0] +A_CRUISE_MAX_VALS = [1.125, 1.125, 1.125, 1.125, 1.25, 1.25, 1.5] +A_CRUISE_MIN = -1.0 + +ALLOW_THROTTLE_THRESHOLD = 0.40 ALLOW_THROTTLE_HYSTERESIS = 0.05 -ALLOW_THROTTLE_ENABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD + ALLOW_THROTTLE_HYSTERESIS -ALLOW_THROTTLE_DISABLE_THRESHOLD = ALLOW_THROTTLE_THRESHOLD - ALLOW_THROTTLE_HYSTERESIS -ALLOW_THROTTLE_TRANSITION_CONFIRM_TIME = 0.25 -MIN_ALLOW_THROTTLE_SPEED = 5.0 -MODEL_LAUNCH_DISARM_SPEED = 2.0 -MODEL_LAUNCH_COMMIT_TIME = 3.5 -MODEL_LAUNCH_MOVING_SPEED = 1.2 -MODEL_LAUNCH_MAX_ACCEL = 1.5 -RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5 -RAW_LEAD_SAFETY_TTC = 7.0 -RAW_LEAD_SAFETY_DISTANCE = 40.0 -RAW_LEAD_LOW_SPEED_HOLD_MAX_EGO_SPEED = 4.5 -RAW_LEAD_LOW_SPEED_HOLD_MAX_LEAD_SPEED = 3.5 -RAW_LEAD_LOW_SPEED_HOLD_MAX_DISTANCE = 10.0 -RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET = 1.75 -RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED = 0.15 -STANDSTILL_LEAD_NUDGE_ACCEL = 0.05 -STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0 -STANDSTILL_LEAD_NUDGE_MIN_LEAD_ACCEL = 0.2 -STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.35 -STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5 -STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6 -STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 0.8 -STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL = 0.18 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08 -STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1 -STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME = 0.30 -RADAR_STANDSTILL_GAP_SETTLE_ACCEL = 0.18 -RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME = 0.50 -RADAR_STANDSTILL_GAP_SETTLE_ENTRY_MARGIN = 0.60 -RADAR_STANDSTILL_GAP_SETTLE_EXIT_MARGIN = 0.15 -RADAR_STANDSTILL_GAP_SETTLE_MAX_EXTRA_GAP = 1.5 -RADAR_STANDSTILL_GAP_SETTLE_MAX_EGO_SPEED = 0.45 -RADAR_STANDSTILL_GAP_SETTLE_MAX_LEAD_SPEED = 0.15 -RADAR_STANDSTILL_GAP_SETTLE_MAX_LATERAL_OFFSET = 1.0 -LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75 -LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 -LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 -LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 -LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 -LEAD_DEPART_CONFIDENT_CONFIRM_TIME = 0.35 -LEAD_DEPART_RELEASE_HOLD_TIME = 1.5 -LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME = 0.15 -LEAD_DEPART_RELEASE_HOLD_MIN_DISTANCE = 3.0 -LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_SPEED = 0.55 -LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_DELTA = 0.25 -LEAD_DEPART_RELEASE_HOLD_MAX_LEAD_BRAKE = 0.15 -LEAD_DEPART_RELEASE_HOLD_MIN_MODEL_PROB = 0.95 -LEAD_DEPART_RELEASE_HOLD_MAX_LATERAL_OFFSET = 1.0 -LEAD_DEPART_RELEASE_HOLD_CONFLICT_SPEED = 0.25 -LEAD_DEPART_RELEASE_HOLD_CONFLICT_DISTANCE_MARGIN = 3.0 -VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED = 10.0 -VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE = 25.0 -VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME = 1.35 -VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY = 1.35 -VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC = 8.0 -VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET = 1.5 -STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED = 0.5 -STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED = 0.45 -STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA = 0.35 -STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB = 0.95 -STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET = 1.75 -STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE = 3.0 -STANDSTILL_STOPPED_LEAD_GUARD_DISTANCE_MARGIN = 3.0 -STANDSTILL_STOPPED_LEAD_GUARD_MIN_BRAKE = 0.16 -STANDSTILL_STOPPED_LEAD_GUARD_MAX_BRAKE = 0.26 -RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6 -RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5 -RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0 -RADAR_DEPART_CONFLICT_MIN_MODEL_PROB = 0.95 -RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE = 18.0 -RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL = 0.9 -RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED = 2.0 -RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH = 4.0 -LEAD_DEPART_ACCEL_HOLD_TIME = 1.2 -LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 2.0 -LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6 -LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_DELTA = 0.5 -LEAD_DEPART_ACCEL_HOLD_MIN_GAP = 3.5 -LEAD_DEPART_ACCEL_HOLD_FULL_GAP = 6.0 -LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED = 2.2 -LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB = 0.85 -LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL = 0.12 -LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE = 0.2 -LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL = 0.25 -LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.55 -LEAD_DEPART_ACCEL_ASSIST = 0.10 -LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_GAP = 3.75 -LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_CLOSING_SPEED = 0.45 -LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_LEAD_BRAKE = 0.2 -LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_HEADWAY_MARGIN = 0.10 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED = 4.5 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE = 4.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE = 18.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED = 4.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.75 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.9 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA = -0.5 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA = 0.75 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL = -0.4 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL = 0.25 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL = 0.08 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL = 0.22 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED = 1.25 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP = 4.0 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED = 1.2 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA = 0.8 -LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL = 0.5 -CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 -INSIDE_GAP_CLOSING_MIN_EGO_SPEED = 8.0 -INSIDE_GAP_CLOSING_MIN_LEAD_SPEED = 5.0 -INSIDE_GAP_CLOSING_MIN_SPEED = 0.5 -INSIDE_GAP_CLOSING_FULL_SPEED = 2.5 -INSIDE_GAP_CLOSING_MIN_DEFICIT = 3.0 -INSIDE_GAP_CLOSING_DEFICIT_RATIO = 0.15 -INSIDE_GAP_CLOSING_BRAKE_DEFICIT_RATIO = 0.25 -INSIDE_GAP_CLOSING_BRAKE_MIN_SPEED = 1.0 -INSIDE_GAP_CLOSING_MAX_DECEL = 0.65 -INSIDE_GAP_CLOSING_MAX_LATERAL_OFFSET = 1.75 -INSIDE_GAP_CLOSING_VISION_MIN_MODEL_PROB = 0.95 -VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0 -VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5 -VISION_LEAD_APPROACH_FULL_TIME = 1.0 -VISION_LEAD_APPROACH_TIGHT_BUFFER = 2.0 -VISION_LEAD_APPROACH_MAX_DECEL = 0.80 -VISION_LEAD_APPROACH_MIN_DECEL = 0.15 -VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85 -VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 -VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.30 -VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0 -VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20 -VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN = 0.75 -VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45 -VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20 -VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30 -VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75 -VISION_LEAD_APPROACH_CONFIRM_TIME = 0.25 -VISION_LEAD_APPROACH_CONFIRM_BYPASS_DECEL = 1.0 -VISION_LEAD_APPROACH_CONFIRM_BYPASS_CLOSING_SPEED = 4.0 -VISION_LEAD_APPROACH_CONFIRM_BYPASS_LEAD_BRAKE = 0.20 -VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_MIN = 28.0 -VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_TIME = 0.85 -VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB = 0.9 -VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB = 0.97 -VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED = 3.0 -VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO = 0.16 -VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO = 0.24 -VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC = 16.0 -VISION_UNTRACKED_SLOW_LEAD_FULL_TTC = 8.0 -VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_MODEL_PROB = 0.95 -VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_SPEED = 2.0 -VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_RATIO = 0.12 -VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC = 24.0 -VISION_UNTRACKED_SLOW_LEAD_NEAR_MAX_DISTANCE = 45.0 -VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_MODEL_PROB = 0.90 -VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_CLOSING_RATIO = 0.09 -VISION_UNTRACKED_SLOW_LEAD_RELAXED_ENTRY_MAX_LATERAL_OFFSET = 1.2 -VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.4 -VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE = 80.0 -VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE = 120.0 -VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_DISTANCE_TIME = 5.7 -VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL = 0.85 -VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL = 0.1 -VISION_UNTRACKED_SLOW_LEAD_RELAXED_MODEL_PROB = 0.68 -VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED = 8.0 -VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC = 10.0 -VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED = 10.0 -VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED = 16.0 -VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME = 0.30 -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL = 0.55 -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DISTANCE = 45.0 -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_LEAD_BRAKE = 0.10 -VISION_UNTRACKED_APPROACH_LIFT_MIN_EGO_SPEED = 18.0 -VISION_UNTRACKED_APPROACH_LIFT_MIN_MODEL_PROB = 0.95 -VISION_UNTRACKED_APPROACH_LIFT_MAX_LATERAL_OFFSET = 1.2 -VISION_UNTRACKED_APPROACH_LIFT_MIN_CLOSING_SPEED = 0.75 -VISION_UNTRACKED_APPROACH_LIFT_MAX_DISTANCE = 130.0 -VISION_UNTRACKED_APPROACH_LIFT_MIN_GAP_EXCESS = 6.0 -VISION_UNTRACKED_APPROACH_LIFT_TRIGGER_TIME = 20.0 -VISION_UNTRACKED_APPROACH_LIFT_FULL_TIME = 6.0 -VISION_UNTRACKED_APPROACH_LIFT_MAX_ACCEL = 0.22 -VISION_UNTRACKED_APPROACH_LIFT_CONFIRM_TIME = 0.30 -VISION_UNTRACKED_APPROACH_LIFT_HOLD_TIME = 0.75 -VISION_UNTRACKED_APPROACH_LIFT_RATE_DOWN = 0.35 -VISION_UNTRACKED_APPROACH_LIFT_RATE_UP = 0.25 -VISION_SLOW_LEAD_MAX_SPEED = 5.0 -VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5 -VISION_SLOW_LEAD_TRIGGER_TTC = 4.5 -VISION_SLOW_LEAD_FULL_TTC = 2.0 -VISION_SLOW_LEAD_MAX_DECEL = 1.2 -VISION_SLOW_LEAD_MIN_DECEL = 0.18 -VISION_SLOW_LEAD_MIN_MODEL_PROB = 0.9 -LEAD_APPROACH_TFOLLOW_TRIGGER_TIME = 4.5 -LEAD_APPROACH_TFOLLOW_FULL_TIME = 1.5 -LEAD_APPROACH_TFOLLOW_MAX_DELTA = 0.18 -LEAD_APPROACH_TFOLLOW_MAX_CLOSING_SPEED = 6.0 -LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE = 2.5 -LEAD_APPROACH_TFOLLOW_MIN_CLOSING_SPEED = 0.75 -LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE = 0.2 -LEAD_APPROACH_TFOLLOW_WINDOW_MIN = 6.0 -LEAD_APPROACH_TFOLLOW_WINDOW_GAIN = 0.35 -LEAD_APPROACH_TFOLLOW_RATE_UP = 1.0 -LEAD_APPROACH_TFOLLOW_RATE_DOWN = 0.60 -VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA = 0.24 -VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED = 20.0 -VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN = 8.0 -VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN = 0.35 -VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 6.5 -VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 3.25 -VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED = 4.0 -VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9 -VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35 -VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED = -0.2 -VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.8 -VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80 -VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25 -VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9 -VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8 -VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.25 -VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25 -VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED = 0.75 -VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED = 0.8 -VISION_CLOSE_STOP_HOLD_MAX_DISTANCE = 3.5 -VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB = 0.95 -VISION_CLOSE_STOP_HOLD_MIN_BRAKE = 0.20 -VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.36 -VISION_CLOSE_SETTLE_MAX_EGO_SPEED = 0.75 -VISION_CLOSE_SETTLE_MAX_LEAD_SPEED = 2.75 -VISION_CLOSE_SETTLE_MAX_DISTANCE = 4.2 -VISION_CLOSE_SETTLE_MAX_LEAD_DELTA = 2.6 -VISION_CLOSE_SETTLE_MIN_BRAKE = 0.16 -VISION_CLOSE_SETTLE_MAX_BRAKE = 0.30 -VISION_CLOSE_FINAL_GUARD_MAX_EGO_SPEED = 0.5 -VISION_CLOSE_FINAL_GUARD_MAX_LEAD_SPEED = 2.75 -VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE = 4.5 -VISION_CLOSE_FINAL_GUARD_MIN_BRAKE = 0.18 -VISION_CLOSE_FINAL_GUARD_MAX_BRAKE = 0.28 -VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED = 2.5 -VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED = 3.5 -VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE = 4.2 -VISION_CLOSE_RELEASE_HOLD_MIN_MODEL_PROB = 0.98 -VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA = -0.1 -VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA = 1.5 -VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE = 0.18 -VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE = 0.40 -MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0 -MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0 -MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL = 0.2 +ALLOW_THROTTLE_CONFIRM_TIME = 0.25 +MIN_ALLOW_THROTTLE_SPEED = 1.5 +COAST_TARGET_SPEED = 5.0 +COAST_TARGET_GAIN = 0.35 + +MODEL_STOP_BEGIN_TIME = 9.0 +MODEL_STOP_FULL_TIME = 4.0 +MODEL_STOP_MIN_END_SPEED = 2.0 +MODEL_FIRST_AUTHORITY = 0.75 +MODEL_FIRST_OPEN_ROAD_AUTHORITY = 0.35 +MODEL_FIRST_CRUISE_ERROR = 1.5 + +STOP_ENTER_AUTHORITY = 0.72 +STOP_RELEASE_TIME = 0.50 +STOPPED_SPEED = 0.15 +STOP_CONTROL_SPEED = 1.5 +DEPART_COMPLETE_SPEED = 2.5 +DEPART_LEAD_MIN_SPEED = 0.10 +DEPART_LEAD_FAST_SPEED = 0.75 +DEPART_CONFIRM_TIME = 0.20 +DEPART_LEAD_MIN_GAP = 0.0 +DEPART_MODEL_MIN_ACCEL = 0.08 +DEPART_LEAD_MAX_BRAKE = 0.30 + +RAW_SAFETY_REACTION_BUFFER = 0.20 +RAW_SAFETY_MIN_CLOSING_SPEED = 0.25 +RAW_SAFETY_MIN_DECEL = 0.20 +RAW_SAFETY_URGENT_DECEL = 0.80 +RAW_SAFETY_URGENT_TTC = 4.0 +RAW_SAFETY_EXTRA_STOP_BUFFER = 1.0 + FORCE_STOP_HANDOFF_MAX_VCRUISE = 0.5 -LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 -LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 -LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0 -LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15 -RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN = 0.75 -RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP = 0.04 -RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR = 0.75 -RADAR_CATCHUP_ACCEL_CAP_ENTRY_FULL_SPEED = 12.0 -RADAR_CATCHUP_ACCEL_CAP_ENTRY_MAX = 1.5 -POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME = 75.0 -POST_DEPARTURE_FOLLOW_SETTLE_MIN_SPEED = 8.0 -POST_DEPARTURE_FOLLOW_SETTLE_MAX_ARM_SPEED = 16.0 -POST_DEPARTURE_FOLLOW_SETTLE_MIN_MODEL_PROB = 0.9 -POST_DEPARTURE_FOLLOW_SETTLE_MAX_LATERAL_OFFSET = 1.15 -POST_DEPARTURE_FOLLOW_SETTLE_MAX_CLOSING_SPEED = 0.8 -POST_DEPARTURE_FOLLOW_SETTLE_MAX_LEAD_BRAKE = 0.10 -POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_LEAD_DELTA = -0.10 -POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_LEAD_ACCEL = 0.20 -POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_HEADWAY_MARGIN = 0.08 -POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN = 0.05 -FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED = 12.0 -FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN = 0.10 -FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_HEADWAY_MARGIN = 0.90 -FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN = 4.0 -FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_CLOSING_SPEED = 0.20 -FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED = 1.50 -FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE = 0.35 -FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_ACCEL = 0.55 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED = 12.0 -LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB = 0.85 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE = 0.20 -LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_LEAD_DELTA = -1.2 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_MIN = 6.0 -LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.35 -LOW_SPEED_FOLLOW_TRANSITION_MIN_SPEED = 3.0 -LOW_SPEED_FOLLOW_TRANSITION_MAX_SPEED = 12.0 -LOW_SPEED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.85 -LOW_SPEED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.20 -LOW_SPEED_FOLLOW_TRANSITION_MIN_GAP_MARGIN = 1.0 -LOW_SPEED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 0.6 -LOW_SPEED_FOLLOW_TRANSITION_PREV_ACCEL_MIN = 0.18 -LOW_SPEED_FOLLOW_TRANSITION_TARGET_BRAKE_MIN = -0.18 -LOW_SPEED_FOLLOW_TRANSITION_MAX_BRAKE = 0.14 -LOW_SPEED_FOLLOW_TRANSITION_MIN_BRAKE = 0.08 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED = 10.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED = 20.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.85 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE = 0.25 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED = 1.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN = 12.0 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.9 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.15 -CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED = 1.5 -CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA = 0.25 -CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL = 0.18 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_SPEED = 12.0 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_SPEED = 22.0 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_MODEL_PROB = 0.9 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LEAD_BRAKE = 0.35 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LATERAL_OFFSET = 1.15 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED = 2.25 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_HEADWAY_ABOVE_TARGET = 0.95 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_DELTA_A = 0.18 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_STEP = 0.06 -CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_STEP = 0.18 -# Uncertainty-based filter disable thresholds -UNCERT_SLOPE_TRIG = 0.12 # per second -UNCERT_MAG_TRIG = 0.50 -UNCERT_PANIC_MIN_CLOSING_SPEED = 2.0 -UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN = 0.08 -UNCERT_PANIC_MAX_GAP_BUFFER_MIN = 8.0 -UNCERT_PANIC_MAX_GAP_BUFFER_GAIN = 0.35 -UNCERT_DUPLICATE_VISION_MIN_TTC = 6.0 -UNCERT_DUPLICATE_VISION_MIN_HEADWAY = 0.85 -UNCERT_DUPLICATE_VISION_HEADWAY_BELOW_TARGET = 0.45 -STEADY_FOLLOW_SMOOTHING_MIN_SPEED = 22.0 -STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED = 0.15 -STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED = 1.8 -STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY = 0.95 -STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET = 0.35 -STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET = 0.90 -STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE = 0.35 -STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB = 0.7 -STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR = 0.24 -STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY = 1.05 -STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET = 0.90 -STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED = -1.2 -STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED = 2.2 -STEADY_FOLLOW_BRAKE_CAP_ZERO_REL_SPEED_DECEL = 0.12 -STEADY_FOLLOW_BRAKE_CAP_OPENING_DECEL = 0.08 -STEADY_FOLLOW_BRAKE_CAP_MIN_DECEL = 0.18 -STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL = 0.32 -STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_HEADWAY_MARGIN = 0.45 -STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MIN_HEADWAY = 1.80 -STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MAX_LEAD_BRAKE = 0.15 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DISTANCE = 80.0 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED = 0.1 -FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED = 2.0 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_MODEL_PROB = 0.95 -FAR_LEAD_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE = 0.12 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_TTC = 7.5 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN = 0.55 -FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 1.00 -FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL = 0.05 -FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL = 0.18 -FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DISTANCE = 40.0 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED = 0.5 -FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED = 1.8 -FAR_RADAR_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE = 0.12 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_TTC = 20.0 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN = 0.55 -FAR_RADAR_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 0.95 -FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DECEL = 0.04 -FAR_RADAR_COMFORT_BRAKE_CAP_MAX_DECEL = 0.14 -FAR_RADAR_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 -EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME = 0.75 -# At low speed, preserve the existing handoff slew long enough to bridge a -# brief slow-lead CEM dropout without extending high-speed release behavior. -EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_HOLD_TIME = 3.0 -EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_THRESHOLD = 12.0 -EXPERIMENTAL_RELEASE_ACCEL_MIN_SPEED = 2.5 -EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_SPEED = 5.0 -EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_DELTA = -1.0 -EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_DELTA = 1.5 -EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_BRAKE = 0.2 -EXPERIMENTAL_RELEASE_ACCEL_MIN_MODEL_PROB = 0.9 -EXPERIMENTAL_RELEASE_ACCEL_MAX_LATERAL_OFFSET = 1.5 -EXPERIMENTAL_RELEASE_ACCEL_MIN_HEADWAY_MARGIN = 0.0 -EXPERIMENTAL_RELEASE_ACCEL_MIN_DELTA_A = 0.12 -EXPERIMENTAL_RELEASE_ACCEL_STEP = 0.06 -MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 20.0 -MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.25 -MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 0.75 -MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.9 -MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.18 -MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 1.75 -MATCHED_FOLLOW_TRANSITION_MIN_TTC = 12.0 -MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP = 0.08 -MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.18 -MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.08 -MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.16 -MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.10 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 10.0 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED = MATCHED_FOLLOW_TRANSITION_MIN_SPEED -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.45 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN = 0.20 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_LEAD_DELTA = 0.25 -MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_CLOSING_SPEED = 0.25 -MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP = 0.05 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 1.00 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.98 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.08 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 1.25 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC = 18.0 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP = 0.06 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.10 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.05 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.08 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.06 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET = -0.12 -LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A = 0.12 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_SPEED = 3.0 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_SPEED = 35.0 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_MODEL_PROB = 0.95 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_LEAD_BRAKE = 0.80 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_CLOSING_SPEED = 3.25 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_TTC = 6.0 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_HEADWAY_MARGIN = 0.25 -MILD_FOLLOW_ZERO_CROSS_GUARD_FULL_HEADWAY_MARGIN = 0.85 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DELTA_A = 0.08 -MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DEADBAND = 0.04 -MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_DEADBAND = 0.08 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_SPEED = 20.0 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_DISTANCE = 80.0 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_PROB = 0.85 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_LEAD_DELTA = 1.0 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_HEADWAY_MARGIN = 0.75 -FAR_OPENING_RADAR_BRAKE_GUARD_MAX_LEAD_BRAKE = 0.25 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_PREV_TARGET = -0.08 -FAR_OPENING_RADAR_BRAKE_GUARD_MAX_NEW_TARGET = -0.15 -FAR_OPENING_RADAR_BRAKE_GUARD_MAX_CRUISE_DEFICIT = 0.25 -FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_ACCEL = -0.50 -NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED = 20.0 -NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB = 0.95 -NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE = 0.35 -NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED = 3.5 -NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC = 8.0 -NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET = 0.45 -NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET = 0.85 -NEAR_DUPLICATE_VISION_TRANSITION_MIN_HEADWAY_MARGIN = 0.55 -NEAR_DUPLICATE_VISION_TRANSITION_EXTRA_CLOSING_SPEED = 1.25 -LOW_SPEED_IDENTICAL_RADAR_DUPLICATE_TRANSITION_EXTRA_HEADWAY = 0.15 -LOW_SPEED_DUPLICATE_VISION_TRANSITION_EXTRA_HEADWAY = 0.75 -NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35 -NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22 -NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP = 0.32 -NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP = 0.18 -DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED = 12.0 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB = 0.95 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED = 0.5 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_LEAD_BRAKE = 0.8 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_HEADWAY_ABOVE_TARGET = 0.85 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_DELTA_A = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_DREL_DIFF = 1.5 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_VREL_DIFF = 0.35 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_POSITIVE_STEP = 0.12 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_POSITIVE_STEP = 0.28 -DUPLICATE_SLOW_LEAD_BRAKE_HOLD_SIGN_CROSS_STEP = 0.22 -TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0 -TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95 -TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL = 0.80 -TRACKED_VISION_MODEL_FLOOR_MIN_CLOSING_SPEED = 1.0 -TRACKED_VISION_MODEL_FLOOR_MAX_TTC = 22.0 -TRACKED_VISION_MODEL_FLOOR_MIN_GAP_MARGIN = -2.0 -TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_MIN = 4.0 -TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_GAIN = 0.25 -TRACKED_VISION_MODEL_FLOOR_MIN_DECEL = 0.35 -TRACKED_VISION_MODEL_FLOOR_MAX_DECEL = 1.10 -TRACKED_VISION_MODEL_FLOOR_LEAD_BRAKE_MAX = 0.18 -TRACKED_VISION_MODEL_CAP_MIN_SPEED = 14.0 -TRACKED_VISION_MODEL_CAP_MAX_SPEED = 32.0 -TRACKED_VISION_MODEL_CAP_MIN_MODEL_PROB = 0.95 -TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL = 0.75 -TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED = 0.75 -TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED = 4.0 -TRACKED_VISION_MODEL_CAP_MAX_LEAD_BRAKE = 0.55 -TRACKED_VISION_MODEL_CAP_MIN_TTC = 7.0 -TRACKED_VISION_MODEL_CAP_MIN_GAP_MARGIN = -1.5 -TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_MIN = 4.0 -TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_GAIN = 0.25 -TRACKED_VISION_MODEL_CAP_MIN_DECEL = 0.45 -TRACKED_VISION_MODEL_CAP_MAX_DECEL = 1.10 - -# Lookup table for turns _A_TOTAL_MAX_V = [3.5, 3.5, 3.2] -_A_TOTAL_MAX_BP = [0., 20., 40.] - -_preap_follow_cache = None +_A_TOTAL_MAX_BP = [0.0, 20.0, 40.0] -def get_preap_follow_limit(v_ego): - global _preap_follow_cache - if _preap_follow_cache is None: - try: - from opendbc.car.tesla.preap.constants import ACCEL_PREAP_BP, ACCEL_PREAP_FOLLOW - _preap_follow_cache = (ACCEL_PREAP_BP, ACCEL_PREAP_FOLLOW) - except ImportError: - _preap_follow_cache = (None, None) - bp, values = _preap_follow_cache - if bp is None: - return None - return float(np.interp(v_ego, bp, values)) +class PlanState(IntEnum): + moving = 0 + stopping = 1 + stopped = 2 + departing = 3 -def get_longitudinal_personality(sm): - return sm['selfdriveState'].personality +def get_coast_accel(pitch): + return np.sin(pitch) * -5.65 - 0.3 def get_max_accel(v_ego): - return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) - -def get_coast_accel(pitch): - return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py + return float(np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)) -def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): - """ - This function returns a limited long acceleration allowed, depending on the existing lateral acceleration - this should avoid accelerating when losing the target in turns - """ - # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel - # The lookup table for turns should also be updated if we do this +def limit_accel_in_turns(v_ego, angle_steers, accel_limits, CP): a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) - - return [a_target[0], min(a_target[1], a_x_allowed)] - - -def should_publish_planner_fcw(crash_cnt: int, car_state, radar_state) -> bool: - return ( - crash_cnt > 2 and - not car_state.standstill and - should_trigger_planner_fcw(radar_state.leadOne, float(car_state.vEgo)) - ) + a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.0)) + return [accel_limits[0], min(accel_limits[1], a_x_allowed)] def get_vehicle_min_accel(CP, v_ego): - # Planner-side physical decel capability estimate for GM pedal-long paths. is_gm = getattr(CP, "carName", "") == "gm" or getattr(CP, "brand", "") == "gm" if is_gm and getattr(CP, "enableGasInterceptorDEPRECATED", False): try: - from opendbc.car.gm.values import GMFlags, CAR + from opendbc.car.gm.values import CAR, GMFlags if bool(CP.flags & GMFlags.PEDAL_LONG.value): - bolt_pedal_long_cars = { + bolt_pedal_long = { CAR.CHEVROLET_BOLT_CC_2017, CAR.CHEVROLET_BOLT_CC_2018_2021, CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, CAR.CHEVROLET_BOLT_CC_2022_2023, CAR.CHEVROLET_MALIBU_HYBRID_CC, } - if CP.carFingerprint in bolt_pedal_long_cars: - return float(np.interp(v_ego, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0], - [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95])) - return float(np.interp(v_ego, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0], - [-0.95, -1.3, -1.85, -2.3, -2.6, -2.8])) + values = [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95] if CP.carFingerprint in bolt_pedal_long else \ + [-0.95, -1.30, -1.85, -2.30, -2.60, -2.80] + return float(np.interp(v_ego, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0], values)) except Exception: pass return float(ACCEL_MIN) def get_planner_v_ego(CP, car_state): - v_ego = max(car_state.vEgo, car_state.vEgoCluster) - is_gm = getattr(CP, "carName", "") == "gm" or getattr(CP, "brand", "") == "gm" if is_gm and getattr(CP, "enableGasInterceptorDEPRECATED", False): try: from opendbc.car.gm.values import GMFlags - is_gm_pedal_long = bool(CP.flags & GMFlags.PEDAL_LONG.value) - if is_gm_pedal_long: + if bool(CP.flags & GMFlags.PEDAL_LONG.value): return float(car_state.vEgo) except Exception: pass - - return float(v_ego) + return float(max(car_state.vEgo, car_state.vEgoCluster)) -def get_accel_from_plan_classic(CP, speeds, accels, vEgoStopping): - if len(speeds) == CONTROL_N: - v_target_now = np.interp(DT_MDL, CONTROL_N_T_IDX, speeds) - a_target_now = np.interp(DT_MDL, CONTROL_N_T_IDX, accels) +def get_accel_from_plan(speeds, accels, action_t=DT_MDL, v_ego_stopping=0.05): + if len(speeds) != CONTROL_N: + return 0.0, False - v_target = np.interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds) - if v_target != v_target_now: - a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now - else: - a_target = a_target_now - - v_target_1sec = np.interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds) - else: - v_target = 0.0 - v_target_1sec = 0.0 - a_target = 0.0 - should_stop = (v_target < vEgoStopping and - v_target_1sec < vEgoStopping) + v_now = float(speeds[0]) + a_now = float(accels[0]) + action_t = max(float(action_t), DT_MDL) + v_target = float(np.interp(action_t, CONTROL_N_T_IDX, speeds)) + a_target = 2.0 * (v_target - v_now) / action_t - a_now + v_target_1sec = float(np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)) + should_stop = v_target < v_ego_stopping and v_target_1sec < v_ego_stopping return a_target, should_stop -def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): - if len(speeds) == CONTROL_N: - v_now = speeds[0] - a_now = accels[0] - - v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds) - a_target = 2 * (v_target - v_now) / (action_t) - a_now - v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds) - else: - v_target = 0.0 - v_target_1sec = 0.0 - a_target = 0.0 - should_stop = (v_target < vEgoStopping and - v_target_1sec < vEgoStopping) - return a_target, should_stop +def _smoothstep(value, low, high): + if high <= low: + return float(value >= high) + x = float(np.clip((value - low) / (high - low), 0.0, 1.0)) + return x * x * (3.0 - 2.0 * x) class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP - self.mpc = LongitudinalMpc(dt=dt) - self.fcw = False self.dt = dt - self.model_allow_throttle = True - self.model_allow_throttle_transition_t = 0.0 - self.allow_throttle = True - self.mode = 'acc' - self.is_preap = ( - CP.brand == "tesla" and CP.carFingerprint == "TESLA_MODEL_S_PREAP" and - CP.openpilotLongitudinalControl and not CP.pcmCruise - ) - self.nap_adaptive_accel = False - self._preap_params = None - self._preap_param_frame = 0 - - self.generation = None + self.mpc = LongitudinalMpc(dt=dt) + self.tune = get_longitudinal_planner_tune(CP) + self.mpc.lead_filter_tau = self.tune.lead_filter_tau + self.v_desired_filter = FirstOrderFilter(init_v, 2.0, dt) self.a_desired = init_a - self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) - self.v_model_error = 0.0 - self.output_a_target = 0.0 + self.output_a_target = init_a self.output_should_stop = False - self.far_follow_brake_slew_rate, self.far_follow_release_slew_rate = get_far_follow_output_slew_rates(CP) - self.untracked_slow_lead_decel_scale = get_untracked_slow_lead_decel_scale(CP) - self.far_follow_output_slew_active = False - self.model_launch_armed = False - self.model_launch_stop_seen = False - self.confident_lead_depart_elapsed = 0.0 - self.slow_creep_lead_depart_elapsed = 0.0 - self.lead_depart_release_candidate_elapsed = 0.0 - self.lead_depart_release_pending = False - self.lead_depart_release_hold_remaining = 0.0 - self.radar_standstill_gap_settle_elapsed = 0.0 - self.radar_standstill_gap_settle_active = False + self.allow_throttle = True + self._throttle_transition_time = 0.0 + + self.state = PlanState.moving + self.stop_release_time = 0.0 + self.departure_confirm_time = 0.0 + self.model_authority = 0.0 + self.plan_source = "cruise" + self.fcw = False self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) - self.solverExecutionTime = 0.0 - - # ---- Rubberband mitigation state ---- - # Two uncertainty tracks (slow/fast) for asymmetric gating - self.uncert_slow = FirstOrderFilter(0.0, 1.6, self.dt) # ~lam=0.6 - self.uncert_fast = FirstOrderFilter(0.0, 0.9, self.dt) # faster cool-down for accel decisions - # Lead stability tracking - self.prev_lead_dist = None - self.last_big_brake_t = 0.0 - self.stable_lead = False - # Smoothed lead distance - self.lead_dist_f = None - - # Uncertainty slope tracking - self._uncert_last = 0.0 - self._uncert_last_t = None - self._panic_bypass_log_t = 0.0 - self.effective_t_follow = None - self.vision_low_speed_stop_hold_until = 0.0 - self.vision_lead_approach_confirm_t = 0.0 - self.untracked_slow_lead_confirm_t = 0.0 - self.untracked_vision_approach_lift_confirm_t = 0.0 - self.untracked_vision_approach_lift_cap = None - self.untracked_vision_approach_lift_target = None - self.untracked_vision_approach_lift_hold_until = 0.0 - self.manual_stop_resume_override_until = 0.0 - self.lead_depart_accel_hold_until = 0.0 - self.lead_depart_accel_hold_floor = None - self.post_departure_follow_settle_until = 0.0 - self.duplicate_vision_comfort_lead_source = None - self.prev_experimental_mode = None - self.experimental_release_accel_until = 0.0 - - if self.is_preap: - try: - from openpilot.common.params import Params - self._preap_params = Params() - self.nap_adaptive_accel = self._preap_params.get_bool("NAPAdaptiveAccel") - except Exception: - self._preap_params = None - self.nap_adaptive_accel = False - - @property - def mlsim(self): - return is_tinygrad_model_version(self.generation) - - def get_mpc_mode(self) -> str: - if not self.mlsim: - return self.mode - return getattr(self.mpc, 'mode', 'acc') @staticmethod - def get_model_speed_error(model_msg, v_ego): - try: - temporal_pose = model_msg.temporalPoseDEPRECATED - except AttributeError: - try: - temporal_pose = model_msg.temporalPose - except AttributeError: - return 0.0 - if len(temporal_pose.trans): - return float(np.clip(temporal_pose.trans[0] - v_ego, -5.0, 5.0)) - return 0.0 - - @staticmethod - def parse_model(model_msg, model_error, v_ego, starpilot_toggles): - if (len(model_msg.position.x) == ModelConstants.IDX_N and - len(model_msg.velocity.x) == ModelConstants.IDX_N and - len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error - a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) - j = np.zeros(len(T_IDXS_MPC)) - else: - x = np.zeros(len(T_IDXS_MPC)) - v = np.zeros(len(T_IDXS_MPC)) - a = np.zeros(len(T_IDXS_MPC)) - j = np.zeros(len(T_IDXS_MPC)) - - if starpilot_toggles.taco_tune: - max_lat_accel = np.interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0]) - curvatures = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.orientationRate.z) / np.clip(v, 0.3, 100.0) - max_v = np.sqrt(max_lat_accel / (np.abs(curvatures) + 1e-3)) - 2.0 - v = np.minimum(max_v, v) - - if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1: - throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] - else: - throttle_prob = 1.0 - return x, v, a, j, throttle_prob - - @staticmethod - def get_model_launch_accel(model_v, model_a, action_t, v_ego): - if len(model_v) != len(T_IDXS_MPC) or len(model_a) != len(T_IDXS_MPC): - return None - if float(np.interp(MODEL_LAUNCH_COMMIT_TIME, T_IDXS_MPC, model_v)) <= MODEL_LAUNCH_DISARM_SPEED: - return None - - moving_idxs = np.flatnonzero(np.asarray(model_v) > MODEL_LAUNCH_MOVING_SPEED) - if len(moving_idxs) == 0: - return None - - t_cut = min(float(T_IDXS_MPC[int(moving_idxs[0])]), MODEL_LAUNCH_COMMIT_TIME) - shifted_t = T_IDXS_MPC + t_cut - shifted_v = np.interp(shifted_t, T_IDXS_MPC, model_v) - shifted_a = np.interp(shifted_t, T_IDXS_MPC, model_a) - safe_action_t = max(float(action_t), 1e-3) - v_target = float(np.interp(safe_action_t, T_IDXS_MPC, shifted_v)) - a_launch = 2.0 * (v_target - float(shifted_v[0])) / safe_action_t - float(shifted_a[0]) - accel_cap = float(np.interp( - float(v_ego), - [MODEL_LAUNCH_MOVING_SPEED, MODEL_LAUNCH_DISARM_SPEED], - [MODEL_LAUNCH_MAX_ACCEL, 0.0], - )) - return float(np.clip(a_launch, 0.0, accel_cap)) - - def get_close_lead_brake_cap(self, lead, v_ego, accel_min): - if lead is None or not lead.status: - return None - - lead_brake = max(0.0, -float(lead.aLeadK)) - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - closing_speed = max(0.0, v_ego - lead.vLead) - projected_closing_speed = closing_speed + lead_brake * reaction_t - if projected_closing_speed < 0.1 and lead_brake < 0.5: - return None - - target_gap = float(np.clip(2.0 + 0.2 * v_ego, 2.0, 6.0)) - delay_buffer = projected_closing_speed * reaction_t - available_gap = max(float(lead.dRel) - target_gap - delay_buffer, 0.5) - projected_ttc = available_gap / max(projected_closing_speed, 0.1) - if projected_ttc > CLOSE_LEAD_BRAKE_CAP_MAX_TTC: - return None - required_decel = (projected_closing_speed ** 2) / (2.0 * available_gap) + 0.7 * lead_brake - if required_decel < 0.2: - return None - - return max(accel_min, -required_decel) - - @staticmethod - def get_inside_gap_closing_lead_accel_cap(lead, v_ego, accel_min, t_follow): - if lead is None or not lead.status: - return None - - ego_speed = float(v_ego) - lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) - if ego_speed < INSIDE_GAP_CLOSING_MIN_EGO_SPEED or lead_speed < INSIDE_GAP_CLOSING_MIN_LEAD_SPEED: - return None - if abs(float(getattr(lead, "yRel", 0.0))) > INSIDE_GAP_CLOSING_MAX_LATERAL_OFFSET: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < INSIDE_GAP_CLOSING_VISION_MIN_MODEL_PROB: - return None - - closing_speed = ego_speed - lead_speed - if closing_speed < INSIDE_GAP_CLOSING_MIN_SPEED: - return None - - desired_gap = float(desired_follow_distance(ego_speed, lead_speed, float(t_follow))) - gap_deficit = desired_gap - float(lead.dRel) - trigger_deficit = max(INSIDE_GAP_CLOSING_MIN_DEFICIT, - INSIDE_GAP_CLOSING_DEFICIT_RATIO * desired_gap) - if gap_deficit <= trigger_deficit: - return None - - brake_deficit = INSIDE_GAP_CLOSING_BRAKE_DEFICIT_RATIO * desired_gap - deficit_factor = float(np.clip( - (gap_deficit - brake_deficit) / max(brake_deficit, 1.0), - 0.0, - 1.0, - )) - closing_factor = float(np.clip( - (closing_speed - INSIDE_GAP_CLOSING_BRAKE_MIN_SPEED) / - (INSIDE_GAP_CLOSING_FULL_SPEED - INSIDE_GAP_CLOSING_BRAKE_MIN_SPEED), - 0.0, - 1.0, - )) - required_decel = 0.45 * deficit_factor + 0.20 * closing_factor - required_decel = min(required_decel, INSIDE_GAP_CLOSING_MAX_DECEL) - return max(float(accel_min), -required_decel) - - def get_vision_lead_approach_cap(self, lead, v_ego, accel_min, t_follow): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_LEAD_APPROACH_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(lead.aLeadK)) - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - closing_speed = max(0.0, v_ego - lead.vLead) - projected_closing_speed = closing_speed + lead_brake * reaction_t - if projected_closing_speed < VISION_LEAD_APPROACH_MIN_CLOSING_SPEED: - return None - - tight_follow_gap = float(t_follow * v_ego + VISION_LEAD_APPROACH_TIGHT_BUFFER) - gap_to_tight_follow = float(lead.dRel) - tight_follow_gap - time_to_tight_follow = gap_to_tight_follow / max(projected_closing_speed, 0.1) - if time_to_tight_follow > VISION_LEAD_APPROACH_TRIGGER_TIME: - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - if float(lead.dRel) > desired_gap + VISION_LEAD_APPROACH_TIGHT_BUFFER: - return None - - time_factor = float(np.clip((VISION_LEAD_APPROACH_TRIGGER_TIME - time_to_tight_follow) / - (VISION_LEAD_APPROACH_TRIGGER_TIME - VISION_LEAD_APPROACH_FULL_TIME), 0.0, 1.0)) - prob_factor = float(np.clip((lead_prob - VISION_LEAD_APPROACH_MIN_MODEL_PROB) / - (VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_LEAD_APPROACH_MIN_MODEL_PROB), 0.0, 1.0)) - closing_factor = float(np.clip(projected_closing_speed / (VISION_LEAD_APPROACH_MIN_CLOSING_SPEED + 2.5), 0.0, 1.0)) - tight_follow_deficit = max(tight_follow_gap - float(lead.dRel), 0.0) - tight_follow_buffer = max(VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN, - VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN * float(v_ego) + 1.0) - deficit_factor = float(np.clip(tight_follow_deficit / tight_follow_buffer, 0.0, 1.0)) - - approach_decel = VISION_LEAD_APPROACH_MAX_DECEL * time_factor * (0.45 + 0.55 * prob_factor) - approach_decel *= 0.6 + 0.4 * closing_factor - deficit_decel = VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL * deficit_factor * prob_factor - deficit_decel *= 0.5 + 0.5 * closing_factor - approach_decel = max(approach_decel, deficit_decel) - - # If a tracked vision lead is already far inside the tight-follow window and - # it is actively braking, don't stay stuck at the softer comfort cap. - if deficit_factor >= VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN and lead_brake >= VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE: - braking_floor = float(np.interp( - lead_brake, - [VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE, VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE], - [VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL, VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL], - )) - braking_floor *= 0.85 + 0.15 * max(closing_factor, prob_factor) - approach_decel = max(approach_decel, braking_floor) - - if approach_decel < VISION_LEAD_APPROACH_MIN_DECEL: - return None - - return max(accel_min, -approach_decel) - - def get_vision_untracked_slow_lead_cap(self, lead, v_ego, accel_min): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - - lead_brake = max(0.0, -float(lead.aLeadK)) - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - closing_speed = max(0.0, v_ego - lead.vLead) - projected_closing_speed = closing_speed + lead_brake * reaction_t - closing_ratio = projected_closing_speed / max(float(v_ego), 0.1) - projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) - - standard_entry = bool( - projected_closing_speed >= VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED and - closing_ratio >= VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO and - projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - ) - centered_relaxed_entry = ( - abs(float(getattr(lead, "yRel", 0.0))) <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_ENTRY_MAX_LATERAL_OFFSET - ) - high_confidence_early_entry = bool( - centered_relaxed_entry and - lead_prob + 1e-6 >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_MODEL_PROB and - projected_closing_speed >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_SPEED and - closing_ratio >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_RATIO and - projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC - ) - near_early_entry = bool( - centered_relaxed_entry and - float(lead.dRel) <= VISION_UNTRACKED_SLOW_LEAD_NEAR_MAX_DISTANCE and - lead_prob + 1e-6 >= VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_MODEL_PROB and - projected_closing_speed >= VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_SPEED and - closing_ratio >= VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_CLOSING_RATIO and - projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC - ) - if not (standard_entry or high_confidence_early_entry or near_early_entry): - return None - - trigger_ttc = VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - min_closing_ratio = VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO - if high_confidence_early_entry: - trigger_ttc = VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC - min_closing_ratio = VISION_UNTRACKED_SLOW_LEAD_EARLY_MIN_CLOSING_RATIO - if near_early_entry: - trigger_ttc = VISION_UNTRACKED_SLOW_LEAD_EARLY_TRIGGER_TTC - min_closing_ratio = VISION_UNTRACKED_SLOW_LEAD_NEAR_MIN_CLOSING_RATIO - - min_model_prob = VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB - max_distance_time = VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME - if float(lead.vLead) <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED and \ - projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC: - closing_relax = float(np.clip((projected_closing_speed - VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED) / - (VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED - - VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED), 0.0, 1.0)) - ttc_relax = float(np.clip((VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC - projected_ttc) / - (VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC - - VISION_UNTRACKED_SLOW_LEAD_FULL_TTC), 0.0, 1.0)) - relax_factor = closing_relax * ttc_relax - min_model_prob = float(np.interp(relax_factor, [0.0, 1.0], - [VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB, - VISION_UNTRACKED_SLOW_LEAD_RELAXED_MODEL_PROB])) - max_distance_time = float(np.interp(relax_factor, [0.0, 1.0], - [VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME, - VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_DISTANCE_TIME])) - - max_distance = float(np.clip(max_distance_time * v_ego, - VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE, - VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE)) - if float(lead.dRel) > max_distance: - return None - - if lead_prob + 1e-6 < min_model_prob: - return None - - time_factor = float(np.clip((trigger_ttc - projected_ttc) / - (trigger_ttc - VISION_UNTRACKED_SLOW_LEAD_FULL_TTC), - 0.0, 1.0)) - prob_factor = float(np.clip((lead_prob - min_model_prob) / - (VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - min_model_prob), - 0.0, 1.0)) - closing_factor = float(np.clip((closing_ratio - min_closing_ratio) / - (VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO - min_closing_ratio), - 0.0, 1.0)) - approach_decel = VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL * self.untracked_slow_lead_decel_scale * np.clip( - 0.5 * time_factor + 0.3 * prob_factor + 0.2 * closing_factor, 0.0, 1.0) - if approach_decel < VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL: - return None - - return max(accel_min, -approach_decel) - - @staticmethod - def get_vision_untracked_approach_lift_cap(lead, v_ego, t_follow): - """Trim throttle before a confident vision lead reaches the tracking window.""" - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - if float(v_ego) < VISION_UNTRACKED_APPROACH_LIFT_MIN_EGO_SPEED: - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_UNTRACKED_APPROACH_LIFT_MIN_MODEL_PROB: - return None - if abs(float(getattr(lead, "yRel", 0.0))) > VISION_UNTRACKED_APPROACH_LIFT_MAX_LATERAL_OFFSET: - return None - if float(lead.dRel) > VISION_UNTRACKED_APPROACH_LIFT_MAX_DISTANCE: - return None - - closing_speed = float(v_ego) - float(lead.vLead) - if closing_speed < VISION_UNTRACKED_APPROACH_LIFT_MIN_CLOSING_SPEED: - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - gap_excess = float(lead.dRel) - desired_gap - if gap_excess < VISION_UNTRACKED_APPROACH_LIFT_MIN_GAP_EXCESS: + def _model_stop_authority(model, v_ego): + if bool(getattr(model.action, "shouldStop", False)): + return 1.0 + if not len(model.position.x): return 0.0 - time_to_desired_gap = gap_excess / max(closing_speed, 0.1) - if time_to_desired_gap > VISION_UNTRACKED_APPROACH_LIFT_TRIGGER_TIME: - return None - - # This path only removes positive acceleration. Braking remains exclusively - # owned by lead tracking and the existing close-lead safety caps. - return float(np.interp( - time_to_desired_gap, - [VISION_UNTRACKED_APPROACH_LIFT_FULL_TIME, VISION_UNTRACKED_APPROACH_LIFT_TRIGGER_TIME], - [0.0, VISION_UNTRACKED_APPROACH_LIFT_MAX_ACCEL], - )) - - def update_vision_untracked_approach_lift_cap(self, raw_cap, output_a_target, prev_output_a_target, - now_t, untracked): - if untracked and raw_cap is not None: - if self.untracked_vision_approach_lift_cap is None: - self.untracked_vision_approach_lift_confirm_t = min( - self.untracked_vision_approach_lift_confirm_t + self.dt, - VISION_UNTRACKED_APPROACH_LIFT_CONFIRM_TIME, - ) - if self.untracked_vision_approach_lift_confirm_t >= VISION_UNTRACKED_APPROACH_LIFT_CONFIRM_TIME: - self.untracked_vision_approach_lift_cap = float(prev_output_a_target) - if self.untracked_vision_approach_lift_cap is not None: - self.untracked_vision_approach_lift_target = float(raw_cap) - self.untracked_vision_approach_lift_hold_until = now_t + VISION_UNTRACKED_APPROACH_LIFT_HOLD_TIME - elif self.untracked_vision_approach_lift_cap is None: - self.untracked_vision_approach_lift_confirm_t = 0.0 - - active_cap = self.untracked_vision_approach_lift_cap - if active_cap is None: - return None - - holding = untracked and now_t < self.untracked_vision_approach_lift_hold_until - target = self.untracked_vision_approach_lift_target if holding else float(output_a_target) - if target is None: - target = float(output_a_target) - - lower = active_cap - VISION_UNTRACKED_APPROACH_LIFT_RATE_DOWN * self.dt - upper = active_cap + VISION_UNTRACKED_APPROACH_LIFT_RATE_UP * self.dt - active_cap = float(np.clip(target, lower, upper)) - self.untracked_vision_approach_lift_cap = active_cap - - if not holding and active_cap >= float(output_a_target) - 1e-6: - self.untracked_vision_approach_lift_confirm_t = 0.0 - self.untracked_vision_approach_lift_cap = None - self.untracked_vision_approach_lift_target = None - self.untracked_vision_approach_lift_hold_until = 0.0 - return None - - return active_cap - - def get_vision_slow_stopped_lead_cap(self, lead, v_ego, accel_min, t_follow): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_SLOW_LEAD_MIN_MODEL_PROB or float(lead.vLead) > VISION_SLOW_LEAD_MAX_SPEED: - return None - - lead_brake = max(0.0, -float(lead.aLeadK)) - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - closing_speed = max(0.0, v_ego - lead.vLead) - projected_closing_speed = closing_speed + lead_brake * reaction_t - if projected_closing_speed < VISION_SLOW_LEAD_MIN_CLOSING_SPEED: - return None - - stop_gap = float(max(STOP_DISTANCE + 1.0, 2.5 + 0.15 * max(float(lead.vLead), 0.0))) - delay_buffer = projected_closing_speed * reaction_t - available_gap = max(float(lead.dRel) - stop_gap - delay_buffer, 0.5) - projected_ttc = available_gap / max(projected_closing_speed, 0.1) - if projected_ttc > VISION_SLOW_LEAD_TRIGGER_TTC: - return None - - time_factor = float(np.clip((VISION_SLOW_LEAD_TRIGGER_TTC - projected_ttc) / - (VISION_SLOW_LEAD_TRIGGER_TTC - VISION_SLOW_LEAD_FULL_TTC), 0.0, 1.0)) - prob_factor = float(np.clip((lead_prob - VISION_SLOW_LEAD_MIN_MODEL_PROB) / - (VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_SLOW_LEAD_MIN_MODEL_PROB), 0.0, 1.0)) - speed_factor = float(np.clip((VISION_SLOW_LEAD_MAX_SPEED - max(float(lead.vLead), 0.0)) / - VISION_SLOW_LEAD_MAX_SPEED, 0.0, 1.0)) - required_decel = (projected_closing_speed ** 2) / (2.0 * available_gap) - decel_scale = 0.45 + 0.35 * time_factor + 0.20 * speed_factor - approach_decel = min(VISION_SLOW_LEAD_MAX_DECEL, required_decel * decel_scale) - approach_decel *= 0.65 + 0.35 * prob_factor - if approach_decel < VISION_SLOW_LEAD_MIN_DECEL: - return None - - return max(accel_min, -approach_decel) - - def tracked_vision_lead_approach_needs_immediate_brake(self, lead, v_ego, approach_cap): - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - projected_closing_speed = max(0.0, v_ego - float(lead.vLead)) + lead_brake * reaction_t - bypass_distance = max(VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_MIN, - VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_TIME * float(v_ego)) - return ( - approach_cap <= -VISION_LEAD_APPROACH_CONFIRM_BYPASS_DECEL or - projected_closing_speed >= VISION_LEAD_APPROACH_CONFIRM_BYPASS_CLOSING_SPEED or - lead_brake >= VISION_LEAD_APPROACH_CONFIRM_BYPASS_LEAD_BRAKE or - float(lead.dRel) <= bypass_distance - ) - - def get_dynamic_t_follow(self, base_t_follow, lead, v_ego): - base_t_follow = float(base_t_follow) - target_t_follow = base_t_follow - - if lead is not None and lead.status: - lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) - if bool(getattr(lead, "radar", False)) or lead_prob >= VISION_LEAD_APPROACH_MIN_MODEL_PROB: - lead_brake = max(0.0, -float(lead.aLeadK)) - closing_speed = max(0.0, v_ego - lead.vLead) - if closing_speed >= LEAD_APPROACH_TFOLLOW_MIN_CLOSING_SPEED or lead_brake >= LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE: - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, base_t_follow)) - approach_window = max(LEAD_APPROACH_TFOLLOW_WINDOW_MIN, LEAD_APPROACH_TFOLLOW_WINDOW_GAIN * float(v_ego)) - if float(lead.dRel) <= desired_gap + approach_window: - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - projected_closing_speed = closing_speed + 0.5 * lead_brake * reaction_t - gap_to_follow = max(float(lead.dRel) - desired_gap, 0.0) - time_to_follow = gap_to_follow / max(projected_closing_speed, 0.1) - time_factor = float(np.clip((LEAD_APPROACH_TFOLLOW_TRIGGER_TIME - time_to_follow) / - (LEAD_APPROACH_TFOLLOW_TRIGGER_TIME - LEAD_APPROACH_TFOLLOW_FULL_TIME), 0.0, 1.0)) - closing_factor = float(np.clip(closing_speed / LEAD_APPROACH_TFOLLOW_MAX_CLOSING_SPEED, 0.0, 1.0)) - brake_factor = float(np.clip(lead_brake / LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE, 0.0, 1.0)) - target_delta = LEAD_APPROACH_TFOLLOW_MAX_DELTA * np.clip( - 0.55 * time_factor + 0.25 * closing_factor + 0.20 * brake_factor, 0.0, 1.0) - if not bool(getattr(lead, "radar", False)): - gap_deficit = max(desired_gap - float(lead.dRel), 0.0) - gap_buffer = max(VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN, - VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN * float(v_ego)) - gap_factor = float(np.clip(gap_deficit / gap_buffer, 0.0, 1.0)) - slow_lead_factor = float(np.clip((VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED - float(lead.vLead)) / - VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED, 0.0, 1.0)) - vision_extra = VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA * np.clip( - 0.40 * time_factor + 0.30 * gap_factor + 0.20 * slow_lead_factor + 0.10 * closing_factor, - 0.0, 1.0) - target_delta += vision_extra - target_t_follow = base_t_follow + float(target_delta) - - if self.effective_t_follow is None: - self.effective_t_follow = base_t_follow - - rate = LEAD_APPROACH_TFOLLOW_RATE_UP if target_t_follow > self.effective_t_follow else LEAD_APPROACH_TFOLLOW_RATE_DOWN - step = rate * self.dt - self.effective_t_follow = float(np.clip(target_t_follow, self.effective_t_follow - step, self.effective_t_follow + step)) - self.effective_t_follow = max(base_t_follow, self.effective_t_follow) - return self.effective_t_follow - - def get_vision_low_speed_stop_buffer_cap(self, lead, v_ego, accel_min): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None, False - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB: - return None, False - - lead_speed = max(float(lead.vLead), 0.0) - relative_speed = float(v_ego) - lead_speed - closing_speed = max(0.0, v_ego - lead_speed) - entry_context = ( - v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and - lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED and - closing_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED - ) - hold_context = ( - v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and - lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED and - relative_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED - ) - - now_t = time.monotonic() - entry_buffer = max(3.2, VISION_LOW_SPEED_STOP_BUFFER_BASE + - VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN * float(v_ego) + - VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN * lead_speed) - release_buffer = entry_buffer + VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN - if entry_context and float(lead.dRel) <= entry_buffer: - self.vision_low_speed_stop_hold_until = now_t + VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME - - latched = now_t < self.vision_low_speed_stop_hold_until - active = bool( - (entry_context and float(lead.dRel) <= entry_buffer) or - (latched and hold_context and float(lead.dRel) <= release_buffer) - ) - if not active: - return None, False - - min_stop_brake = VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE + VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN * float(v_ego) - return max(accel_min, -min_stop_brake), True - - def get_vision_close_stop_hold_cap(self, lead, v_ego, accel_min, should_stop): - if not should_stop or lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB: - return None - - lead_speed = max(float(lead.vLead), 0.0) - near_standstill_settle = bool( - float(v_ego) <= VISION_CLOSE_SETTLE_MAX_EGO_SPEED and - lead_speed <= VISION_CLOSE_SETTLE_MAX_LEAD_SPEED and - float(lead.dRel) <= VISION_CLOSE_SETTLE_MAX_DISTANCE - ) - max_lead_speed = VISION_CLOSE_SETTLE_MAX_LEAD_SPEED if near_standstill_settle else VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED - max_distance = VISION_CLOSE_SETTLE_MAX_DISTANCE if near_standstill_settle else VISION_CLOSE_STOP_HOLD_MAX_DISTANCE - if ( - float(v_ego) > VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED or - lead_speed > max_lead_speed or - float(lead.dRel) > max_distance - ): - return None - - distance_factor = float(np.clip((max_distance - float(lead.dRel)) / - max(max_distance - 1.8, 0.1), 0.0, 1.0)) - speed_factor = float(np.clip(float(v_ego) / max(VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED, 0.1), 0.0, 1.0)) - hold_brake = VISION_CLOSE_STOP_HOLD_MIN_BRAKE + 0.08 * distance_factor + 0.08 * speed_factor - if near_standstill_settle: - settle_brake = VISION_CLOSE_SETTLE_MIN_BRAKE + 0.10 * distance_factor + 0.04 * speed_factor - hold_brake = max(hold_brake, settle_brake) - hold_brake = float(np.clip(hold_brake, VISION_CLOSE_STOP_HOLD_MIN_BRAKE, VISION_CLOSE_STOP_HOLD_MAX_BRAKE)) - brake_floor = -hold_brake - return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor) - - def get_vision_close_release_hold_cap(self, lead, v_ego, accel_min, should_stop): - if should_stop or lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_CLOSE_RELEASE_HOLD_MIN_MODEL_PROB: - return None - - lead_speed = max(float(lead.vLead), 0.0) - lead_delta = lead_speed - float(v_ego) - near_standstill_settle = bool( - float(v_ego) <= VISION_CLOSE_SETTLE_MAX_EGO_SPEED and - lead_speed <= VISION_CLOSE_SETTLE_MAX_LEAD_SPEED and - float(lead.dRel) <= VISION_CLOSE_SETTLE_MAX_DISTANCE and - lead_delta <= VISION_CLOSE_SETTLE_MAX_LEAD_DELTA - ) - max_lead_speed = VISION_CLOSE_SETTLE_MAX_LEAD_SPEED if near_standstill_settle else VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED - max_distance = VISION_CLOSE_SETTLE_MAX_DISTANCE if near_standstill_settle else VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE - max_lead_delta = VISION_CLOSE_SETTLE_MAX_LEAD_DELTA if near_standstill_settle else VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA - if ( - float(v_ego) > VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED or - lead_speed > max_lead_speed or - float(lead.dRel) > max_distance or - lead_delta < VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA or - lead_delta > max_lead_delta - ): - return None - - distance_factor = float(np.clip((max_distance - float(lead.dRel)) / - max(max_distance - 2.8, 0.1), 0.0, 1.0)) - speed_factor = float(np.clip(float(v_ego) / max(VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED, 0.1), 0.0, 1.0)) - delta_factor = float(np.clip((max_lead_delta - lead_delta) / - max(max_lead_delta - VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA, 0.1), - 0.0, 1.0)) - hold_brake = VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE + 0.12 * distance_factor + 0.06 * speed_factor + 0.04 * delta_factor - if near_standstill_settle: - settle_brake = VISION_CLOSE_SETTLE_MIN_BRAKE + 0.08 * distance_factor + 0.02 * delta_factor - hold_brake = max(hold_brake, settle_brake) - hold_brake = float(np.clip(hold_brake, VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE, VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE)) - brake_floor = -hold_brake - return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor) - - def get_vision_close_settle_cap(self, lead, v_ego, accel_min, stop_guard_active): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB: - return None - - lead_speed = max(float(lead.vLead), 0.0) - lead_delta = lead_speed - float(v_ego) - if ( - float(v_ego) > VISION_CLOSE_SETTLE_MAX_EGO_SPEED or - lead_speed > VISION_CLOSE_SETTLE_MAX_LEAD_SPEED or - float(lead.dRel) > VISION_CLOSE_SETTLE_MAX_DISTANCE or - lead_delta > VISION_CLOSE_SETTLE_MAX_LEAD_DELTA - ): - return None - - if not stop_guard_active and lead_delta < 0.0: - return None - - distance_factor = float(np.clip((VISION_CLOSE_SETTLE_MAX_DISTANCE - float(lead.dRel)) / - max(VISION_CLOSE_SETTLE_MAX_DISTANCE - 2.8, 0.1), 0.0, 1.0)) - delta_factor = float(np.clip((VISION_CLOSE_SETTLE_MAX_LEAD_DELTA - lead_delta) / - max(VISION_CLOSE_SETTLE_MAX_LEAD_DELTA, 0.1), 0.0, 1.0)) - hold_brake = VISION_CLOSE_SETTLE_MIN_BRAKE + 0.10 * distance_factor + 0.04 * delta_factor - hold_brake = float(np.clip(hold_brake, VISION_CLOSE_SETTLE_MIN_BRAKE, VISION_CLOSE_SETTLE_MAX_BRAKE)) - brake_floor = -hold_brake - return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor) - - def get_vision_close_final_guard_cap(self, lead, v_ego, accel_min): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - lead_speed = max(float(lead.vLead), 0.0) - if ( - lead_prob < VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB or - float(v_ego) > VISION_CLOSE_FINAL_GUARD_MAX_EGO_SPEED or - lead_speed > VISION_CLOSE_FINAL_GUARD_MAX_LEAD_SPEED or - float(lead.dRel) > VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE - ): - return None - - distance_factor = float(np.clip((VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE - float(lead.dRel)) / - max(VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE - 2.8, 0.1), 0.0, 1.0)) - hold_brake = VISION_CLOSE_FINAL_GUARD_MIN_BRAKE + 0.10 * distance_factor - hold_brake = float(np.clip(hold_brake, VISION_CLOSE_FINAL_GUARD_MIN_BRAKE, VISION_CLOSE_FINAL_GUARD_MAX_BRAKE)) - brake_floor = -hold_brake - return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor) - - def _update_manual_stop_resume_override(self, sm): - now_t = time.monotonic() - lead = sm["radarState"].leadOne - no_lead = not bool(getattr(lead, "status", False)) - try: - starpilot_car_state = sm["starpilotCarState"] - except KeyError: - starpilot_car_state = None - accel_pressed = bool(getattr(starpilot_car_state, "accelPressed", False)) - model_should_stop = bool(getattr(sm["modelV2"].action, "shouldStop", False)) - standstill = bool(getattr(sm["carState"], "standstill", False)) - forcing_stop = bool(getattr(sm["starpilotPlan"], "forcingStop", False)) - red_light = bool(getattr(sm["starpilotPlan"], "redLight", False)) - - if standstill and no_lead and accel_pressed and (forcing_stop or red_light or model_should_stop): - self.manual_stop_resume_override_until = now_t + MANUAL_STOP_RESUME_OVERRIDE_TIME - - return bool( - no_lead and - float(getattr(sm["carState"], "vEgo", 0.0)) < MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED and - now_t < self.manual_stop_resume_override_until - ) - - def is_confident_lead_depart(self, lead, v_ego): - if lead is None or not lead.status: - return False - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: - return False - - lead_speed = max(float(lead.vLead), 0.0) - lead_delta = lead_speed - float(v_ego) - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - return bool( - float(lead.dRel) >= LEAD_DEPART_CONFIDENT_MIN_GAP and - float(lead.dRel) <= LEAD_DEPART_CONFIDENT_MAX_GAP and - lead_speed >= LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED and - lead_delta >= LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA and - lead_accel >= LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL - ) - - def is_slow_creep_lead_depart(self, lead, v_ego, standstill_nudge_gap): - if lead is None or not lead.status: - return False - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB: - return False - - if abs(float(getattr(lead, "yRel", 0.0))) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET: - return False - - lead_gap = float(getattr(lead, "dRel", 0.0)) - lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - return bool( - float(v_ego) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED and - lead_gap >= standstill_nudge_gap + STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN and - lead_speed >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED and - lead_accel >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL - ) - - def get_safe_depart_release_hold_lead(self, v_ego): - credible_leads = [ - lead for lead in (self.lead_one, self.lead_two) - if lead is not None and - bool(getattr(lead, "status", False)) and - (bool(getattr(lead, "radar", False)) or - float(getattr(lead, "modelProb", 0.0)) >= LEAD_DEPART_RELEASE_HOLD_MIN_MODEL_PROB) and - abs(float(getattr(lead, "yRel", 0.0))) <= LEAD_DEPART_RELEASE_HOLD_MAX_LATERAL_OFFSET - ] - moving_leads = [ - lead for lead in credible_leads - if float(getattr(lead, "dRel", 0.0)) >= LEAD_DEPART_RELEASE_HOLD_MIN_DISTANCE and - float(getattr(lead, "vLead", 0.0)) >= LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_SPEED and - float(getattr(lead, "vLead", 0.0)) - float(v_ego) >= LEAD_DEPART_RELEASE_HOLD_MIN_LEAD_DELTA and - max(0.0, -float(getattr(lead, "aLeadK", 0.0))) <= LEAD_DEPART_RELEASE_HOLD_MAX_LEAD_BRAKE - ] - if not moving_leads: - return None - - selected_lead = min(moving_leads, key=lambda lead: float(lead.dRel)) - stopped_conflict = any( - lead is not selected_lead and - float(getattr(lead, "dRel", float("inf"))) <= float(selected_lead.dRel) + LEAD_DEPART_RELEASE_HOLD_CONFLICT_DISTANCE_MARGIN and - float(getattr(lead, "vLead", 0.0)) < LEAD_DEPART_RELEASE_HOLD_CONFLICT_SPEED - for lead in credible_leads - ) - return None if stopped_conflict else selected_lead - - def get_vehicle_far_follow_slew_target(self, v_ego, prev_target, target, output_should_stop, panic_bypass): - if self.far_follow_brake_slew_rate <= 0.0 or self.far_follow_release_slew_rate <= 0.0 or output_should_stop or panic_bypass: - self.far_follow_output_slew_active = False - return target - - centered_leads = [ - lead for lead in (self.lead_one, self.lead_two) - if bool(getattr(lead, "status", False)) and - abs(float(getattr(lead, "yRel", 0.0))) <= VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET - ] - safe_far_follow = bool(centered_leads and float(v_ego) >= VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED) - for lead in centered_leads: - distance = float(getattr(lead, "dRel", 0.0)) - closing_speed = max(0.0, float(v_ego) - float(getattr(lead, "vLead", v_ego))) - ttc = distance / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - headway = distance / max(float(v_ego), 1e-3) - safe_far_follow &= bool( - distance >= max(VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE, - VEHICLE_FAR_FOLLOW_SLEW_MIN_DISTANCE_TIME * float(v_ego)) and - headway >= VEHICLE_FAR_FOLLOW_SLEW_MIN_HEADWAY and - ttc >= VEHICLE_FAR_FOLLOW_SLEW_MIN_TTC - ) - - slew_was_active = self.far_follow_output_slew_active - self.far_follow_output_slew_active = safe_far_follow - if not safe_far_follow or not slew_was_active: - return target - - return float(np.clip( - target, - float(prev_target) - self.far_follow_brake_slew_rate * self.dt, - float(prev_target) + self.far_follow_release_slew_rate * self.dt, - )) + model_length = max(float(model.position.x[-1]), 0.0) + horizon = model_length / max(float(v_ego), 1.0) + end_speed = float(model.velocity.x[-1]) if len(model.velocity.x) else float(v_ego) + time_score = 1.0 - _smoothstep(horizon, MODEL_STOP_FULL_TIME, MODEL_STOP_BEGIN_TIME) + speed_score = 1.0 - _smoothstep(end_speed, 0.5, MODEL_STOP_MIN_END_SPEED) + return float(time_score * speed_score) @staticmethod - def is_radar_standstill_gap_settle_candidate(lead, v_ego, target_gap, active=False): - if lead is None or not lead.status or not bool(getattr(lead, "radar", False)): - return False - if float(v_ego) > RADAR_STANDSTILL_GAP_SETTLE_MAX_EGO_SPEED: - return False - if abs(float(getattr(lead, "yRel", 0.0))) > RADAR_STANDSTILL_GAP_SETTLE_MAX_LATERAL_OFFSET: - return False - if abs(float(getattr(lead, "vLead", 0.0))) > RADAR_STANDSTILL_GAP_SETTLE_MAX_LEAD_SPEED: - return False + def _curve_authority(sm, v_ego): + curvature = abs(float(getattr(sm["starpilotPlan"], "roadCurvature", 0.0))) + lateral_accel = curvature * float(v_ego) ** 2 + curve_score = _smoothstep(lateral_accel, 0.7, 2.0) + turning = bool(sm["carState"].leftBlinker or sm["carState"].rightBlinker) + turn_score = 0.55 if turning and v_ego < 15.0 else 0.0 + return max(curve_score, turn_score) - lead_gap = float(getattr(lead, "dRel", 0.0)) - min_margin = RADAR_STANDSTILL_GAP_SETTLE_EXIT_MARGIN if active else RADAR_STANDSTILL_GAP_SETTLE_ENTRY_MARGIN - return bool( - lead_gap > target_gap + min_margin and - lead_gap <= target_gap + RADAR_STANDSTILL_GAP_SETTLE_MAX_EXTRA_GAP - ) - - def update_radar_standstill_gap_settle(self, sm, target_gap): - vetoed = bool( - getattr(sm["carState"], "brakePressed", False) or - getattr(sm["carState"], "gasPressed", False) or + def _get_model_authority(self, sm, v_ego, v_cruise, has_lead, model_first, stop_authority): + curve_authority = self._curve_authority(sm, v_ego) + hard_stop = bool( + getattr(sm["starpilotPlan"], "redLight", False) or getattr(sm["starpilotPlan"], "forcingStop", False) or - getattr(sm["starpilotPlan"], "redLight", False) + getattr(sm["starpilotPlan"], "stopSignConfirmed", False) ) - candidates = [ - lead for lead in (self.lead_one, self.lead_two) - if self.is_radar_standstill_gap_settle_candidate( - lead, - float(sm["carState"].vEgo), - target_gap, - active=self.radar_standstill_gap_settle_active, - ) + scene_authority = 1.0 if hard_stop else max(stop_authority, curve_authority) + + if not model_first: + return scene_authority + + cruise_error = max(0.0, float(v_cruise) - float(v_ego)) + open_road = not has_lead and scene_authority < 0.1 + open_road_blend = _smoothstep(cruise_error, 0.0, MODEL_FIRST_CRUISE_ERROR) + base_authority = ( + MODEL_FIRST_AUTHORITY + + (MODEL_FIRST_OPEN_ROAD_AUTHORITY - MODEL_FIRST_AUTHORITY) * open_road_blend + if open_road else MODEL_FIRST_AUTHORITY + ) + return max(scene_authority, base_authority) + + @staticmethod + def _lead_safety_cap(lead, v_ego, reaction_time, stop_buffer): + if lead is None or not bool(getattr(lead, "status", False)): + return None, False + + d_rel = max(float(getattr(lead, "dRel", 0.0)), 0.0) + v_lead = max(float(getattr(lead, "vLead", 0.0)), 0.0) + closing_speed = max(0.0, float(v_ego) - v_lead) + if closing_speed < RAW_SAFETY_MIN_CLOSING_SPEED: + return None, False + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + projected_closing = closing_speed + lead_brake * reaction_time + available_distance = d_rel - stop_buffer - closing_speed * reaction_time + ttc = max(d_rel - stop_buffer, 0.0) / max(closing_speed, 1e-3) + + if available_distance <= 0.0: + return float(ACCEL_MIN), True + + required_decel = projected_closing ** 2 / (2.0 * available_distance) + required_decel += 0.35 * lead_brake + urgent = required_decel >= RAW_SAFETY_URGENT_DECEL or ttc <= RAW_SAFETY_URGENT_TTC + if required_decel < RAW_SAFETY_MIN_DECEL and not urgent: + return None, False + return -float(required_decel), urgent + + def _get_raw_safety_cap(self, sm, v_ego, reaction_time): + stop_buffer = ( + RAW_SAFETY_EXTRA_STOP_BUFFER + + float(getattr(sm["starpilotPlan"], "increasedStoppedDistance", 0.0)) + ) + caps = [ + self._lead_safety_cap(lead, v_ego, reaction_time, stop_buffer) + for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo) ] - - if vetoed or not candidates: - self.radar_standstill_gap_settle_elapsed = 0.0 - self.radar_standstill_gap_settle_active = False - return False - - if self.radar_standstill_gap_settle_active: - return True - - if not bool(sm["carState"].standstill): - self.radar_standstill_gap_settle_elapsed = 0.0 - return False - - self.radar_standstill_gap_settle_elapsed = min( - RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME, - self.radar_standstill_gap_settle_elapsed + self.dt, - ) - self.radar_standstill_gap_settle_active = ( - self.radar_standstill_gap_settle_elapsed + 1e-6 >= RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME - ) - return self.radar_standstill_gap_settle_active + valid_caps = [cap for cap, _ in caps if cap is not None] + return (min(valid_caps) if valid_caps else None), any(urgent for _, urgent in caps) @staticmethod - def get_centered_model_lead(model_data): - try: - leads = model_data.leadsV3 - except Exception: - return None + def _safe_departure(sm, model_accel, stop_buffer): + force_stop = bool(getattr(sm["starpilotPlan"], "forcingStop", False)) + stop_sign = bool(getattr(sm["starpilotPlan"], "stopSignConfirmed", False)) + if force_stop or stop_sign or bool(sm["carState"].brakePressed): + return False, False - best_candidate = None - for i in range(3): - try: - lead = leads[i] - prob = float(lead.prob) - x = float(lead.x[0]) - y = float(lead.y[0]) - v = float(lead.v[0]) - except Exception: - continue + leads = [lead for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo) + if bool(getattr(lead, "status", False))] + if not leads: + return model_accel >= DEPART_MODEL_MIN_ACCEL, False - if ( - prob < RADAR_DEPART_CONFLICT_MIN_MODEL_PROB or - x <= 0.0 or - x > RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE or - abs(y) > RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL or - max(v, 0.0) > RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED - ): - continue - - if best_candidate is None or x < best_candidate[0]: - best_candidate = (x, y, v, prob) - - return best_candidate - - def has_offcenter_radar_depart_conflict(self, sm): - if float(getattr(sm["carState"], "vEgo", 0.0)) > RADAR_DEPART_CONFLICT_MAX_EGO_SPEED: - return False - - centered_model_lead = self.get_centered_model_lead(sm["modelV2"]) - if centered_model_lead is None: - return False - - centered_model_dist = float(centered_model_lead[0]) - for lead in (self.lead_one, self.lead_two): - if not lead.status or not bool(getattr(lead, "radar", False)): - continue - - lead_dist = float(getattr(lead, "dRel", 0.0)) - if lead_dist <= 0.0 or lead_dist > RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE: - continue - if abs(float(getattr(lead, "yRel", 0.0))) < RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL: - continue - if abs(lead_dist - centered_model_dist) > RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH: - continue - - return True - - return False - - def get_lead_depart_accel_floor(self, lead, v_ego, model_desired_accel): - if lead is None or not lead.status: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: - return None - - lead_speed = max(float(lead.vLead), 0.0) + lead = min(leads, key=lambda item: float(item.dRel)) lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - lead_delta = lead_speed - float(v_ego) - confident_depart = self.is_confident_lead_depart(lead, v_ego) - min_lead_speed = LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED if confident_depart else LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED - min_lead_delta = LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA if confident_depart else LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_DELTA - min_gap = LEAD_DEPART_CONFIDENT_MIN_GAP if confident_depart else LEAD_DEPART_ACCEL_HOLD_MIN_GAP - min_model_accel = 0.0 if confident_depart else LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL - if ( - float(v_ego) > LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED or - lead_speed < min_lead_speed or - lead_delta < min_lead_delta or - float(lead.dRel) < min_gap or - lead_brake > LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE or - float(model_desired_accel) < min_model_accel - ): - return None - - gap_factor = float(np.clip((float(lead.dRel) - LEAD_DEPART_ACCEL_HOLD_MIN_GAP) / - max(LEAD_DEPART_ACCEL_HOLD_FULL_GAP - LEAD_DEPART_ACCEL_HOLD_MIN_GAP, 0.1), 0.0, 1.0)) - lead_factor = float(np.clip((lead_speed - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED) / - max(LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED, 0.1), 0.0, 1.0)) - accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip( - 0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0) - assisted_model_accel = float(model_desired_accel) + LEAD_DEPART_ACCEL_ASSIST - return min(accel_cap, max(assisted_model_accel, LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL)) - - def get_reusable_lead_depart_accel_floor(self, lead, v_ego, t_follow): - if self.lead_depart_accel_hold_floor is None or lead is None or not lead.status: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: - return None - - if abs(float(getattr(lead, "yRel", 0.0))) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET: - return None - - d_rel = float(getattr(lead, "dRel", 0.0)) - if d_rel < LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_GAP: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_LEAD_BRAKE: - return None - - closing_speed = max(float(v_ego) - float(getattr(lead, "vLead", 0.0)), 0.0) - if closing_speed > LEAD_DEPART_ACCEL_HOLD_REUSE_MAX_CLOSING_SPEED: - return None - - actual_headway = d_rel / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(t_follow) - if headway_margin < LEAD_DEPART_ACCEL_HOLD_REUSE_MIN_HEADWAY_MARGIN: - return None - - return float(self.lead_depart_accel_hold_floor) - - def get_low_speed_weak_lead_accel_cap(self, lead, v_ego): - if lead is None or not lead.status: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB: - return None - - d_rel = float(getattr(lead, "dRel", 0.0)) - lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) - lead_delta = lead_speed - float(v_ego) - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - lead_lateral = abs(float(getattr(lead, "yRel", 0.0))) - if ( - float(v_ego) > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED or - d_rel > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE or - lead_speed > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED or - lead_lateral > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET or - lead_delta > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA or - lead_accel > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL - ): - return None - - if ( - float(v_ego) <= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED and - d_rel >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP and - lead_speed >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED and - lead_delta >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA and - lead_accel >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL - ): - return None - - distance_factor = float(np.clip( - (d_rel - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE) / - max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE, 0.1), - 0.0, 1.0, - )) - delta_factor = float(np.clip( - (lead_delta - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA) / - max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA, 0.1), - 0.0, 1.0, - )) - accel_factor = float(np.clip( - (lead_accel - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL) / - max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL, 0.1), - 0.0, 1.0, - )) - cap_strength = float(np.clip(0.5 * distance_factor + 0.3 * delta_factor + 0.2 * accel_factor, 0.0, 1.0)) - return LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL + ( - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL - ) * cap_strength - - def get_standstill_stopped_lead_guard_cap(self, lead, v_ego, accel_min, stop_distance, - release_ready, confident_depart_ready): - if lead is None or not lead.status or release_ready or confident_depart_ready: - return None - if float(v_ego) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB: - return None - - if abs(float(getattr(lead, "yRel", 0.0))) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET: - return None - - lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) - lead_delta = lead_speed - float(v_ego) - max_distance = max( - STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE, - float(stop_distance) + STANDSTILL_STOPPED_LEAD_GUARD_DISTANCE_MARGIN, + candidate = bool( + float(lead.vLead) >= DEPART_LEAD_MIN_SPEED and + float(lead.dRel) >= stop_buffer + DEPART_LEAD_MIN_GAP and + lead_brake <= DEPART_LEAD_MAX_BRAKE ) - if ( - float(getattr(lead, "dRel", float("inf"))) > max_distance or - lead_speed > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED or - lead_delta > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA - ): - return None + return candidate, candidate and float(lead.vLead) >= DEPART_LEAD_FAST_SPEED - distance_factor = float(np.clip((max_distance - float(lead.dRel)) / - max(max_distance - STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE, 0.1), - 0.0, 1.0)) - speed_factor = float(np.clip(lead_speed / max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED, 0.1), 0.0, 1.0)) - delta_factor = float(np.clip((STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA - lead_delta) / - max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA, 0.1), - 0.0, 1.0)) - hold_brake = STANDSTILL_STOPPED_LEAD_GUARD_MIN_BRAKE + 0.06 * distance_factor + 0.02 * speed_factor + 0.02 * delta_factor - hold_brake = float(np.clip( - hold_brake, - STANDSTILL_STOPPED_LEAD_GUARD_MIN_BRAKE, - STANDSTILL_STOPPED_LEAD_GUARD_MAX_BRAKE, - )) - brake_floor = -hold_brake - return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor) + def _update_state(self, sm, v_ego, stop_request, safe_departure): + if stop_request: + self.stop_release_time = 0.0 + self.state = PlanState.stopped if v_ego <= STOPPED_SPEED else PlanState.stopping + return - def post_departure_follow_settle_active(self, lead, v_ego, t_follow): - if lead is None or not lead.status: - return False - if float(v_ego) < POST_DEPARTURE_FOLLOW_SETTLE_MIN_SPEED: - return False + if self.state in (PlanState.stopping, PlanState.stopped): + if safe_departure: + self.state = PlanState.departing + self.stop_release_time = 0.0 + else: + self.stop_release_time += self.dt + if self.stop_release_time >= STOP_RELEASE_TIME and v_ego > STOPPED_SPEED: + self.state = PlanState.moving + return - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < POST_DEPARTURE_FOLLOW_SETTLE_MIN_MODEL_PROB: - return False + if self.state == PlanState.departing and v_ego >= DEPART_COMPLETE_SPEED: + self.state = PlanState.moving + elif self.state == PlanState.departing and not safe_departure: + self.state = PlanState.stopped if v_ego <= STOPPED_SPEED else PlanState.stopping - if abs(float(getattr(lead, "yRel", 0.0))) > POST_DEPARTURE_FOLLOW_SETTLE_MAX_LATERAL_OFFSET: - return False - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(t_follow) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) - now = time.monotonic() - if now > self.post_departure_follow_settle_until: - self.post_departure_follow_settle_until = 0.0 - lead_delta = float(lead.vLead) - float(v_ego) - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - # Rolling launches can miss the standstill release edge that normally arms - # this latch. Confirm the same safe pull-away state from lead motion instead. - rolling_departure = ( - float(v_ego) <= POST_DEPARTURE_FOLLOW_SETTLE_MAX_ARM_SPEED and - lead_delta >= POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_LEAD_DELTA and - lead_accel >= POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_LEAD_ACCEL and - headway_margin >= POST_DEPARTURE_FOLLOW_SETTLE_ARM_MIN_HEADWAY_MARGIN - ) - if not rolling_departure: - return False - self.post_departure_follow_settle_until = now + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME - - if ( - headway_margin <= POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN or - lead_brake > POST_DEPARTURE_FOLLOW_SETTLE_MAX_LEAD_BRAKE or - closing_speed > POST_DEPARTURE_FOLLOW_SETTLE_MAX_CLOSING_SPEED - ): - self.post_departure_follow_settle_until = 0.0 - return False - - return True - - @staticmethod - def get_follow_accel_cap_allowance(lead, v_ego, t_follow): - if lead is None or not lead.status or float(v_ego) < FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED: - return 0.0 - - closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(t_follow) - if (headway_margin <= FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN or - closing_speed >= FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED or - lead_brake >= FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE): - return 0.0 - - headway_factor = float(np.clip( - (headway_margin - FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN) / - (FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_HEADWAY_MARGIN - FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_HEADWAY_MARGIN), - 0.0, - 1.0, - )) - closing_factor = float(np.clip( - (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED - closing_speed) / - (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_CLOSING_SPEED - FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_CLOSING_SPEED), - 0.0, - 1.0, - )) - brake_factor = float(np.clip( - (FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE - lead_brake) / - FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_LEAD_BRAKE, - 0.0, - 1.0, - )) - return FOLLOW_ACCEL_CAP_ALLOWANCE_MAX_ACCEL * headway_factor * closing_factor * brake_factor - - def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow, current_source=None, tracking_lead_active=False): - if lead is None or not lead.status: - return None - - if self.post_departure_follow_settle_active(lead, v_ego, t_follow): - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - low_speed_follow_window = ( - not lead_radar and - v_ego <= LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED and - lead_prob >= LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB and - lead_brake <= LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE - ) - if v_ego < LEAD_CATCHUP_ACCEL_MIN_EGO and not low_speed_follow_window: - return None - - lead_delta = float(lead.vLead) - float(v_ego) - min_lead_delta = LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_LEAD_DELTA if low_speed_follow_window else LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - if low_speed_follow_window: - gap_buffer = max(LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_MIN, - LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego)) + def _update_allow_throttle(self, throttle_prob, v_ego): + if v_ego <= MIN_ALLOW_THROTTLE_SPEED: + self.allow_throttle = True + self._throttle_transition_time = 0.0 else: - gap_buffer = max(LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN, - LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN * float(v_ego)) - gap_error = float(lead.dRel) - desired_gap - - radar_matched_follow_active = ( - lead_radar and - tracking_lead_active and - self.lead_is_matched_follow_window(lead, v_ego, t_follow) - ) - if radar_matched_follow_active and gap_error > (gap_buffer - RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN): - return None - - if (radar_matched_follow_active and current_source == "cruise" and - gap_error <= RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR and - lead_delta < min_lead_delta): - return RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP - - if lead_delta < min_lead_delta: - return None - - if gap_error > gap_buffer: - return None - - # Keep the near-target cap conservative, then continuously relax it when - # there is real headway to use. Avoid binary bypasses around zero vRel. - if low_speed_follow_window: - edge_cap = float(np.interp(lead_delta, [min_lead_delta, 0.0, 1.0, 2.0], [0.20, 0.24, 0.38, 0.55])) - near_cap = min(edge_cap, 0.16) - else: - edge_cap = float(np.interp(lead_delta, [-0.5, 0.0, 1.0], [0.16, 0.08, 0.02])) - near_cap = min(edge_cap, 0.03) - gap_factor = float(np.clip(max(gap_error, 0.0) / max(gap_buffer, 0.1), 0.0, 1.0)) - cap = float(np.interp(gap_factor, [0.0, 1.0], [near_cap, edge_cap])) - allowance_factor = float(np.clip(gap_error / FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN, 0.0, 1.0)) - cap += allowance_factor * self.get_follow_accel_cap_allowance(lead, v_ego, t_follow) - if not low_speed_follow_window: - entry_factor = float(np.clip( - (float(v_ego) - LEAD_CATCHUP_ACCEL_MIN_EGO) / - (RADAR_CATCHUP_ACCEL_CAP_ENTRY_FULL_SPEED - LEAD_CATCHUP_ACCEL_MIN_EGO), - 0.0, - 1.0, - )) - cap = float(np.interp(entry_factor, [0.0, 1.0], [RADAR_CATCHUP_ACCEL_CAP_ENTRY_MAX, cap])) - return cap - - def get_low_speed_follow_transition_brake_cap(self, lead, v_ego, t_follow, prev_output_a_target, output_a_target): - if lead is None or not lead.status: - return None - if bool(getattr(lead, "radar", False)): - return None - if not (LOW_SPEED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) <= LOW_SPEED_FOLLOW_TRANSITION_MAX_SPEED): - return None - if prev_output_a_target <= LOW_SPEED_FOLLOW_TRANSITION_PREV_ACCEL_MIN: - return None - if output_a_target >= LOW_SPEED_FOLLOW_TRANSITION_TARGET_BRAKE_MIN: - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < LOW_SPEED_FOLLOW_TRANSITION_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > LOW_SPEED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE: - return None - - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - if closing_speed > LOW_SPEED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED: - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - if float(lead.dRel) < desired_gap + LOW_SPEED_FOLLOW_TRANSITION_MIN_GAP_MARGIN: - return None - - cap_decel = float(np.interp( - closing_speed, - [0.0, LOW_SPEED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED], - [LOW_SPEED_FOLLOW_TRANSITION_MIN_BRAKE, LOW_SPEED_FOLLOW_TRANSITION_MAX_BRAKE], - )) - return -cap_decel - - def get_cruise_tracking_lead_accel_cap(self, lead, v_ego, t_follow, current_source, tracking_lead_active): - if lead is None or not lead.status or current_source != "cruise": - return None - if self.post_departure_follow_settle_active(lead, v_ego, t_follow): - return None - if not (CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED): - return None - - lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) - if not bool(getattr(lead, "radar", False)) and lead_prob < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE and not tracking_lead_active: - return None - - if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET: - return None - - lead_delta = float(lead.vLead) - float(v_ego) - if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED: - return None - - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - if ( - float(v_ego) < FOLLOW_ACCEL_CAP_ALLOWANCE_MIN_SPEED and - lead_delta >= 0.35 and - lead_accel >= 0.25 - ): - return None - - closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) - raw_close_lead = self.raw_close_lead_needs_control(lead, v_ego) - unresolved_slow_lead = ( - closing_speed >= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED and - lead_delta <= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA - ) - if not tracking_lead_active and not raw_close_lead and not unresolved_slow_lead: - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - gap_error = float(lead.dRel) - desired_gap - gap_buffer = max(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN, - CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego)) - if gap_error > gap_buffer: - return None - - base_cap = float(np.interp( - lead_delta, - [-1.5, -0.5, 0.0, 0.5, CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED], - [0.0, 0.04, 0.08, 0.12, 0.16], - )) - - if raw_close_lead: - base_cap = min(base_cap, float(np.interp(closing_speed, [0.5, 1.5, 3.5], [0.10, 0.05, 0.0]))) - else: - base_cap = min(base_cap, float(np.interp(closing_speed, [0.0, 1.0, 2.0], [0.18, 0.12, 0.06]))) - - gap_factor = float(np.clip(gap_error / max(gap_buffer, 0.1), 0.0, 1.0)) - cap = min(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL, base_cap + 0.06 * gap_factor) - allowance_factor = float(np.clip(gap_error / FOLLOW_ACCEL_CAP_ALLOWANCE_FULL_GAP_MARGIN, 0.0, 1.0)) - cap += allowance_factor * self.get_follow_accel_cap_allowance(lead, v_ego, t_follow) - return max(0.0, cap) - - def get_cruise_tracking_lead_accel_transition_target(self, lead, v_ego, t_follow, - prev_output_a_target, output_a_target, - current_source): - if lead is None or not lead.status or current_source != "cruise": - return None - if not (CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_SPEED): - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if target_delta < CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_DELTA_A: - return None - - lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) - if not bool(getattr(lead, "radar", False)) and lead_prob < CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LEAD_BRAKE: - return None - - if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_LATERAL_OFFSET: - return None - - lead_delta = float(lead.vLead) - float(v_ego) - if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(t_follow) - if headway_margin > CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_HEADWAY_ABOVE_TARGET: - return None - - positive_step = float(np.interp( - lead_delta, - [-1.0, 0.0, 1.0, CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED], - [CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_STEP, - 0.08, - 0.12, - CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_STEP], - )) - if headway_margin > 0.0: - headway_factor = float(np.clip( - headway_margin / max(CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_HEADWAY_ABOVE_TARGET, 1e-3), - 0.0, - 1.0, - )) - positive_step = float(np.interp( - headway_factor, - [0.0, 1.0], - [positive_step, CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_STEP], - )) - - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(min(output_a_target, upper)) - return smoothed_target if smoothed_target < float(output_a_target) - 1e-6 else None - - def lead_is_matched_follow_window(self, lead, v_ego, base_t_follow): - if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: - return False - - relative_speed = float(v_ego) - float(lead.vLead) - if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED): - return False - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE: - return False - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and not is_radarless_matched_follow_window( - v_ego, - lead.dRel, - lead.vLead, - base_t_follow, - radar=lead_radar, - lead_brake=lead_brake, - lead_prob=lead_prob, - ): - return False - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway < max(STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY, float(base_t_follow) - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET): - return False - if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: - return False - return True - - def get_matched_follow_control_lead(self, v_ego, t_follow): - if self.mpc.source == 'lead1' and self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow): - return self.lead_two - if self.lead_is_matched_follow_window(self.lead_one, v_ego, t_follow): - return self.lead_one - if self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow): - return self.lead_two - return None - - def get_follow_control_lead(self, lead_control_active, v_ego, t_follow, *, allow_optional_far_lead_logic=True): - if allow_optional_far_lead_logic: - matched_follow_lead = self.get_matched_follow_control_lead(v_ego, t_follow) - if matched_follow_lead is not None: - return matched_follow_lead - - if not lead_control_active: - return None - - if self.lead_one.status: - return self.lead_one - if self.lead_two.status: - return self.lead_two - return None - - def get_duplicate_vision_comfort_lead(self, v_ego): - if not (self.lead_one.status and self.lead_two.status): - self.duplicate_vision_comfort_lead_source = None - return None - - if bool(getattr(self.lead_one, "radar", False)) or bool(getattr(self.lead_two, "radar", False)): - self.duplicate_vision_comfort_lead_source = None - return None - - if not self.mpc.leads_are_near_duplicates( - self.lead_one, - self.lead_two, - v_ego, - vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED, - ): - self.duplicate_vision_comfort_lead_source = None - return None - - lead_sources = { - "lead0": self.lead_one, - "lead1": self.lead_two, - } - latched_lead = lead_sources.get(self.duplicate_vision_comfort_lead_source) - if latched_lead is not None and latched_lead.status: - return latched_lead - - min_center_offset = min(abs(float(getattr(lead, "yRel", 0.0))) for lead in lead_sources.values()) - centered_sources = [ - (source, lead) for source, lead in lead_sources.items() - if abs(float(getattr(lead, "yRel", 0.0))) <= min_center_offset + DUPLICATE_VISION_COMFORT_LEAD_CENTER_TIE_MARGIN - ] - selected_source, selected_lead = min( - centered_sources, - key=lambda item: ( - float(item[1].dRel), - float(item[1].vLead), - -max(0.0, -float(getattr(item[1], "aLeadK", 0.0))), - abs(float(getattr(item[1], "yRel", 0.0))), - ), - ) - self.duplicate_vision_comfort_lead_source = selected_source - return selected_lead - - def is_nonurgent_duplicate_vision_follow(self, v_ego, t_follow): - if bool(getattr(self.lead_one, "radar", False)) or bool(getattr(self.lead_two, "radar", False)): - return False - if not self.mpc.leads_are_near_duplicates( - self.lead_one, - self.lead_two, - v_ego, - vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED, - ): - return False - - lead = self.lead_one - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - minimum_headway = max( - UNCERT_DUPLICATE_VISION_MIN_HEADWAY, - float(t_follow) - UNCERT_DUPLICATE_VISION_HEADWAY_BELOW_TARGET, - ) - return ttc > UNCERT_DUPLICATE_VISION_MIN_TTC and actual_headway > minimum_headway - - def lead_is_spacious_brake_cap_window(self, lead, v_ego, base_t_follow): - if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: - return False - - relative_speed = float(v_ego) - float(lead.vLead) - if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED): - return False - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MAX_LEAD_BRAKE: - return False - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB: - return False - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway < max(float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_HEADWAY_MARGIN, - STEADY_FOLLOW_BRAKE_CAP_SPACIOUS_MIN_HEADWAY): - return False - if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: - return False - return True - - def get_matched_follow_brake_cap(self, lead, v_ego, base_t_follow): - if not ( - self.lead_is_matched_follow_window(lead, v_ego, base_t_follow) or - self.lead_is_spacious_brake_cap_window(lead, v_ego, base_t_follow) - ): - return None - - relative_speed = float(v_ego) - float(lead.vLead) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - cap_decel = float(np.interp( - relative_speed, - [STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED, 0.0, STEADY_FOLLOW_BRAKE_CAP_MAX_CLOSING_SPEED], - [STEADY_FOLLOW_BRAKE_CAP_OPENING_DECEL, - STEADY_FOLLOW_BRAKE_CAP_ZERO_REL_SPEED_DECEL, - STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL], - )) - headway_deficit = float(np.clip((float(base_t_follow) - actual_headway) / STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET, 0.0, 1.0)) - cap_decel = min(STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL, cap_decel + 0.05 * headway_deficit) - return -cap_decel - - def get_far_lead_brake_cap(self, lead, v_ego, base_t_follow): - if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: - return None - - relative_speed = float(v_ego) - float(lead.vLead) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - - if bool(getattr(lead, "radar", False)): - if not (FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED <= relative_speed <= FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED): - return None - if lead_brake > FAR_RADAR_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE: - return None - if float(lead.dRel) < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DISTANCE or headway_margin < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN: - return None - - ttc = float(lead.dRel) / max(relative_speed, 1e-3) - if ttc < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_TTC: - return None - - cap_decel = float(np.interp( - relative_speed, - [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED, FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED], - [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DECEL, FAR_RADAR_COMFORT_BRAKE_CAP_MAX_DECEL], - )) - relax_decel = float(np.interp( - headway_margin, - [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN, FAR_RADAR_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN], - [0.0, FAR_RADAR_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL], - )) - return -max(0.0, cap_decel - relax_decel) - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_MODEL_PROB: - return None - - if not (FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED <= relative_speed <= FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED): - return None - - if lead_brake > FAR_LEAD_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE: - return None - - if float(lead.dRel) < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DISTANCE or headway_margin < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN: - return None - - ttc = float(lead.dRel) / max(relative_speed, 1e-3) - if ttc < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_TTC: - return None - - cap_decel = float(np.interp( - relative_speed, - [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED, FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED], - [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL, FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL], - )) - relax_decel = float(np.interp( - headway_margin, - [FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN, FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN], - [0.0, FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL], - )) - return -max(0.0, cap_decel - relax_decel) - - def update_experimental_release_accel_state(self, experimental_mode, now_t, v_ego=None): - if self.prev_experimental_mode is True and not experimental_mode: - low_speed_release = v_ego is not None and float(v_ego) < EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_THRESHOLD - hold_time = EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_HOLD_TIME if low_speed_release else EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME - self.experimental_release_accel_until = now_t + hold_time - elif experimental_mode: - self.experimental_release_accel_until = 0.0 - self.prev_experimental_mode = bool(experimental_mode) - - def get_experimental_release_accel_target(self, lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - release_active): - if not release_active or lead is None or not lead.status or v_ego < EXPERIMENTAL_RELEASE_ACCEL_MIN_SPEED: - return None - - lead_speed = float(lead.vLead) - lead_delta = lead_speed - float(v_ego) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if lead_speed < EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_SPEED: - return None - if not (EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_DELTA <= lead_delta <= EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_DELTA): - return None - if lead_brake > EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_BRAKE: - return None - if not lead_radar and lead_prob < EXPERIMENTAL_RELEASE_ACCEL_MIN_MODEL_PROB: - return None - if abs(float(getattr(lead, "yRel", 0.0))) > EXPERIMENTAL_RELEASE_ACCEL_MAX_LATERAL_OFFSET: - return None - if actual_headway < float(base_t_follow) + EXPERIMENTAL_RELEASE_ACCEL_MIN_HEADWAY_MARGIN: - return None - if float(output_a_target) - float(prev_output_a_target) < EXPERIMENTAL_RELEASE_ACCEL_MIN_DELTA_A: - return None - - return min(float(output_a_target), float(prev_output_a_target) + EXPERIMENTAL_RELEASE_ACCEL_STEP) - - def get_matched_follow_transition_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - low_speed_extension_active = ( - bool(tracking_lead_active) and - current_source == "cruise" and - LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED - ) - if float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED and not low_speed_extension_active: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 0.0)) - min_model_prob = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB - if lead_prob < min_model_prob: - return None - - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - lead_brake = max(0.0, -lead_accel) - max_lead_brake = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE - if lead_brake > max_lead_brake: - return None - - relative_speed = float(v_ego) - float(lead.vLead) - opening_radar_transition = bool( - lead_radar and - -relative_speed >= LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_LEAD_DELTA and - -relative_speed <= CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED and - lead_accel >= 0.0 - ) - relative_speed_in_range = STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED - if not relative_speed_in_range and not opening_radar_transition: - return None - - closing_speed = max(0.0, relative_speed) - max_closing_speed = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED - if closing_speed > max_closing_speed: - return None - - ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - min_ttc = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_TTC - if ttc < min_ttc: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN - opening_radar_lead = bool( - low_speed_extension_active and - lead_radar and - opening_radar_transition - ) - if opening_radar_lead: - min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN - full_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN - if headway_margin < min_headway_margin: - return None - if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET: - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if low_speed_extension_active: - if float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET: - return None - if float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET: - return None - if abs(target_delta) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A: - return None - elif abs(target_delta) < 1e-3: - return None - - headway_factor = float(np.clip( - (headway_margin - min_headway_margin) / - max(full_headway_margin - min_headway_margin, 1e-3), - 0.0, - 1.0, - )) - - min_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP - max_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP - positive_step = float(np.interp( - max(float(lead.vLead) - float(v_ego), 0.0), - [0.0, 1.0], - [min_positive_step, max_positive_step], - )) - min_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP - max_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP - negative_step = float(np.interp( - closing_speed, - [0.0, max_closing_speed], - [min_negative_step, max_negative_step], - )) - - # The more space we still have, the less abrupt the comfort path should be. - positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, min_positive_step])) - negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, min_negative_step])) - - stable_radar_catchup = bool( - lead_radar and - relative_speed <= MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_CLOSING_SPEED and - lead_accel >= 0.0 and - headway_margin >= LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN - ) - if stable_radar_catchup: - positive_step = min(positive_step, MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP) - negative_step = min(negative_step, MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP) - - if float(prev_output_a_target) * float(output_a_target) < 0.0: - sign_cross_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP - positive_step = min(positive_step, sign_cross_step) - negative_step = min(negative_step, sign_cross_step) - - lower = float(prev_output_a_target) - negative_step - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(np.clip(output_a_target, lower, upper)) - return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None - - def get_near_duplicate_lead_transition_target(self, lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - if current_source not in ("cruise", "lead0", "lead1"): - return None - if current_source == "cruise" and not tracking_lead_active: - return None - if not (self.lead_one.status and self.lead_two.status): - return None - identical_radar_duplicates = self.mpc.leads_share_identical_radar_track(self.lead_one, self.lead_two) - if not self.mpc.leads_are_near_duplicates( - self.lead_one, - self.lead_two, - v_ego, - vision_min_speed=LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED, - ): - return None - low_speed_extension_active = bool( - tracking_lead_active and - current_source in ("cruise", "lead0", "lead1") and - LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED - ) - if float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED: - return None - if float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED and not low_speed_extension_active: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE: - return None - - relative_speed = float(v_ego) - float(lead.vLead) - closing_speed = max(0.0, relative_speed) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway < max(0.0, float(base_t_follow) - NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET): - return None - max_headway_above_target = NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET - if low_speed_extension_active and identical_radar_duplicates: - max_headway_above_target += LOW_SPEED_IDENTICAL_RADAR_DUPLICATE_TRANSITION_EXTRA_HEADWAY - elif low_speed_extension_active and not lead_radar: - max_headway_above_target += LOW_SPEED_DUPLICATE_VISION_TRANSITION_EXTRA_HEADWAY - if actual_headway > float(base_t_follow) + max_headway_above_target: - return None - - max_closing_speed = NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED - if ( - not identical_radar_duplicates and - not lead_radar and - actual_headway >= float(base_t_follow) + NEAR_DUPLICATE_VISION_TRANSITION_MIN_HEADWAY_MARGIN - ): - max_closing_speed += NEAR_DUPLICATE_VISION_TRANSITION_EXTRA_CLOSING_SPEED - if closing_speed > max_closing_speed: - return None - - ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - if ttc < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC: - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if abs(target_delta) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A: - return None - - if low_speed_extension_active and (float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET or - float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET): - return None - - positive_step = NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP - negative_step = NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP - if float(prev_output_a_target) * float(output_a_target) < 0.0: - positive_step = min(positive_step, NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP) - negative_step = min(negative_step, NEAR_DUPLICATE_LEAD_TRANSITION_SIGN_CROSS_STEP) - - lower = float(prev_output_a_target) - negative_step - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(np.clip(output_a_target, lower, upper)) - return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None - - def get_mild_follow_zero_cross_guard_target(self, lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - if current_source not in ("cruise", "lead0", "lead1") and not tracking_lead_active: - return None - if not (MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_SPEED <= float(v_ego) <= MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_SPEED): - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if abs(target_delta) < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DELTA_A: - return None - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_MODEL_PROB: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_LEAD_BRAKE: - return None - - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - if closing_speed > MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_CLOSING_SPEED: - return None - - ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - if ttc < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_TTC: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - if headway_margin < MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_HEADWAY_MARGIN: - return None - - deadband = float(np.interp( - headway_margin, - [MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_HEADWAY_MARGIN, MILD_FOLLOW_ZERO_CROSS_GUARD_FULL_HEADWAY_MARGIN], - [MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DEADBAND, MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_DEADBAND], - )) - - if float(prev_output_a_target) * float(output_a_target) < 0.0: - return 0.0 - - if abs(float(output_a_target)) < deadband and abs(float(prev_output_a_target)) < deadband + 0.06: - return 0.0 - - return None - - @staticmethod - def get_far_opening_radar_brake_guard_target(lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - v_cruise, model_desired_accel, - current_source, tracking_lead_active): - if lead is None or not lead.status or not bool(getattr(lead, "radar", False)): - return None - if current_source != "cruise" or not tracking_lead_active: - return None - if float(v_ego) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_SPEED: - return None - if float(lead.dRel) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_DISTANCE: - return None - - lead_prob = float(getattr(lead, "modelProb", 1.0)) - lead_delta = float(lead.vLead) - float(v_ego) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) - if ( - lead_prob < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_PROB or - lead_delta < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_LEAD_DELTA or - lead_brake > FAR_OPENING_RADAR_BRAKE_GUARD_MAX_LEAD_BRAKE or - headway_margin < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_HEADWAY_MARGIN - ): - return None - - if float(prev_output_a_target) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_PREV_TARGET: - return None - if float(output_a_target) > FAR_OPENING_RADAR_BRAKE_GUARD_MAX_NEW_TARGET: - return None - if float(v_cruise) < float(v_ego) - FAR_OPENING_RADAR_BRAKE_GUARD_MAX_CRUISE_DEFICIT: - return None - if float(model_desired_accel) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_ACCEL: - return None - - # A far lead that is pulling away cannot justify a one-frame braking pulse. - # Preserve real speed-target, model, and closing-lead deceleration above. - return 0.0 - - def get_duplicate_slow_lead_brake_hold_target(self, lead, v_ego, base_t_follow, - prev_output_a_target, output_a_target, - current_source, tracking_lead_active): - if lead is None or not lead.status: - return None - if current_source not in ("cruise", "lead0", "lead1") and not tracking_lead_active: - return None - if not (self.lead_one.status and self.lead_two.status): - return None - if ( - abs(float(self.lead_one.dRel) - float(self.lead_two.dRel)) > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_DREL_DIFF or - abs(float(self.lead_one.vRel) - float(self.lead_two.vRel)) > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_VREL_DIFF - ): - return None - if float(v_ego) < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_SPEED: - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if bool(getattr(lead, "radar", False)) or lead_prob < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_MODEL_PROB: - return None - - prev_brake = max(0.0, -float(prev_output_a_target)) - if prev_brake < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_PREV_DECEL: - return None - - target_delta = float(output_a_target) - float(prev_output_a_target) - if target_delta < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_DELTA_A: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_LEAD_BRAKE: - return None - - closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) - if closing_speed < DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED: - return None - - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - if actual_headway > float(base_t_follow) + DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_HEADWAY_ABOVE_TARGET: - return None - - positive_step = float(np.interp( - closing_speed, - [DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_CLOSING_SPEED, 1.5, 4.0, 8.0], - [DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MIN_POSITIVE_STEP, - 0.16, - 0.22, - DUPLICATE_SLOW_LEAD_BRAKE_HOLD_MAX_POSITIVE_STEP], - )) - if float(prev_output_a_target) * float(output_a_target) < 0.0: - positive_step = min(positive_step, DUPLICATE_SLOW_LEAD_BRAKE_HOLD_SIGN_CROSS_STEP) - - upper = float(prev_output_a_target) + positive_step - smoothed_target = float(min(float(output_a_target), upper)) - return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None - - def get_tracked_vision_model_brake_floor(self, lead, v_ego, accel_min, t_follow, model_desired): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - if float(v_ego) < TRACKED_VISION_MODEL_FLOOR_MIN_SPEED: - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB: - return None - - model_brake = max(0.0, -float(model_desired)) - if model_brake < TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - projected_closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) + lead_brake * reaction_t - if projected_closing_speed < TRACKED_VISION_MODEL_FLOOR_MIN_CLOSING_SPEED: - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - gap_margin = float(lead.dRel) - desired_gap - max_gap_margin = max(TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_MIN, - TRACKED_VISION_MODEL_FLOOR_MAX_GAP_BUFFER_GAIN * float(v_ego)) - if gap_margin < TRACKED_VISION_MODEL_FLOOR_MIN_GAP_MARGIN or gap_margin > max_gap_margin: - return None - - projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) - if projected_ttc > TRACKED_VISION_MODEL_FLOOR_MAX_TTC: - return None - - floor_decel = float(np.interp( - model_brake, - [TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL, 1.6], - [TRACKED_VISION_MODEL_FLOOR_MIN_DECEL, TRACKED_VISION_MODEL_FLOOR_MAX_DECEL], - )) - floor_decel += float(np.interp(lead_brake, [0.0, 0.8], [0.0, TRACKED_VISION_MODEL_FLOOR_LEAD_BRAKE_MAX])) - return max(accel_min, -min(TRACKED_VISION_MODEL_FLOOR_MAX_DECEL, floor_decel)) - - def get_tracked_vision_model_brake_cap(self, lead, v_ego, t_follow, model_desired): - if lead is None or not lead.status or bool(getattr(lead, "radar", False)): - return None - if not (TRACKED_VISION_MODEL_CAP_MIN_SPEED <= float(v_ego) <= TRACKED_VISION_MODEL_CAP_MAX_SPEED): - return None - - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if lead_prob < TRACKED_VISION_MODEL_CAP_MIN_MODEL_PROB: - return None - - model_brake = max(0.0, -float(model_desired)) - if model_brake > TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL: - return None - - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > TRACKED_VISION_MODEL_CAP_MAX_LEAD_BRAKE: - return None - - reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) - projected_closing_speed = max(0.0, float(v_ego) - float(lead.vLead)) + lead_brake * reaction_t - if not (TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED <= projected_closing_speed <= TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED): - return None - - desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) - gap_margin = float(lead.dRel) - desired_gap - max_gap_margin = max(TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_MIN, - TRACKED_VISION_MODEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego)) - if gap_margin < TRACKED_VISION_MODEL_CAP_MIN_GAP_MARGIN or gap_margin > max_gap_margin: - return None - - projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1) - if projected_ttc < TRACKED_VISION_MODEL_CAP_MIN_TTC: - return None - - cap_decel = float(np.interp( - model_brake, - [0.0, TRACKED_VISION_MODEL_CAP_MAX_MODEL_DECEL], - [TRACKED_VISION_MODEL_CAP_MIN_DECEL, TRACKED_VISION_MODEL_CAP_MAX_DECEL], - )) - cap_decel += float(np.interp( - projected_closing_speed, - [TRACKED_VISION_MODEL_CAP_MIN_CLOSING_SPEED, TRACKED_VISION_MODEL_CAP_MAX_CLOSING_SPEED], - [0.0, 0.08], - )) - return -min(TRACKED_VISION_MODEL_CAP_MAX_DECEL, cap_decel) - - @staticmethod - def raw_close_lead_needs_control(lead, v_ego): - if lead is None or not lead.status: - return False - - d_rel = max(float(lead.dRel), 0.0) - lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) - closing_speed = float(v_ego - lead.vLead) - lead_braking = float(lead.aLeadK) < -0.5 - centered_lead = abs(float(getattr(lead, "yRel", 0.0))) <= RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET - if ( - centered_lead and - float(v_ego) <= RAW_LEAD_LOW_SPEED_HOLD_MAX_EGO_SPEED and - lead_speed <= RAW_LEAD_LOW_SPEED_HOLD_MAX_LEAD_SPEED and - d_rel <= RAW_LEAD_LOW_SPEED_HOLD_MAX_DISTANCE and - closing_speed >= RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED - ): - return True - - if closing_speed <= RAW_LEAD_SAFETY_MIN_CLOSING_SPEED and not lead_braking: - return False - - dynamic_distance = max(RAW_LEAD_SAFETY_DISTANCE, 3.0 * float(v_ego)) - ttc = d_rel / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") - return d_rel < dynamic_distance and (ttc < RAW_LEAD_SAFETY_TTC or lead_braking) + lower = ALLOW_THROTTLE_THRESHOLD - ALLOW_THROTTLE_HYSTERESIS + upper = ALLOW_THROTTLE_THRESHOLD + ALLOW_THROTTLE_HYSTERESIS + requested = throttle_prob <= lower if self.allow_throttle else throttle_prob > upper + self._throttle_transition_time = self._throttle_transition_time + self.dt if requested else 0.0 + if self._throttle_transition_time >= ALLOW_THROTTLE_CONFIRM_TIME: + self.allow_throttle = not self.allow_throttle + self._throttle_transition_time = 0.0 + + def _smooth_output(self, target, urgent): + if urgent or target <= self.output_a_target - RAW_SAFETY_URGENT_DECEL: + return float(target) + lower = self.output_a_target - self.tune.brake_slew_rate * self.dt + upper = self.output_a_target + self.tune.accel_slew_rate * self.dt + return float(np.clip(target, lower, upper)) def update(self, sm, starpilot_toggles): - if self.is_preap: - self._preap_param_frame += 1 - if self._preap_params is not None and (self._preap_param_frame % 20) == 0: - self.nap_adaptive_accel = self._preap_params.get_bool("NAPAdaptiveAccel") - - self.generation = getattr(starpilot_toggles, "model_version", None) - experimental_mode = bool(sm['selfdriveState'].experimentalMode) - self.mode = 'blended' if experimental_mode else 'acc' - self.mpc.mode = 'acc' - if not self.mlsim: - self.mpc.mode = self.mode - - if len(sm['carControl'].orientationNED) == 3: - accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) - else: - accel_coast = ACCEL_MAX - - v_ego = get_planner_v_ego(self.CP, sm['carState']) - scene_v_ego = float(sm['carState'].vEgo) - v_cruise = sm['starpilotPlan'].vCruise + car_state = sm["carState"] + v_ego = get_planner_v_ego(self.CP, car_state) + scene_v_ego = float(car_state.vEgo) + v_cruise = float(sm["starpilotPlan"].vCruise) if not np.isfinite(v_cruise): - cloudlog.error(f"Longitudinal planner received non-finite vCruise={v_cruise}, falling back to v_ego={v_ego:.2f}") + cloudlog.error(f"Longitudinal planner received non-finite vCruise={v_cruise}") v_cruise = max(v_ego, 0.0) - v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET - long_control_off = sm['controlsState'].longControlState == LongCtrlState.off - force_slow_decel = sm['controlsState'].forceDecel + reset_state = ( + (sm["controlsState"].longControlState == LongCtrlState.off + if self.CP.openpilotLongitudinalControl else not sm["selfdriveState"].enabled) or + car_state.vCruise == V_CRUISE_UNSET + ) - # Reset current state when not engaged, or user is controlling the speed - reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled - # PCM cruise speed may be updated a few cycles later, check if initialized - reset_state = reset_state or not v_cruise_initialized - - # No change cost when user is controlling the speed, or when standstill - prev_accel_constraint = not (reset_state or sm['carState'].standstill) - - if self.mpc.mode == 'acc': - accel_limits = [sm['starpilotPlan'].minAcceleration, sm['starpilotPlan'].maxAcceleration] - steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) - accel_limits_turns[0] = max(get_vehicle_min_accel(self.CP, v_ego), accel_limits_turns[0]) - else: - accel_limits = [ACCEL_MIN, ACCEL_MAX] - accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] + accel_limits = [ + float(sm["starpilotPlan"].minAcceleration), + float(sm["starpilotPlan"].maxAcceleration), + ] + steer_angle = car_state.steeringAngleDeg - sm["liveParameters"].angleOffsetDeg + accel_limits = limit_accel_in_turns(v_ego, steer_angle, accel_limits, self.CP) + accel_limits[0] = max(get_vehicle_min_accel(self.CP, v_ego), accel_limits[0]) if reset_state: self.v_desired_filter.x = v_ego - # Clip aEgo to cruise limits to prevent large accelerations when becoming active - self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) - self.model_allow_throttle = True - self.model_allow_throttle_transition_t = 0.0 + self.a_desired = float(np.clip(car_state.aEgo, *accel_limits)) + self.output_a_target = self.a_desired + self.state = PlanState.moving + self.stop_release_time = 0.0 + self.departure_confirm_time = 0.0 - # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - # Compute model v_ego error - self.v_model_error = self.get_model_speed_error(sm['modelV2'], v_ego) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, starpilot_toggles) - if bool(sm['carState'].standstill): - self.model_launch_armed = True - self.model_launch_stop_seen |= bool( - sm['modelV2'].action.shouldStop or - getattr(sm['starpilotPlan'], 'redLight', False) or - getattr(sm['starpilotPlan'], 'forcingStop', False) - ) - elif scene_v_ego > MODEL_LAUNCH_DISARM_SPEED: - self.model_launch_armed = False - self.model_launch_stop_seen = False - model_launch_v = np.array(v, copy=True) - model_launch_a = np.array(a, copy=True) - # Don't clip at low speeds since throttle_prob doesn't account for creep. Raw - # gasPressProb can cross both hysteresis thresholds for only a few model frames, - # so confirm transitions before changing the physical coast cap. - if v_ego <= MIN_ALLOW_THROTTLE_SPEED: - self.model_allow_throttle = True - self.model_allow_throttle_transition_t = 0.0 - else: - transition_requested = ( - throttle_prob <= ALLOW_THROTTLE_DISABLE_THRESHOLD if self.model_allow_throttle - else throttle_prob > ALLOW_THROTTLE_ENABLE_THRESHOLD - ) - if transition_requested: - self.model_allow_throttle_transition_t += self.dt - if self.model_allow_throttle_transition_t + 1e-6 >= ALLOW_THROTTLE_TRANSITION_CONFIRM_TIME: - self.model_allow_throttle = not self.model_allow_throttle - self.model_allow_throttle_transition_t = 0.0 - else: - self.model_allow_throttle_transition_t = 0.0 - self.allow_throttle = self.model_allow_throttle and not sm['starpilotPlan'].disableThrottle + gas_probs = sm["modelV2"].meta.disengagePredictions.gasPressProbs + throttle_prob = float(gas_probs[1]) if len(gas_probs) > 1 else 1.0 + self._update_allow_throttle(throttle_prob, v_ego) if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) - # Hold the output cap to the physical coasting limit until throttle is - # allowed again. Relaxing back toward positive accel while the gate is - # still closed can stall downhill coastdown well above the target speed. - accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast) - no_throttle_output_max = accel_limits_turns[1] + pitch = sm["carControl"].orientationNED[1] if len(sm["carControl"].orientationNED) == 3 else 0.0 + coast_accel = get_coast_accel(pitch) + speed_guard = -COAST_TARGET_GAIN * (v_ego - COAST_TARGET_SPEED) + accel_limits[1] = min(accel_limits[1], max(coast_accel, speed_guard, accel_limits[0])) - if force_slow_decel: + if sm["controlsState"].forceDecel: v_cruise = 0.0 - # clip limits, cannot init MPC outside of bounds - accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) - accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - tracking_lead = bool(sm['starpilotPlan'].trackingLead) - self.lead_one = sm['radarState'].leadOne - self.lead_two = sm['radarState'].leadTwo - raw_close_lead_control = any(self.raw_close_lead_needs_control(lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) - # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead - # safety path so ACC/chill does not ignore a visible lead during that debounce. - lead_control_active = tracking_lead or raw_close_lead_control - lead_one_active = bool(self.lead_one.status and lead_control_active) - effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego) - - if self.is_preap and self.nap_adaptive_accel and lead_one_active: - follow_limit = get_preap_follow_limit(v_ego) - if follow_limit is not None: - safe_dist = get_safe_obstacle_distance(v_ego, effective_t_follow) - lead_dist_ratio = float(self.lead_one.dRel) / max(safe_dist, 1.0) - cap_strength = float(np.clip(1.0 - (lead_dist_ratio - 1.2) / 0.3, 0.0, 1.0)) - if cap_strength > 0.0: - accel_limits_turns[1] = min( - accel_limits_turns[1], - accel_limits_turns[1] * (1.0 - cap_strength) + follow_limit * cap_strength, - ) - - lead_dist = self.lead_one.dRel if lead_one_active else 50.0 - - # Smooth lead distance (EMA) to avoid chatter in thresholds - alpha = max(0.02, min(0.15, 0.05 + 0.002 * v_ego)) - if self.lead_dist_f is None: - self.lead_dist_f = float(lead_dist) - else: - self.lead_dist_f += alpha * (float(lead_dist) - self.lead_dist_f) - - # Lead stability estimation and recent-brake timer - now_t = time.monotonic() - self.update_experimental_release_accel_state(experimental_mode, now_t, scene_v_ego) - # relative speed (ego - lead) positive when closing - v_rel = (v_ego - self.lead_one.vLead) if lead_one_active else 0.0 - if self.prev_lead_dist is None: - d_rel_dot = 0.0 - else: - d_rel_dot = (lead_dist - self.prev_lead_dist) / max(self.dt, 1e-3) - self.prev_lead_dist = lead_dist - - # Remember time of last non-trivial model brake risk - if 'raw_brake_max' in locals() and raw_brake_max is not None and raw_brake_max > 0.02: - self.last_big_brake_t = now_t - - # Stable lead heuristic (short window, cheap to compute) - recently_braked = (now_t - self.last_big_brake_t) < 0.7 - self.stable_lead = ( - lead_one_active and - abs(v_rel) < 0.5 and - abs(d_rel_dot) < 0.5 and - not recently_braked + accel_limits[0] = min(accel_limits[0], self.a_desired + 0.05) + accel_limits[1] = max(accel_limits[1], self.a_desired - 0.05) + self.mpc.set_accel_limits(*accel_limits) + self.mpc.set_weights( + acceleration_jerk=float(sm["starpilotPlan"].accelerationJerk) / 200.0, + danger_jerk=float(sm["starpilotPlan"].dangerJerk) / 100.0, + speed_jerk=float(sm["starpilotPlan"].speedJerk) / 5.0, + prev_accel_constraint=not (reset_state or car_state.standstill), ) - - # Calculate scene uncertainty from model desire prediction entropy and disengage predictions - uncertainty = 0.0 - if hasattr(sm['modelV2'], 'meta'): - # Desire prediction entropy (maneuver uncertainty), normalized to [0, 1] - desire_entropy = 0.0 - if hasattr(sm['modelV2'].meta, 'desirePrediction'): - desire_probs = sm['modelV2'].meta.desirePrediction - if len(desire_probs) > 1: - probs = np.asarray(desire_probs, dtype=float) - total = float(np.sum(probs)) - if total > 1e-6: - p = probs / total - entropy = -np.sum(p * np.log(p + 1e-10)) - max_entropy = np.log(len(p)) - desire_entropy = float(entropy / max(max_entropy, 1e-6)) # normalized entropy in [0,1] - else: - desire_entropy = 0.0 # guard against all-zero vector - - # Disengage prediction risk (intervention likelihood) - disengage_risk = 0.0 - raw_brake_max = -1.0 - lam = -1.0 - if hasattr(sm['modelV2'].meta, 'disengagePredictions'): - # Use brake press probabilities as primary risk indicator - brake_probs = sm['modelV2'].meta.disengagePredictions.brakePressProbs - if len(brake_probs) > 0: - # Exponentially decayed max over the full horizon - probs = np.asarray(brake_probs, dtype=float) - # Clip tiny brake blips so they don't inflate uncertainty - if float(np.max(probs)) < 0.015: - probs = probs * 0.5 - raw_brake_max = float(np.max(probs)) - # Time vector assuming model horizon step = DT_MDL - t = np.arange(len(probs), dtype=float) * DT_MDL - lam = 0.6 # decay rate per second (tunable: 0.5–0.9 typical) - weights = np.exp(-lam * t) - disengage_risk = float(np.max(probs * weights)) - - # Combined uncertainty metric (range roughly 0..2), with dual-track filtering - raw_uncertainty = desire_entropy + disengage_risk - # Update filters - self.uncert_slow.update(raw_uncertainty) - self.uncert_fast.update(raw_uncertainty) - # Use a more permissive track for accel decisions - uncertainty = self.uncert_slow.x - uncertainty_accel = min(self.uncert_slow.x, self.uncert_fast.x) - - # --- Slope-based panic bypass --- - if self._uncert_last_t is None: - uncert_slope = 0.0 - else: - dt_u = max(1e-3, now_t - self._uncert_last_t) - uncert_slope = (uncertainty - self._uncert_last) / dt_u - self._uncert_last = uncertainty - self._uncert_last_t = now_t - - panic_close_window = False - closing_fast = False - desired_gap = None - closing_speed = 0.0 - if lead_one_active: - desired_gap = float(desired_follow_distance(v_ego, self.lead_one.vLead, effective_t_follow)) - scene_desired_gap = float(desired_follow_distance(scene_v_ego, self.lead_one.vLead, effective_t_follow)) - close_gap_window = max(UNCERT_PANIC_MAX_GAP_BUFFER_MIN, - UNCERT_PANIC_MAX_GAP_BUFFER_GAIN * float(v_ego)) - panic_close_window = float(self.lead_one.dRel) <= scene_desired_gap + close_gap_window - closing_speed = max(0.0, scene_v_ego - self.lead_one.vLead) - closing_fast = closing_speed >= max( - UNCERT_PANIC_MIN_CLOSING_SPEED, - UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN * float(scene_v_ego), - ) - - # Only bypass lead smoothing when we're closing meaningfully and already - # near the follow window. Far or nearly pace-matched leads should stay on - # the smoothed path so the planner doesn't flip-flop between accel and brake. - panic_bypass = panic_close_window and closing_fast and ( - uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG - ) - # Duplicate vision tracks can share the same noisy velocity spike. Keep the - # comfort path unless distance, TTC, or lead braking makes the scene urgent. - nonurgent_duplicate_vision_follow = self.is_nonurgent_duplicate_vision_follow(scene_v_ego, effective_t_follow) - if panic_bypass and nonurgent_duplicate_vision_follow: - panic_bypass = False - - steady_follow_filter_floor = 0.0 - if lead_one_active and desired_gap is not None and not panic_bypass: - lead_brake = max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0))) - lead_radar = bool(getattr(self.lead_one, "radar", False)) - lead_prob = float(getattr(self.lead_one, "modelProb", 1.0 if lead_radar else 0.0)) - actual_headway = float(self.lead_one.dRel) / max(scene_v_ego, 1e-3) - matched_follow_window = ( - is_radarless_matched_follow_window( - scene_v_ego, - self.lead_one.dRel, - self.lead_one.vLead, - effective_t_follow, - radar=lead_radar, - lead_brake=lead_brake, - lead_prob=lead_prob, - ) or ( - lead_radar and - scene_v_ego >= STEADY_FOLLOW_SMOOTHING_MIN_SPEED and - STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED and - actual_headway >= max(STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY, - effective_t_follow - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET) and - actual_headway <= effective_t_follow + STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET and - lead_brake <= STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE - ) - ) - if matched_follow_window: - steady_follow_filter_floor = STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR - - if panic_bypass: - if now_t - self._panic_bypass_log_t > 5.0: - self._panic_bypass_log_t = now_t - try: - cloudlog.warning( - "LON_SLOPE close bypass: " - f"slope={uncert_slope:.3f}/s uncertainty={uncertainty:.3f} " - f"v_ego={v_ego:.2f} v_rel={(v_ego - self.lead_one.vLead) if lead_one_active else 0.0:.2f} " - f"lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}" - ) - except Exception: - pass - - personality = get_longitudinal_personality(sm) - - self.mpc.set_weights(sm['starpilotPlan'].accelerationJerk, - sm['starpilotPlan'].dangerJerk, - sm['starpilotPlan'].speedJerk, - prev_accel_constraint, - personality=personality, - v_ego=v_ego, - lead_dist=self.lead_dist_f if lead_one_active and self.lead_dist_f is not None else 50.0, - uncertainty=uncertainty, - panic_bypass=panic_bypass, - filter_time_factor_floor=steady_follow_filter_floor) - self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - # After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim - dec_mpc_mode = self.get_mpc_mode() - if not self.mlsim: - self.mpc.mode = dec_mpc_mode - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, - sm['starpilotPlan'].dangerFactor, effective_t_follow, - personality=personality, tracking_lead=lead_control_active, - optional_far_lead_comfort=True, - smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass) + self.mpc.update( + sm["radarState"], + v_cruise, + danger_factor=float(sm["starpilotPlan"].dangerFactor), + t_follow=float(sm["starpilotPlan"].tFollow), + personality=sm["selfdriveState"].personality, + ) - self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution) - # TODO counter is only needed because radar is glitchy, remove once radar is gone - self.fcw = should_publish_planner_fcw(self.mpc.crash_cnt, sm['carState'], sm['radarState']) + previous_a = self.a_desired + self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) + self.v_desired_filter.x += self.dt * (self.a_desired + previous_a) / 2.0 + + actuator_delay = float(getattr(starpilot_toggles, "longitudinalActuatorDelay", self.CP.longitudinalActuatorDelay)) + action_t = max(actuator_delay + self.dt, MIN_PLAN_HORIZON) + mpc_target, mpc_should_stop = get_accel_from_plan( + self.v_desired_trajectory, + self.a_desired_trajectory, + action_t, + float(getattr(starpilot_toggles, "vEgoStopping", 0.05)), + ) + + model_target = float(getattr(sm["modelV2"].action, "desiredAcceleration", mpc_target)) + if not np.isfinite(model_target): + model_target = mpc_target + + leads = (sm["radarState"].leadOne, sm["radarState"].leadTwo) + has_lead = any(bool(getattr(lead, "status", False)) for lead in leads) + model_first = bool(getattr(starpilot_toggles, "longitudinal_model_preference", False)) + stop_authority = self._model_stop_authority(sm["modelV2"], scene_v_ego) + self.model_authority = self._get_model_authority( + sm, scene_v_ego, v_cruise, has_lead, model_first, stop_authority, + ) + model_limited_target = min(mpc_target, model_target) + target = mpc_target + self.model_authority * (model_limited_target - mpc_target) + self.plan_source = "e2e" if model_limited_target < mpc_target - 1e-3 and self.model_authority > 0.05 else self.mpc.source + + pinned_stop = bool( + getattr(sm["starpilotPlan"], "forcingStop", False) or + getattr(sm["starpilotPlan"], "stopSignConfirmed", False) or + car_state.brakePressed + ) + scene_stop = bool( + getattr(sm["starpilotPlan"], "redLight", False) or + pinned_stop or + getattr(sm["modelV2"].action, "shouldStop", False) + ) + model_stop_request = stop_authority >= STOP_ENTER_AUTHORITY + stop_request = bool(scene_stop or mpc_should_stop or model_stop_request) + increased_stop_distance = float(getattr(sm["starpilotPlan"], "increasedStoppedDistance", 0.0)) + departure_buffer = STOP_DISTANCE + increased_stop_distance + departure_candidate, fast_departure = self._safe_departure(sm, model_target, departure_buffer) + self.departure_confirm_time = self.departure_confirm_time + self.dt if departure_candidate else 0.0 + safe_departure = departure_candidate and ( + fast_departure or self.departure_confirm_time >= DEPART_CONFIRM_TIME + ) + if safe_departure and not pinned_stop: + stop_request = False + was_departing = self.state == PlanState.departing + was_stopped = self.state == PlanState.stopped + self._update_state(sm, scene_v_ego, stop_request, safe_departure) + departure_aborted = was_departing and self.state in (PlanState.stopping, PlanState.stopped) + departure_started = was_stopped and self.state == PlanState.departing + + if self.state == PlanState.stopped: + target = min(target, float(getattr(starpilot_toggles, "stopAccel", -0.5))) + elif self.state == PlanState.stopping and scene_v_ego <= DEPART_COMPLETE_SPEED: + target = min(target, float(getattr(starpilot_toggles, "stopAccel", -0.5))) + elif self.state == PlanState.departing and safe_departure: + target = max(target, self.tune.launch_accel) + + safety_cap, urgent = self._get_raw_safety_cap( + sm, + scene_v_ego, + actuator_delay + RAW_SAFETY_REACTION_BUFFER, + ) + urgent |= departure_aborted or departure_started + if safety_cap is not None: + target = min(target, safety_cap) + if safety_cap is not None and safety_cap < -RAW_SAFETY_MIN_DECEL: + if self.state == PlanState.departing: + self.state = PlanState.stopping + stop_request |= scene_v_ego <= STOP_CONTROL_SPEED + + target = float(np.clip(target, accel_limits[0], accel_limits[1])) + self.output_a_target = self._smooth_output(target, urgent) + self.output_should_stop = bool( + self.state in (PlanState.stopping, PlanState.stopped) and + (scene_v_ego <= STOP_CONTROL_SPEED or mpc_should_stop) + ) + + self.fcw = self.mpc.crash_cnt > 2 and not car_state.standstill and any( + should_trigger_planner_fcw(lead, scene_v_ego) for lead in leads + ) if self.fcw: cloudlog.info("FCW triggered") - # Safety checks for rubber-banding mitigation - max_jerk = np.max(np.abs(self.mpc.j_solution)) - max_accel_change = np.max(np.abs(np.diff(self.mpc.a_solution))) - if max_jerk > 5.0: # m/s^3 - cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3") - if max_accel_change > 2.0: # m/s^2 - cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2") - - # Interpolate 0.05 seconds and save as starting point for next iteration - a_prev = self.a_desired - self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) - self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - - # Anticipatory pre-brake to avoid "coming in hot" when closing on a lead - if lead_one_active: - rel_v = max(0.0, v_ego - self.lead_one.vLead) - # dynamic time headway adds a small buffer when uncertainty is elevated - base_th = max(1.6, effective_t_follow) - th = base_th + 0.6 * max(0.0, uncertainty - 0.42) - desired_gap = th * v_ego - if (self.lead_dist_f is not None and self.lead_dist_f < desired_gap and rel_v > 0.5): - k_rel, k_unc = 0.04, 0.20 - pre_brake = k_rel * rel_v + k_unc * max(0.0, uncertainty - 0.42) - pre_brake = min(pre_brake, 0.06) - self.a_desired = float(self.a_desired - pre_brake) - - # Small deadzone around zero accel to kill micro-dithers - if -0.05 < self.a_desired < 0.05: - self.a_desired = 0.0 - - classic_model = bool(getattr(starpilot_toggles, "classic_model", False)) - tinygrad_model = bool(getattr(starpilot_toggles, "tinygrad_model", False)) - experimental_mlsim = bool(tinygrad_model and self.mlsim and self.mode != 'acc') - action_t = self.CP.longitudinalActuatorDelay + DT_MDL - prev_output_a_target = float(self.output_a_target) - model_launch_accel = None - if self.model_launch_armed and not bool(sm['modelV2'].action.shouldStop): - model_launch_accel = self.get_model_launch_accel(model_launch_v, model_launch_a, action_t, scene_v_ego) - - if classic_model: - output_a_target, output_should_stop = get_accel_from_plan_classic( - self.CP, self.v_desired_trajectory, self.a_desired_trajectory, starpilot_toggles.vEgoStopping) - elif tinygrad_model: - output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan( - self.v_desired_trajectory, self.a_desired_trajectory, - action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop - - if self.mode == 'acc' or self.generation == 'v9': - output_a_target = output_a_target_mpc - output_should_stop = output_should_stop_mpc - else: - output_a_target = min(output_a_target_mpc, output_a_target_e2e) - output_should_stop = output_should_stop_e2e or output_should_stop_mpc - else: - output_a_target, output_should_stop = get_accel_from_plan( - self.v_desired_trajectory, self.a_desired_trajectory, - action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) - - comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] - vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego)) - output_accel_min = comfort_output_accel_min - model_desired_accel = float(sm['modelV2'].action.desiredAcceleration) - - raw_approach_lift_cap = None - if not tracking_lead: - approach_lift_caps = [ - cap for cap in ( - self.get_vision_untracked_approach_lift_cap(self.lead_one, v_ego, effective_t_follow), - self.get_vision_untracked_approach_lift_cap(self.lead_two, v_ego, effective_t_follow), - ) if cap is not None - ] - if approach_lift_caps: - raw_approach_lift_cap = min(approach_lift_caps) - - pretracking_vision_caps = [] - for lead in (self.lead_one, self.lead_two): - if lead.status and not bool(getattr(lead, "radar", False)): - pretracking_cap = self.get_vision_untracked_slow_lead_cap(lead, v_ego, vision_cap_accel_min) - if pretracking_cap is not None: - pretracking_vision_caps.append((pretracking_cap, lead)) - - if pretracking_vision_caps: - pretracking_vision_cap, pretracking_vision_lead = min(pretracking_vision_caps, key=lambda cap_and_lead: cap_and_lead[0]) - lead_brake = max(0.0, -float(getattr(pretracking_vision_lead, "aLeadK", 0.0))) - immediate_pretracking_cap = ( - pretracking_vision_cap <= -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL or - float(getattr(pretracking_vision_lead, "dRel", float("inf"))) <= VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DISTANCE or - lead_brake >= VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_LEAD_BRAKE or - float(getattr(pretracking_vision_lead, "vLead", float("inf"))) <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED - ) - - if immediate_pretracking_cap: - self.untracked_slow_lead_confirm_t = VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME - else: - self.untracked_slow_lead_confirm_t = min( - self.untracked_slow_lead_confirm_t + self.dt, - VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME, - ) - - if self.untracked_slow_lead_confirm_t >= VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME: - self.a_desired = min(self.a_desired, pretracking_vision_cap) - output_a_target = min(output_a_target, pretracking_vision_cap) - else: - self.untracked_slow_lead_confirm_t = 0.0 - else: - self.untracked_slow_lead_confirm_t = 0.0 - - approach_lift_cap = self.update_vision_untracked_approach_lift_cap( - raw_approach_lift_cap, - output_a_target, - prev_output_a_target, - now_t, - not tracking_lead, - ) - if approach_lift_cap is not None: - self.a_desired = min(self.a_desired, approach_lift_cap) - output_a_target = min(output_a_target, approach_lift_cap) - - close_lead_caps = [] - tracked_vision_approach_caps = [] - vision_low_speed_stop_active = False - vision_brake_cap_active = False - if lead_control_active: - for lead in (self.lead_one, self.lead_two): - cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min) - if cap is not None: - close_lead_caps.append(cap) - slow_stop_cap = self.get_vision_slow_stopped_lead_cap(lead, v_ego, vision_cap_accel_min, effective_t_follow) - if slow_stop_cap is not None: - close_lead_caps.append(slow_stop_cap) - vision_brake_cap_active = True - approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, vision_cap_accel_min, effective_t_follow) - if approach_cap is not None: - tracked_vision_approach_caps.append(( - approach_cap, - self.tracked_vision_lead_approach_needs_immediate_brake(lead, v_ego, approach_cap), - )) - low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, vision_cap_accel_min) - if low_speed_stop_cap is not None: - close_lead_caps.append(low_speed_stop_cap) - vision_brake_cap_active = True - vision_low_speed_stop_active |= low_speed_stop_active - if tracked_vision_approach_caps: - if any(immediate for _, immediate in tracked_vision_approach_caps): - self.vision_lead_approach_confirm_t = VISION_LEAD_APPROACH_CONFIRM_TIME - else: - self.vision_lead_approach_confirm_t = min( - self.vision_lead_approach_confirm_t + self.dt, - VISION_LEAD_APPROACH_CONFIRM_TIME, - ) - - if self.vision_lead_approach_confirm_t >= VISION_LEAD_APPROACH_CONFIRM_TIME: - close_lead_caps.append(min(cap for cap, _ in tracked_vision_approach_caps)) - vision_brake_cap_active = True - else: - self.vision_lead_approach_confirm_t = 0.0 - if close_lead_caps: - close_lead_brake_cap = min(close_lead_caps) - self.a_desired = min(self.a_desired, close_lead_brake_cap) - output_a_target = min(output_a_target, close_lead_brake_cap) - - standstill_nudge_gap = STOP_DISTANCE - 0.5 - moving_leads = [lead for lead in (self.lead_one, self.lead_two) - if lead.status and - lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap] - accelerating_nudge_lead = any( - lead.status and - float(getattr(lead, "vLead", 0.0)) > STANDSTILL_LEAD_NUDGE_MIN_SPEED and - float(getattr(lead, "aLeadK", 0.0)) >= STANDSTILL_LEAD_NUDGE_MIN_LEAD_ACCEL and - float(getattr(lead, "dRel", 0.0)) >= standstill_nudge_gap - for lead in (self.lead_one, self.lead_two) - ) - confident_depart_detected = any(self.is_confident_lead_depart(lead, float(sm['carState'].vEgo)) - for lead in (self.lead_one, self.lead_two)) - lead_depart_ready = any( - lead.status and - lead.vLead >= STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED and - lead.dRel >= standstill_nudge_gap + STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN - for lead in (self.lead_one, self.lead_two) - ) - depart_safety_veto = (not bool(getattr(starpilot_toggles, "radar_takeoffs", False)) - and self.has_offcenter_radar_depart_conflict(sm)) - safe_depart_release_hold_lead = self.get_safe_depart_release_hold_lead(float(sm['carState'].vEgo)) - depart_release_hold_context = bool( - lead_control_active and - float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED and - not depart_safety_veto and - not bool(getattr(sm['carState'], 'brakePressed', False)) and - not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and - not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and - safe_depart_release_hold_lead is not None - ) - if depart_release_hold_context: - self.lead_depart_release_candidate_elapsed = min( - LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME, - self.lead_depart_release_candidate_elapsed + self.dt, - ) - else: - self.lead_depart_release_candidate_elapsed = 0.0 - self.lead_depart_release_pending = False - self.lead_depart_release_hold_remaining = 0.0 - - if self.lead_depart_release_hold_remaining > 0.0: - self.lead_depart_release_hold_remaining = max(0.0, self.lead_depart_release_hold_remaining - self.dt) - depart_release_hold_active = bool(depart_release_hold_context and self.lead_depart_release_hold_remaining > 0.0) - if ( - lead_control_active and - sm['carState'].standstill and - not depart_safety_veto and - not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and - not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and - confident_depart_detected - ): - self.confident_lead_depart_elapsed = min( - LEAD_DEPART_CONFIDENT_CONFIRM_TIME, - self.confident_lead_depart_elapsed + self.dt, - ) - else: - self.confident_lead_depart_elapsed = 0.0 - confident_depart_ready = ( - confident_depart_detected and - self.confident_lead_depart_elapsed >= LEAD_DEPART_CONFIDENT_CONFIRM_TIME - ) - slow_creep_depart_detected = any( - self.is_slow_creep_lead_depart(lead, float(sm['carState'].vEgo), standstill_nudge_gap) - for lead in (self.lead_one, self.lead_two) - ) - if ( - lead_control_active and - sm['carState'].standstill and - not depart_safety_veto and - not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and - not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and - slow_creep_depart_detected - ): - self.slow_creep_lead_depart_elapsed = min( - STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME, - self.slow_creep_lead_depart_elapsed + self.dt, - ) - else: - self.slow_creep_lead_depart_elapsed = 0.0 - slow_creep_depart_ready = ( - slow_creep_depart_detected and - self.slow_creep_lead_depart_elapsed >= STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME - ) - radar_gap_settle_active = self.update_radar_standstill_gap_settle(sm, standstill_nudge_gap) - - standstill_stopped_lead_guard_cap = None - standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two)) - if standstill_guard_lead_present and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED): - release_ready = bool( - lead_depart_ready or confident_depart_ready or slow_creep_depart_ready or - radar_gap_settle_active or depart_release_hold_active - ) - standstill_stopped_lead_guard_caps = [ - cap for cap in ( - self.get_standstill_stopped_lead_guard_cap( - self.lead_one, - float(sm['carState'].vEgo), - output_accel_min, - standstill_nudge_gap, - release_ready, - confident_depart_ready, - ), - self.get_standstill_stopped_lead_guard_cap( - self.lead_two, - float(sm['carState'].vEgo), - output_accel_min, - standstill_nudge_gap, - release_ready, - confident_depart_ready, - ), - ) if cap is not None - ] - if standstill_stopped_lead_guard_caps: - standstill_stopped_lead_guard_cap = min(standstill_stopped_lead_guard_caps) - output_should_stop = True - - if lead_control_active and sm['carState'].standstill and moving_leads and not depart_safety_veto: - output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) - - if ( - lead_control_active and - sm['carState'].standstill and - (confident_depart_ready or lead_depart_ready or slow_creep_depart_ready) and - not depart_safety_veto and - not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and - not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and - (confident_depart_ready or slow_creep_depart_ready or model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL) - ): - vision_low_speed_stop_active = False - output_should_stop = False - depart_min_accel = STANDSTILL_LEAD_DEPART_MIN_ACCEL - if slow_creep_depart_ready and not (confident_depart_ready or lead_depart_ready): - depart_min_accel = STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL - output_a_target = max(output_a_target, depart_min_accel) - self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME - - if depart_release_hold_context and bool(self.output_should_stop) and not bool(output_should_stop): - self.lead_depart_release_pending = True - if ( - depart_release_hold_context and - self.lead_depart_release_pending and - not bool(output_should_stop) and - self.lead_depart_release_candidate_elapsed >= LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME - ): - self.lead_depart_release_hold_remaining = LEAD_DEPART_RELEASE_HOLD_TIME - self.lead_depart_release_pending = False - depart_release_hold_active = True - elif bool(output_should_stop) and self.lead_depart_release_hold_remaining <= 0.0: - self.lead_depart_release_pending = False - - if depart_release_hold_active: - vision_low_speed_stop_active = False - output_should_stop = False - self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME - - if lead_control_active and lead_depart_ready and not depart_safety_veto and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED: - output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) - self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME - - if radar_gap_settle_active: - vision_low_speed_stop_active = False - output_should_stop = False - output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL - - lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two)) - confirmed_lead_release = bool(confident_depart_ready or lead_depart_ready or slow_creep_depart_ready) - model_launch_allowed = bool( - model_launch_accel is not None and - not output_should_stop and - not vision_low_speed_stop_active and - not bool(getattr(sm['carState'], 'brakePressed', False)) and - not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and - not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and - not depart_safety_veto and - ( - (lead_present and lead_control_active and confirmed_lead_release) or - (not lead_present and (self.mode != 'acc' or self.model_launch_stop_seen)) - ) - ) - if model_launch_allowed: - output_a_target = max(output_a_target, model_launch_accel) - - if depart_safety_veto or output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)): - self.lead_depart_accel_hold_until = 0.0 - self.lead_depart_accel_hold_floor = None - - lead_depart_accel_floor = None - lead_depart_accel_floor_reused = False - if lead_control_active and not output_should_stop and not depart_safety_veto: - lead_depart_accel_floors = [ - floor for floor in ( - self.get_lead_depart_accel_floor(self.lead_one, scene_v_ego, model_desired_accel), - self.get_lead_depart_accel_floor(self.lead_two, scene_v_ego, model_desired_accel), - ) if floor is not None - ] - if lead_depart_accel_floors: - lead_depart_accel_floor = max(lead_depart_accel_floors) - self.lead_depart_accel_hold_floor = lead_depart_accel_floor - if sm['carState'].standstill: - self.lead_depart_accel_hold_until = now_t + LEAD_DEPART_ACCEL_HOLD_TIME - elif self.lead_depart_accel_hold_floor is not None and now_t < self.lead_depart_accel_hold_until: - reusable_hold_floors = [ - floor for floor in ( - self.get_reusable_lead_depart_accel_floor(self.lead_one, scene_v_ego, effective_t_follow), - self.get_reusable_lead_depart_accel_floor(self.lead_two, scene_v_ego, effective_t_follow), - ) if floor is not None - ] - if reusable_hold_floors: - lead_depart_accel_floor = max(reusable_hold_floors) - lead_depart_accel_floor_reused = True - else: - self.lead_depart_accel_hold_floor = None - elif now_t >= self.lead_depart_accel_hold_until: - self.lead_depart_accel_hold_floor = None - - lead_depart_accel_hold_active = ( - lead_depart_accel_floor is not None and - now_t < self.lead_depart_accel_hold_until and - float(sm['carState'].vEgo) <= LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED - ) - - low_speed_weak_lead_accel_cap = None - if not output_should_stop: - low_speed_weak_lead_accel_caps = [ - cap for cap in ( - self.get_low_speed_weak_lead_accel_cap(self.lead_one, scene_v_ego), - self.get_low_speed_weak_lead_accel_cap(self.lead_two, scene_v_ego), - ) if cap is not None - ] - if low_speed_weak_lead_accel_caps: - low_speed_weak_lead_accel_cap = min(low_speed_weak_lead_accel_caps) - - close_stop_active = bool(output_should_stop or vision_low_speed_stop_active) - - close_stop_hold_cap = None - if lead_control_active and close_stop_active: - close_stop_hold_caps = [ - cap for cap in ( - self.get_vision_close_stop_hold_cap(self.lead_one, v_ego, output_accel_min, close_stop_active), - self.get_vision_close_stop_hold_cap(self.lead_two, v_ego, output_accel_min, close_stop_active), - ) if cap is not None - ] - if close_stop_hold_caps: - close_stop_hold_cap = min(close_stop_hold_caps) - - close_release_hold_cap = None - if lead_control_active and not close_stop_active: - close_release_hold_caps = [ - cap for cap in ( - self.get_vision_close_release_hold_cap(self.lead_one, v_ego, vision_cap_accel_min, close_stop_active), - self.get_vision_close_release_hold_cap(self.lead_two, v_ego, vision_cap_accel_min, close_stop_active), - ) if cap is not None - ] - if close_release_hold_caps: - close_release_hold_cap = min(close_release_hold_caps) - - close_settle_guard_active = bool( - output_should_stop or - vision_low_speed_stop_active or - sm['controlsState'].longControlState == LongCtrlState.stopping - ) - close_settle_cap = None - if lead_control_active: - close_settle_caps = [ - cap for cap in ( - self.get_vision_close_settle_cap(self.lead_one, v_ego, output_accel_min, close_settle_guard_active), - self.get_vision_close_settle_cap(self.lead_two, v_ego, output_accel_min, close_settle_guard_active), - ) if cap is not None - ] - if close_settle_caps: - close_settle_cap = min(close_settle_caps) - - close_final_guard_cap = None - if lead_control_active: - close_final_guard_caps = [ - cap for cap in ( - self.get_vision_close_final_guard_cap(self.lead_one, v_ego, output_accel_min), - self.get_vision_close_final_guard_cap(self.lead_two, v_ego, output_accel_min), - ) if cap is not None - ] - if close_final_guard_caps: - close_final_guard_cap = min(close_final_guard_caps) - - if lead_one_active: - lead_catchup_accel_cap = self.get_lead_catchup_accel_cap( - self.lead_one, - scene_v_ego, - effective_t_follow, - current_source=self.mpc.source, - tracking_lead_active=tracking_lead, - ) - if lead_catchup_accel_cap is not None: - self.a_desired = min(self.a_desired, lead_catchup_accel_cap) - output_a_target = min(output_a_target, lead_catchup_accel_cap) - - if lead_control_active and np.isfinite(v_cruise) and any(lead.status for lead in (self.lead_one, self.lead_two)): - # This is only an acceleration cap. A negative cap would manufacture hard - # braking on abrupt cruise-target drops instead of letting the MPC decelerate. - cruise_accel_cap = max(0.0, (v_cruise - v_ego + 0.01) / max(action_t, self.dt)) - output_a_target = min(output_a_target, cruise_accel_cap) - - if vision_brake_cap_active: - output_accel_min = min(output_accel_min, vision_cap_accel_min) - - follow_control_lead = self.get_follow_control_lead( - lead_control_active, - scene_v_ego, - effective_t_follow, - allow_optional_far_lead_logic=True, - ) - duplicate_vision_comfort_lead = self.get_duplicate_vision_comfort_lead(scene_v_ego) - comfort_follow_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None and follow_control_lead is not None else follow_control_lead - if follow_control_lead is not None and not panic_bypass: - if not output_should_stop and not vision_low_speed_stop_active: - tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( - follow_control_lead, - scene_v_ego, - output_accel_min, - effective_t_follow, - model_desired_accel, - ) - if tracked_vision_model_brake_floor is not None: - self.a_desired = min(self.a_desired, tracked_vision_model_brake_floor) - output_a_target = min(output_a_target, tracked_vision_model_brake_floor) - - matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow) - if matched_follow_brake_cap is not None: - self.a_desired = max(self.a_desired, matched_follow_brake_cap) - output_a_target = max(output_a_target, matched_follow_brake_cap) - - if not close_lead_caps and not output_should_stop and not vision_low_speed_stop_active: - low_speed_transition_brake_cap = self.get_low_speed_follow_transition_brake_cap( - follow_control_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - ) - if low_speed_transition_brake_cap is not None: - self.a_desired = max(self.a_desired, low_speed_transition_brake_cap) - output_a_target = max(output_a_target, low_speed_transition_brake_cap) - - comfort_lead = duplicate_vision_comfort_lead if duplicate_vision_comfort_lead is not None else ( - self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one - ) - if comfort_lead is not None and not panic_bypass: - far_lead_brake_cap = self.get_far_lead_brake_cap(comfort_lead, scene_v_ego, effective_t_follow) - if far_lead_brake_cap is not None: - self.a_desired = max(self.a_desired, far_lead_brake_cap) - output_a_target = max(output_a_target, far_lead_brake_cap) - - if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: - tracked_vision_model_brake_cap = self.get_tracked_vision_model_brake_cap( - follow_control_lead, - scene_v_ego, - effective_t_follow, - model_desired_accel, - ) - if tracked_vision_model_brake_cap is not None: - self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap) - output_a_target = max(output_a_target, tracked_vision_model_brake_cap) - - if comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: - matched_follow_transition_target = self.get_matched_follow_transition_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - bool(getattr(sm["starpilotPlan"], "trackingLead", False)), - ) - if matched_follow_transition_target is not None: - if matched_follow_transition_target < output_a_target: - self.a_desired = min(self.a_desired, matched_follow_transition_target) - else: - self.a_desired = max(self.a_desired, matched_follow_transition_target) - output_a_target = matched_follow_transition_target - - if comfort_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: - near_duplicate_transition_target = self.get_near_duplicate_lead_transition_target( - comfort_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - bool(getattr(sm["starpilotPlan"], "trackingLead", False)), - ) - if near_duplicate_transition_target is not None: - if near_duplicate_transition_target < output_a_target: - self.a_desired = min(self.a_desired, near_duplicate_transition_target) - else: - self.a_desired = max(self.a_desired, near_duplicate_transition_target) - output_a_target = near_duplicate_transition_target - - duplicate_slow_lead_brake_hold_target = self.get_duplicate_slow_lead_brake_hold_target( - comfort_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - bool(getattr(sm["starpilotPlan"], "trackingLead", False)), - ) - if duplicate_slow_lead_brake_hold_target is not None: - if duplicate_slow_lead_brake_hold_target < output_a_target: - self.a_desired = min(self.a_desired, duplicate_slow_lead_brake_hold_target) - else: - self.a_desired = max(self.a_desired, duplicate_slow_lead_brake_hold_target) - output_a_target = duplicate_slow_lead_brake_hold_target - - if comfort_follow_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: - cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - self.mpc.source, - tracking_lead, - ) - if cruise_tracking_lead_accel_cap is not None: - self.a_desired = min(self.a_desired, cruise_tracking_lead_accel_cap) - output_a_target = min(output_a_target, cruise_tracking_lead_accel_cap) - - cruise_tracking_lead_transition_target = self.get_cruise_tracking_lead_accel_transition_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - ) - if cruise_tracking_lead_transition_target is not None: - self.a_desired = min(self.a_desired, cruise_tracking_lead_transition_target) - output_a_target = min(output_a_target, cruise_tracking_lead_transition_target) - - mild_follow_zero_cross_guard_target = self.get_mild_follow_zero_cross_guard_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - self.mpc.source, - tracking_lead, - ) - if mild_follow_zero_cross_guard_target is not None: - output_a_target = mild_follow_zero_cross_guard_target - - far_opening_radar_brake_guard_target = self.get_far_opening_radar_brake_guard_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - v_cruise, - model_desired_accel, - self.mpc.source, - tracking_lead, - ) - if far_opening_radar_brake_guard_target is not None: - self.a_desired = max(self.a_desired, far_opening_radar_brake_guard_target) - output_a_target = max(output_a_target, far_opening_radar_brake_guard_target) - - output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] - output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) - - if close_stop_hold_cap is not None: - self.a_desired = min(self.a_desired, close_stop_hold_cap) - output_a_target = min(output_a_target, close_stop_hold_cap) - - if standstill_stopped_lead_guard_cap is not None: - self.a_desired = min(self.a_desired, standstill_stopped_lead_guard_cap) - output_a_target = min(output_a_target, standstill_stopped_lead_guard_cap) - - if close_settle_cap is not None: - self.a_desired = min(self.a_desired, close_settle_cap) - output_a_target = min(output_a_target, close_settle_cap) - - if close_final_guard_cap is not None: - self.a_desired = min(self.a_desired, close_final_guard_cap) - output_a_target = min(output_a_target, close_final_guard_cap) - - if close_release_hold_cap is not None: - self.a_desired = min(self.a_desired, close_release_hold_cap) - output_a_target = min(output_a_target, close_release_hold_cap) - - if depart_safety_veto: - self.a_desired = min(self.a_desired, 0.0) - output_a_target = min(output_a_target, 0.0) - if sm['carState'].standstill: - output_should_stop = True - - if lead_depart_accel_hold_active: - output_a_target = max(output_a_target, lead_depart_accel_floor) - - if low_speed_weak_lead_accel_cap is not None and not (lead_depart_accel_hold_active and lead_depart_accel_floor_reused): - self.a_desired = min(self.a_desired, low_speed_weak_lead_accel_cap) - output_a_target = min(output_a_target, low_speed_weak_lead_accel_cap) - - if ( - lead_control_active and - (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED) and - output_should_stop and - accelerating_nudge_lead and - not depart_safety_veto - ): - output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) - - force_stop_handoff = bool( - getattr(sm['starpilotPlan'], 'forcingStop', False) and - not lead_control_active and - ( - float(getattr(sm['starpilotPlan'], 'forcingStopLength', float('inf'))) < 1.0 or - float(getattr(sm['starpilotPlan'], 'vCruise', float('inf'))) <= FORCE_STOP_HANDOFF_MAX_VCRUISE - ) - ) - - if force_stop_handoff: - output_should_stop = True - - manual_stop_resume_override = self._update_manual_stop_resume_override(sm) - if manual_stop_resume_override: - output_a_target = max(output_a_target, MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL) - output_should_stop = False - - inside_gap_closing_cap = None - if lead_control_active: - inside_gap_closing_lead = self.lead_two if self.mpc.source == 'lead1' else self.lead_one - inside_gap_closing_cap = self.get_inside_gap_closing_lead_accel_cap( - inside_gap_closing_lead, - scene_v_ego, - output_accel_min, - sm['starpilotPlan'].tFollow, - ) - if inside_gap_closing_cap is not None: - self.a_desired = min(self.a_desired, inside_gap_closing_cap) - output_a_target = min(output_a_target, inside_gap_closing_cap) - - experimental_release_accel_target = self.get_experimental_release_accel_target( - comfort_follow_lead, - scene_v_ego, - effective_t_follow, - prev_output_a_target, - output_a_target, - bool( - now_t < self.experimental_release_accel_until and - not output_should_stop and - not vision_low_speed_stop_active and - not getattr(sm['starpilotPlan'], 'forcingStop', False) and - not getattr(sm['starpilotPlan'], 'redLight', False) - ), - ) - if experimental_release_accel_target is not None: - self.a_desired = min(self.a_desired, experimental_release_accel_target) - output_a_target = min(output_a_target, experimental_release_accel_target) - - if depart_release_hold_active: - output_a_target = max(output_a_target, STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL) - - output_a_target = self.get_vehicle_far_follow_slew_target( - scene_v_ego, - prev_output_a_target, - output_a_target, - bool(output_should_stop or vision_low_speed_stop_active), - panic_bypass, - ) - - if radar_gap_settle_active: - output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL - output_should_stop = False - - self.output_a_target = output_a_target - self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) - def publish(self, sm, pm): - plan_send = messaging.new_message('longitudinalPlan') + plan_send = messaging.new_message("longitudinalPlan") + plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"]) - plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState', 'radarState']) + plan = plan_send.longitudinalPlan + plan.modelMonoTime = sm.logMonoTime["modelV2"] + plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime["modelV2"] + plan.solverExecutionTime = self.mpc.solve_time + plan.speeds = self.v_desired_trajectory.tolist() + plan.accels = self.a_desired_trajectory.tolist() + plan.jerks = self.j_desired_trajectory.tolist() + plan.hasLead = any(lead.status for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo)) + plan.longitudinalPlanSource = self.plan_source + plan.fcw = self.fcw + plan.aTarget = float(self.output_a_target) - longitudinalPlan = plan_send.longitudinalPlan - longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] - longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] - longitudinalPlan.solverExecutionTime = self.mpc.solve_time - - longitudinalPlan.speeds = self.v_desired_trajectory.tolist() - longitudinalPlan.accels = self.a_desired_trajectory.tolist() - longitudinalPlan.jerks = self.j_desired_trajectory.tolist() - - longitudinalPlan.hasLead = sm['radarState'].leadOne.status - longitudinalPlan.longitudinalPlanSource = self.mpc.source - longitudinalPlan.fcw = self.fcw - - longitudinalPlan.aTarget = float(self.output_a_target) force_stop_handoff = bool( - sm['starpilotPlan'].forcingStop and ( - sm['starpilotPlan'].forcingStopLength < 1.0 or - sm['starpilotPlan'].vCruise <= FORCE_STOP_HANDOFF_MAX_VCRUISE + sm["starpilotPlan"].forcingStop and ( + sm["starpilotPlan"].forcingStopLength < 1.0 or + sm["starpilotPlan"].vCruise <= FORCE_STOP_HANDOFF_MAX_VCRUISE ) ) - longitudinalPlan.shouldStop = bool(self.output_should_stop) or force_stop_handoff - longitudinalPlan.allowBrake = True - longitudinalPlan.allowThrottle = bool(self.allow_throttle) - - pm.send('longitudinalPlan', plan_send) + plan.shouldStop = bool(self.output_should_stop or force_stop_handoff) + plan.allowBrake = True + plan.allowThrottle = bool(self.allow_throttle) + pm.send("longitudinalPlan", plan_send) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index b12d7f311..e5250aecc 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -1,18 +1,28 @@ -HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0 -HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0 -HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35 +from dataclasses import dataclass -def get_far_follow_output_slew_rates(CP): - if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G": - return ( - HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE, - HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE, - ) - return 0.0, 0.0 +@dataclass(frozen=True) +class LongitudinalPlannerTune: + lead_filter_tau: float = 0.45 + accel_slew_rate: float = 0.90 + brake_slew_rate: float = 1.40 + launch_accel: float = 0.35 -def get_untracked_slow_lead_decel_scale(CP): - if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G": - return HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE - return 1.0 +DEFAULT_TUNE = LongitudinalPlannerTune() + +# Vehicle exceptions are deliberately declarative and limited to physical +# response differences. Planning and safety logic remain global. +VEHICLE_TUNES = { + ("honda", "HONDA_HRV_3G"): LongitudinalPlannerTune( + lead_filter_tau=0.65, + accel_slew_rate=0.65, + brake_slew_rate=1.00, + launch_accel=0.30, + ), +} + + +def get_longitudinal_planner_tune(CP): + key = (str(getattr(CP, "brand", "")), str(getattr(CP, "carFingerprint", ""))) + return VEHICLE_TUNES.get(key, DEFAULT_TUNE) diff --git a/selfdrive/controls/tests/test_conditional_chill_mode.py b/selfdrive/controls/tests/test_conditional_chill_mode.py deleted file mode 100644 index a4635d3d6..000000000 --- a/selfdrive/controls/tests/test_conditional_chill_mode.py +++ /dev/null @@ -1,463 +0,0 @@ -from types import SimpleNamespace - -from openpilot.common.constants import CV -from openpilot.starpilot.common.experimental_state import CCStatus -from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode - - -class FakeParams: - def __init__(self, bools=None, ints=None): - self.bools = dict(bools or {}) - self.ints = dict(ints or {}) - - def get_bool(self, key): - return bool(self.bools.get(key, False)) - - def put_bool(self, key, value): - self.bools[key] = bool(value) - - def get_int(self, key, default=0): - return int(self.ints.get(key, default)) - - def put_int(self, key, value): - self.ints[key] = int(value) - - -class FakeDetector: - def __init__(self): - self.curve_detected = False - self.slow_lead_detected = False - self.stop_light_detected = False - self.stop_light_model_detected = False - - def curve_detection(self, *_args, **_kwargs): - return None - - def slow_lead(self, *_args, **_kwargs): - return None - - def stop_sign_and_light(self, *_args, **_kwargs): - return None - - -class FakeSubMaster: - def __init__(self, services): - self.services = dict(services) - - def __getitem__(self, key): - return self.services[key] - - -def make_sm(): - return { - "carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False), - "selfdriveState": SimpleNamespace(enabled=True), - "longitudinalPlan": SimpleNamespace(allowThrottle=True, shouldStop=False), - "starpilotCarState": SimpleNamespace(trafficModeEnabled=False), - "starpilotPlan": SimpleNamespace(redLight=False, forcingStop=False), - "starpilotRadarState": SimpleNamespace( - leadLeft=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), - leadRight=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), - ), - } - - -def make_submaster_sm(): - return FakeSubMaster(make_sm()) - - -def make_toggles(): - return SimpleNamespace( - conditional_chill_speed=45 * CV.MPH_TO_MS, - conditional_chill_speed_lead=35 * CV.MPH_TO_MS, - conditional_chill_speed_margin=3 * CV.MPH_TO_MS, - conditional_chill_lead=True, - conditional_chill_launch_assist=False, - ) - - -def make_ccm(): - planner = SimpleNamespace( - params=FakeParams(), - params_memory=FakeParams(), - starpilot_vcruise=SimpleNamespace( - stop_sign_confirmed=False, - forcing_stop=False, - slc=SimpleNamespace(experimental_mode=False), - ), - raw_model_stopped=False, - model_stopped=False, - tracking_lead=False, - lead_one=SimpleNamespace( - status=False, - dRel=float("inf"), - vLead=0.0, - vRel=0.0, - aLeadK=0.0, - modelProb=0.0, - radar=False, - ), - ) - detector = FakeDetector() - return planner, detector, ConditionalChillMode(planner, detector) - - -def test_ccm_stays_experimental_when_no_chill_condition_matches(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0) - ccm.update(20 * CV.MPH_TO_MS, 21 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_enters_chill_for_open_road_speed_recovery(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - monotonic_values = iter([10.0, 10.5]) - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) - - v_ego = 55 * CV.MPH_TO_MS - v_cruise = v_ego + 5 * CV.MPH_TO_MS - ccm.update(v_ego, v_cruise, sm, toggles) - ccm.update(v_ego, v_cruise, sm, toggles) - - assert not ccm.experimental_mode - assert ccm.status_value == CCStatus["SPEED"] - assert planner.params_memory.get_int("CCStatus") == CCStatus["SPEED"] - - -def test_ccm_enters_chill_for_stable_lead_cruising(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - monotonic_values = iter([10.0, 11.1]) - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) - - planner.tracking_lead = True - planner.lead_one.status = True - planner.lead_one.dRel = 45.0 - planner.lead_one.vLead = 24.8 - planner.lead_one.radar = True - - v_ego = 58 * CV.MPH_TO_MS - ccm.update(v_ego, v_ego, sm, toggles) - ccm.update(v_ego, v_ego, sm, toggles) - - assert not ccm.experimental_mode - assert ccm.status_value == CCStatus["LEAD"] - - -def test_ccm_stable_lead_requires_longer_entry_debounce(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - monotonic_values = iter([10.0, 10.6]) - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) - - planner.tracking_lead = True - planner.lead_one.status = True - planner.lead_one.dRel = 45.0 - planner.lead_one.vLead = 24.8 - planner.lead_one.radar = True - - v_ego = 58 * CV.MPH_TO_MS - ccm.update(v_ego, v_ego, sm, toggles) - ccm.update(v_ego, v_ego, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_hard_vetoes_force_experimental(monkeypatch): - planner, detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - v_ego = 60 * CV.MPH_TO_MS - v_cruise = v_ego + 5 * CV.MPH_TO_MS - - veto_scenes = [] - - detector.slow_lead_detected = True - veto_scenes.append(("slow_lead", make_sm())) - detector.slow_lead_detected = False - - traffic_sm = make_sm() - traffic_sm["starpilotCarState"].trafficModeEnabled = True - veto_scenes.append(("traffic_mode", traffic_sm)) - - adjacent_sm = make_sm() - adjacent_sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=25.0, vLead=12.0) - veto_scenes.append(("adjacent_lead", adjacent_sm)) - - for index, (_name, scene_sm) in enumerate(veto_scenes, start=1): - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda idx=index: float(idx)) - if _name == "slow_lead": - detector.slow_lead_detected = True - else: - detector.slow_lead_detected = False - ccm.update(v_ego, v_cruise, scene_sm, toggles) - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - planner.starpilot_vcruise.slc.experimental_mode = True - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0) - ccm.update(v_ego, v_cruise, sm, toggles) - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_far_lateral_adjacent_lead_does_not_block_open_road_chill(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - monotonic_values = iter([10.0, 10.5]) - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) - - sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=6.5) - - v_ego = 55 * CV.MPH_TO_MS - v_cruise = v_ego + 5 * CV.MPH_TO_MS - ccm.update(v_ego, v_cruise, sm, toggles) - ccm.update(v_ego, v_cruise, sm, toggles) - - assert not ccm.experimental_mode - assert ccm.status_value == CCStatus["SPEED"] - - -def test_ccm_immediate_adjacent_lead_still_blocks_open_road_chill(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0) - sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=4.5) - - v_ego = 55 * CV.MPH_TO_MS - v_cruise = v_ego + 5 * CV.MPH_TO_MS - ccm.update(v_ego, v_cruise, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_immediately_exits_chill_when_scene_turns_into_slow_lead(monkeypatch): - planner, detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - monotonic_values = iter([10.0, 11.1, 11.2]) - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) - - planner.tracking_lead = True - planner.lead_one.status = True - planner.lead_one.dRel = 40.0 - planner.lead_one.vLead = 24.9 - planner.lead_one.radar = True - - v_ego = 58 * CV.MPH_TO_MS - ccm.update(v_ego, v_ego, sm, toggles) - ccm.update(v_ego, v_ego, sm, toggles) - assert not ccm.experimental_mode - - detector.slow_lead_detected = True - ccm.update(v_ego, v_ego, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_launch_assist_enters_chill_from_standstill_when_planner_wants_to_go(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - sm["carState"].standstill = True - toggles = make_toggles() - toggles.conditional_chill_launch_assist = True - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 20.0) - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - - assert not ccm.experimental_mode - assert ccm.status_value == CCStatus["SPEED"] - - -def test_ccm_launch_assist_does_not_bypass_real_stop_scene(monkeypatch): - planner, detector, ccm = make_ccm() - sm = make_sm() - sm["carState"].standstill = True - sm["longitudinalPlan"].shouldStop = True - toggles = make_toggles() - toggles.conditional_chill_launch_assist = True - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 21.0) - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - detector.stop_light_detected = True - sm["longitudinalPlan"].shouldStop = False - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_launch_assist_rejects_red_light_stationary_lead_even_if_planner_says_go(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - sm["carState"].standstill = True - sm["starpilotPlan"].redLight = True - toggles = make_toggles() - toggles.conditional_chill_launch_assist = True - - planner.tracking_lead = True - planner.lead_one.status = True - planner.lead_one.dRel = 6.1 - planner.lead_one.vLead = 0.01 - planner.lead_one.vRel = -0.08 - planner.lead_one.aLeadK = 0.0 - planner.lead_one.radar = True - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 22.0) - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_launch_assist_requires_real_lead_departure_signal(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - sm["carState"].standstill = True - toggles = make_toggles() - toggles.conditional_chill_launch_assist = True - - planner.tracking_lead = True - planner.lead_one.status = True - planner.lead_one.dRel = 18.0 - planner.lead_one.vLead = 0.12 - planner.lead_one.vRel = 0.05 - planner.lead_one.aLeadK = 0.02 - planner.lead_one.radar = True - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.0) - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - planner.lead_one.vLead = 0.8 - planner.lead_one.vRel = 0.5 - planner.lead_one.aLeadK = 0.2 - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.2) - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - - assert not ccm.experimental_mode - assert ccm.status_value == CCStatus["LEAD"] - - -def test_ccm_launch_assist_exits_once_launch_speed_is_reached(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - sm["carState"].standstill = True - toggles = make_toggles() - toggles.conditional_chill_launch_assist = True - monotonic_values = iter([30.0, 30.2]) - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) - - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - assert not ccm.experimental_mode - - sm["carState"].standstill = False - ccm.update(16 * CV.MPH_TO_MS, 25 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_launch_assist_exits_immediately_if_lead_slows_again(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - sm["carState"].standstill = True - toggles = make_toggles() - toggles.conditional_chill_launch_assist = True - monotonic_values = iter([40.0, 40.1]) - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) - - planner.tracking_lead = True - planner.lead_one.status = True - planner.lead_one.dRel = 18.0 - planner.lead_one.vLead = 2.0 - planner.lead_one.vRel = 2.0 - planner.lead_one.aLeadK = 0.2 - planner.lead_one.radar = True - - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - assert not ccm.experimental_mode - assert ccm.status_value == CCStatus["LEAD"] - - sm["carState"].standstill = False - planner.lead_one.vLead = 0.2 - planner.lead_one.vRel = 0.0 - planner.lead_one.aLeadK = -0.4 - ccm.update(3 * CV.MPH_TO_MS, 25 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_respects_manual_chill_override(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - planner.params_memory.put_int("CCStatus", CCStatus["USER_CHILL"]) - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0) - ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles) - - assert not ccm.experimental_mode - assert ccm.status_value == CCStatus["USER_CHILL"] - - -def test_ccm_launch_assist_is_disabled_by_default(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - sm["carState"].standstill = True - toggles = make_toggles() - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 50.0) - ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] - - -def test_ccm_restores_persisted_manual_experimental_override(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_sm() - toggles = make_toggles() - planner.params.put_bool("PersistChillState", True) - planner.params.put_int("PersistedCCStatus", CCStatus["USER_EXPERIMENTAL"]) - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0) - ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["USER_EXPERIMENTAL"] - assert planner.params_memory.get_int("CCStatus") == CCStatus["USER_EXPERIMENTAL"] - - -def test_ccm_adjacent_lead_veto_works_with_submaster_like_input(monkeypatch): - planner, _detector, ccm = make_ccm() - sm = make_submaster_sm() - toggles = make_toggles() - sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=20.0, vLead=12.0) - - monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0) - ccm.update(60 * CV.MPH_TO_MS, 65 * CV.MPH_TO_MS, sm, toggles) - - assert ccm.experimental_mode - assert ccm.status_value == CCStatus["OFF"] diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py deleted file mode 100644 index c0eb46890..000000000 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ /dev/null @@ -1,1115 +0,0 @@ -from pathlib import Path -from types import SimpleNamespace - -from openpilot.common.constants import CV -from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner -import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module -import openpilot.starpilot.controls.lib.conditional_experimental_mode as conditional_experimental_mode_module -from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode - - -class FakeParams: - def __init__(self, initial=None): - self._store = dict(initial or {}) - - def get_bool(self, key): - return bool(self._store.get(key, False)) - - def put_bool(self, key, value): - self._store[key] = bool(value) - - def get_int(self, key, default=0): - return int(self._store.get(key, default)) - - def put_int(self, key, value): - self._store[key] = int(value) - - -def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False, - lead_status: bool = False, lead_d_rel: float = float("inf"), - lead_v_lead: float = 0.0, lead_model_prob: float = 0.0, lead_radar: bool = False, - stop_sign_confirmed: bool = False, forcing_stop: bool = False): - planner = SimpleNamespace( - params=FakeParams(), - params_memory=FakeParams(), - model_length=model_length, - model_stopped=model_stopped, - tracking_lead=tracking_lead, - starpilot_vcruise=SimpleNamespace( - stop_sign_confirmed=stop_sign_confirmed, - forcing_stop=forcing_stop, - slc=SimpleNamespace(experimental_mode=False), - ), - starpilot_following=SimpleNamespace(slower_lead=False, following_lead=False), - lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel, vLead=lead_v_lead, - modelProb=lead_model_prob, radar=lead_radar), - road_curvature_detected=False, - driving_in_curve=False, - lane_width_left=0.0, - lane_width_right=0.0, - ) - return ConditionalExperimentalMode(planner) - - -def make_sm(traffic_mode_enabled: bool = False): - return { - "carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False, steeringAngleDeg=0.0), - "starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic_mode_enabled), - } - - -def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = False): - for _ in range(steps): - cem.starpilot_planner.tracking_lead = tracking_lead - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - - -def make_update_sm(*, standstill: bool): - return { - "carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False, gasPressed=False, steeringAngleDeg=0.0), - "starpilotCarState": SimpleNamespace(trafficModeEnabled=False, dashboardStopSign=0, accelPressed=False), - } - - -def make_update_toggles(): - return SimpleNamespace( - conditional_limit=0.0, - conditional_limit_lead=0.0, - conditional_signal=0.0, - conditional_signal_lane_detection=False, - lane_detection_width=0.0, - conditional_curves=False, - conditional_curves_lead=False, - conditional_lead=False, - conditional_model_stop_time=7.0, - conditional_slower_lead=False, - conditional_stopped_lead=False, - ) - - -def test_low_speed_cruise_does_not_trigger_stop_light_from_model_stopped(): - v_ego = 10 * CV.MPH_TO_MS - model_length = v_ego * 10.0 - - cem = make_cem(model_length=model_length, model_stopped=True) - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - - assert not cem.stop_light_detected - - -def test_predicted_stop_within_threshold_triggers_stop_light(): - v_ego = 30 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - - cem = make_cem(model_length=model_length) - run_stop_light_detector(cem, v_ego, steps=20) - - assert cem.stop_light_detected - - -def test_chattering_lead_does_not_trigger_stop_light(): - v_ego = 22 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - - cem = make_cem(model_length=model_length) - for i in range(30): - cem.starpilot_planner.tracking_lead = (i % 2 == 0) - cem.starpilot_planner.lead_one.status = (i % 2 == 0) - cem.starpilot_planner.lead_one.dRel = model_length + 5.0 - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - - assert not cem.stop_light_detected - - -def test_close_visible_but_untracked_lead_blocks_stop_light(): - v_ego = 22 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - - cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=model_length + 5.0) - run_stop_light_detector(cem, v_ego, steps=30) - - assert not cem.stop_light_detected - - -def test_far_visible_lead_does_not_block_stop_light(): - v_ego = 22 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - - cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=v_ego * 7.0 + 30.0) - run_stop_light_detector(cem, v_ego, steps=30) - - assert cem.stop_light_detected - - -def test_stop_light_stays_latched_until_untracked_stopped_lead_handoff(): - v_ego = 45 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - - cem = make_cem(model_length=model_length) - run_stop_light_detector(cem, v_ego, steps=30) - assert cem.stop_light_detected - - cem.starpilot_planner.lead_one.status = True - cem.starpilot_planner.lead_one.dRel = model_length - 5.0 - cem.starpilot_planner.lead_one.vLead = 0.5 - run_stop_light_detector(cem, v_ego, steps=10, tracking_lead=False) - - assert cem.stop_light_detected - - -def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_flicker(monkeypatch): - v_ego = 40 * CV.MPH_TO_MS - model_length = v_ego * 3.8 - cem = make_cem( - model_length=model_length, - lead_status=True, - lead_d_rel=model_length - 5.0, - lead_v_lead=3.0, - lead_model_prob=0.98, - ) - - run_stop_light_detector(cem, v_ego, steps=20) - assert cem.stop_light_detected - - monotonic_values = iter([10.0, 10.2]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - cem.stop_light_detected = False - cem.stop_light_model_detected = False - cem.stop_light_filter.x = 0.0 - cem.starpilot_planner.model_length = v_ego * 9.0 - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - - assert cem.stop_light_detected - - -def test_stop_light_hold_bridges_short_no_lead_model_flicker(monkeypatch): - v_ego = 40 * CV.MPH_TO_MS - cem = make_cem(model_length=v_ego * 3.8) - - run_stop_light_detector(cem, v_ego, steps=20) - assert cem.stop_light_detected - cem.stop_light_detected_hold_until = 11.75 - - monotonic_values = iter([10.0, 11.0, 14.5, 16.5, 18.5]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - cem.starpilot_planner.model_length = v_ego * 9.0 - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert not cem.stop_light_detected - - -def test_stop_light_hold_refreshes_through_stopped_approach_lead(monkeypatch): - v_ego = 20 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - cem = make_cem( - model_length=model_length, - lead_status=True, - lead_d_rel=model_length - 5.0, - lead_v_lead=0.5, - lead_model_prob=0.98, - ) - - run_stop_light_detector(cem, v_ego, steps=20) - assert cem.stop_light_detected - - monotonic_values = iter([20.0, 21.2, 22.4]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - cem.starpilot_planner.model_length = v_ego * 9.0 - cem.stop_light_detected = False - cem.stop_light_model_detected = False - cem.stop_light_filter.x = 0.0 - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected - - cem.starpilot_planner.lead_one.vLead = 6.0 - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert not cem.stop_light_detected - - -def test_stop_light_approach_latch_clears_once_tracked_lead_takes_over(monkeypatch): - v_ego = 20 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - cem = make_cem( - model_length=model_length, - lead_status=True, - lead_d_rel=model_length - 5.0, - lead_v_lead=0.5, - lead_model_prob=0.98, - ) - - run_stop_light_detector(cem, v_ego, steps=20) - assert cem.stop_light_detected - - monotonic_values = iter([20.0, 20.2]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - cem.starpilot_planner.model_length = v_ego * 9.0 - cem.stop_light_detected = False - cem.stop_light_model_detected = False - cem.stop_light_filter.x = 0.0 - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected - - cem.starpilot_planner.tracking_lead = True - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert not cem.stop_light_detected - - -def test_stopped_lead_handoff_does_not_hold_cem_on_empty_road(monkeypatch): - v_ego = 40 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 9.0, - tracking_lead=False, - lead_status=False, - ) - cem.stop_light_detected = True - cem.stop_light_model_detected = False - cem.stop_light_filter.x = 0.0 - cem.stop_approach_hold_until = 11.0 - cem.stop_light_detected_hold_until = 0.0 - - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: 10.2) - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - - assert not cem.stop_light_detected - - -def test_stopped_lead_toggle_does_not_self_trigger_without_a_real_lead(): - v_ego = 25 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 6.0, - tracking_lead=False, - lead_status=False, - lead_v_lead=0.0, - ) - toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True) - - for _ in range(24): - cem.slow_lead(toggles, v_ego) - - assert not cem.slow_lead_detected - - -def test_stopped_lead_toggle_still_triggers_for_real_close_stopped_lead(): - v_ego = 25 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 4.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=18.0, - lead_v_lead=0.4, - lead_model_prob=0.99, - ) - toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True) - - for _ in range(24): - cem.slow_lead(toggles, v_ego) - - assert cem.slow_lead_detected - - -def test_borderline_empty_road_model_dip_does_not_refresh_long_hold(monkeypatch): - v_ego = 20 * CV.MPH_TO_MS - stop_threshold = v_ego * 7.0 - cem = make_cem(model_length=stop_threshold - 4.0) - cem.stop_light_filter.x = conditional_experimental_mode_module.THRESHOLD ** 2 - - monotonic_values = iter([10.0, 10.2]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert cem.stop_light_detected_hold_until == 0.0 - - cem.starpilot_planner.model_length = stop_threshold + 20.0 - cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) - assert not cem.stop_light_detected - - -def test_standstill_red_light_keeps_exp_on_even_when_model_stopped_clears(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - - cem.stop_light_detected = True - cem.experimental_mode = False - monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) - - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] - - -def test_standstill_close_stopped_lead_does_not_drop_red_light_exp(monkeypatch): - cem = make_cem( - model_length=80.0, - model_stopped=False, - lead_status=True, - lead_d_rel=8.0, - lead_v_lead=0.0, - ) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - - cem.stop_light_detected = True - monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) - - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] - - -def test_standstill_update_can_activate_exp_from_red_light_detection(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - - def detect_red_light(*args, **kwargs): - cem.stop_light_detected = True - - monkeypatch.setattr(cem, "stop_sign_and_light", detect_red_light) - - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] - - -def test_first_post_standstill_pullaway_frame_does_not_blip_into_exp(monkeypatch): - cem = make_cem( - model_length=70.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=8.4, - lead_v_lead=2.7, - lead_model_prob=0.999, - ) - toggles = make_update_toggles() - toggles.conditional_lead = True - toggles.conditional_slower_lead = True - toggles.conditional_stopped_lead = True - standstill_sm = make_update_sm(standstill=True) - moving_sm = make_update_sm(standstill=False) - - now = [100.0] - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) - - cem.update(0.0, standstill_sm, toggles) - assert not cem.experimental_mode - - now[0] = 100.1 - cem.update(0.12, moving_sm, toggles) - - assert not cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["OFF"] - - -def test_post_stop_speed_trigger_is_suppressed_after_red_light_release(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - toggles.conditional_limit = 5.0 * CV.MPH_TO_MS - standstill_sm = make_update_sm(standstill=True) - moving_sm = make_update_sm(standstill=False) - - now = [100.0] - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) - - def hold_red_light(*args, **kwargs): - cem.stop_light_detected = True - - monkeypatch.setattr(cem, "stop_sign_and_light", hold_red_light) - cem.update(0.0, standstill_sm, toggles) - assert cem.experimental_mode - - def clear_red_light(*args, **kwargs): - cem.stop_light_detected = False - cem.stop_light_model_detected = False - - monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light) - - low_speed_trigger_v_ego = 4.0 * CV.MPH_TO_MS - - now[0] = 100.1 - cem.update(low_speed_trigger_v_ego, moving_sm, toggles) - assert not cem.experimental_mode - assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] - - now[0] = 102.5 - cem.update(low_speed_trigger_v_ego, moving_sm, toggles) - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"] - - -def test_post_stop_suppression_survives_hold_release_before_motion(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - toggles.conditional_limit = 5.0 * CV.MPH_TO_MS - standstill_sm = make_update_sm(standstill=True) - moving_sm = make_update_sm(standstill=False) - - now = [100.0] - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) - - def hold_red_light(*args, **kwargs): - cem.stop_light_detected = True - - monkeypatch.setattr(cem, "stop_sign_and_light", hold_red_light) - cem.update(0.0, standstill_sm, toggles) - assert cem.experimental_mode - - def clear_red_light(*args, **kwargs): - cem.stop_light_detected = False - cem.stop_light_model_detected = False - - monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light) - - now[0] = 100.1 - cem.update(0.0, standstill_sm, toggles) - assert not cem.experimental_mode - assert cem.standstill_stop_release_pending - - low_speed_trigger_v_ego = 4.0 * CV.MPH_TO_MS - - now[0] = 100.2 - cem.update(low_speed_trigger_v_ego, moving_sm, toggles) - assert not cem.experimental_mode - assert not cem.standstill_stop_release_pending - - now[0] = 102.5 - cem.update(low_speed_trigger_v_ego, moving_sm, toggles) - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"] - - -def test_post_stop_slow_lead_trigger_is_suppressed_after_red_light_release(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - toggles.conditional_lead = True - toggles.conditional_slower_lead = True - toggles.conditional_stopped_lead = True - standstill_sm = make_update_sm(standstill=True) - moving_sm = make_update_sm(standstill=False) - - now = [100.0] - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) - - def hold_red_light(*args, **kwargs): - cem.stop_light_detected = True - - def clear_red_light(*args, **kwargs): - cem.stop_light_detected = False - cem.stop_light_model_detected = False - - def detect_slow_lead(*args, **kwargs): - cem.slow_lead_detected = True - - monkeypatch.setattr(cem, "stop_sign_and_light", hold_red_light) - cem.update(0.0, standstill_sm, toggles) - assert cem.experimental_mode - - monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light) - monkeypatch.setattr(cem, "slow_lead", detect_slow_lead) - - launch_v_ego = 8.0 * CV.MPH_TO_MS - - now[0] = 100.1 - cem.update(launch_v_ego, moving_sm, toggles) - assert not cem.experimental_mode - assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] - - now[0] = 102.5 - cem.update(launch_v_ego, moving_sm, toggles) - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"] - - -def test_slow_lead_mode_release_waits_for_stable_credible_lead(monkeypatch): - v_ego = 20.0 - cem = make_cem( - model_length=100.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=34.0, - lead_v_lead=18.5, - lead_model_prob=0.99, - ) - toggles = make_update_toggles() - toggles.conditional_lead = True - toggles.conditional_slower_lead = True - sm = make_update_sm(standstill=False) - slow_lead_active = [True] - now = [100.0] - - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) - - def update_conditions(*args, **kwargs): - cem.slow_lead_detected = slow_lead_active[0] - - monkeypatch.setattr(cem, "update_conditions", update_conditions) - - cem.update(v_ego, sm, toggles) - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"] - - slow_lead_active[0] = False - now[0] = 100.9 - cem.update(v_ego, sm, toggles) - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"] - - now[0] = 101.6 - cem.update(v_ego, sm, toggles) - assert not cem.experimental_mode - assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] - - -def test_slow_lead_mode_release_does_not_hold_missing_lead(monkeypatch): - v_ego = 20.0 - cem = make_cem( - model_length=100.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=34.0, - lead_v_lead=18.5, - lead_model_prob=0.99, - ) - toggles = make_update_toggles() - toggles.conditional_lead = True - toggles.conditional_slower_lead = True - sm = make_update_sm(standstill=False) - slow_lead_active = [True] - now = [200.0] - - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) - - def update_conditions(*args, **kwargs): - cem.slow_lead_detected = slow_lead_active[0] - - monkeypatch.setattr(cem, "update_conditions", update_conditions) - - cem.update(v_ego, sm, toggles) - assert cem.experimental_mode - - slow_lead_active[0] = False - cem.starpilot_planner.lead_one.status = False - cem.starpilot_planner.tracking_lead = False - now[0] = 200.6 - cem.update(v_ego, sm, toggles) - assert cem.experimental_mode - - now[0] = 200.9 - cem.update(v_ego, sm, toggles) - - assert not cem.experimental_mode - assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] - - -def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - sm["starpilotCarState"].dashboardStopSign = 1 - - monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) - - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.standstill_stop_reason == "sign" - assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] - - -def test_standstill_green_light_clears_exp_immediately(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - - cem.stop_light_detected = True - cem.experimental_mode = True - - def clear_red_light(*args, **kwargs): - cem.stop_light_detected = False - cem.stop_light_model_detected = False - - monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light) - - cem.update(0.0, sm, toggles) - - assert not cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["OFF"] - - -def test_standstill_dashboard_stop_sign_keeps_exp_on(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False, stop_sign_confirmed=True) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - - monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) - - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] - - -def test_standstill_force_stop_keeps_exp_on_even_if_red_light_latch_clears(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False, forcing_stop=True) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - - cem.stop_light_detected = True - cem.experimental_mode = True - - def clear_red_light(*args, **kwargs): - cem.stop_light_detected = False - cem.stop_light_model_detected = False - - monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light) - - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] - - -def test_committed_turn_veto_blocks_stop_light_detection(): - v_ego = 10 * CV.MPH_TO_MS - model_length = v_ego * 4.0 - - cem = make_cem(model_length=model_length) - sm = make_sm() - sm["carState"].rightBlinker = True - sm["carState"].steeringAngleDeg = 70.0 - - run_stop_light_detector(cem, v_ego, steps=20) - assert cem.stop_light_detected - - cem.stop_light_detected = False - cem.stop_light_model_detected = False - cem.stop_light_filter.x = 0.0 - - for _ in range(20): - cem.stop_sign_and_light(v_ego, sm, model_time=7.0) - - assert not cem.stop_light_detected - - -def test_standstill_stop_sign_latches_until_pedal_even_after_force_stop_ends(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - sm["starpilotCarState"].dashboardStopSign = 1 - - monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.standstill_stop_reason == "sign" - - sm["starpilotCarState"].dashboardStopSign = 0 - cem.update(0.0, sm, toggles) - - assert cem.experimental_mode - assert cem.standstill_stop_reason == "sign" - - -def test_standstill_stop_sign_releases_on_pedal(monkeypatch): - cem = make_cem(model_length=80.0, model_stopped=False) - toggles = make_update_toggles() - sm = make_update_sm(standstill=True) - sm["starpilotCarState"].dashboardStopSign = 1 - - monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) - cem.update(0.0, sm, toggles) - assert cem.experimental_mode - - sm["starpilotCarState"].dashboardStopSign = 0 - sm["carState"].gasPressed = True - cem.update(0.0, sm, toggles) - - assert not cem.experimental_mode - assert cem.standstill_stop_reason is None - - -def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead(): - v_ego = 35 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=v_ego * 3.5, - lead_v_lead=8.0 * CV.MPH_TO_MS, - lead_model_prob=0.95, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 1.0 - cem.slow_lead_detected = True - - cem.starpilot_planner.tracking_lead = False - cem.starpilot_planner.starpilot_following.slower_lead = False - cem.slow_lead(toggles, v_ego) - assert cem.slow_lead_detected - - -def test_untracked_raw_slow_lead_does_not_self_trigger_without_recent_tracking(monkeypatch): - v_ego = 35 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=False, - lead_status=True, - lead_d_rel=v_ego * 3.0, - lead_v_lead=8.0 * CV.MPH_TO_MS, - lead_model_prob=0.95, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - monotonic_values = iter([20.0 + 0.1 * i for i in range(24)]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - for _ in range(12): - cem.slow_lead(toggles, v_ego) - - assert not cem.slow_lead_detected - - -def test_untracked_raw_slow_lead_continuity_expires_after_tracking_flap(monkeypatch): - v_ego = 35 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=v_ego * 3.0, - lead_v_lead=8.0 * CV.MPH_TO_MS, - lead_model_prob=0.95, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 1.0 - cem.slow_lead_detected = True - monotonic_values = iter([30.0, 30.1, 31.6]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - cem.slow_lead(toggles, v_ego) - assert cem.slow_lead_detected - - cem.starpilot_planner.tracking_lead = False - cem.starpilot_planner.starpilot_following.slower_lead = False - cem.slow_lead(toggles, v_ego) - assert cem.slow_lead_detected - - cem.slow_lead(toggles, v_ego) - assert not cem.slow_lead_detected - - -def test_tracked_highway_mild_closing_lead_does_not_trigger_raw_slow_lead(): - v_ego = 65 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=46.0, - lead_v_lead=v_ego - 1.4, - lead_model_prob=1.0, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 0.0 - cem.slow_lead_detected = False - cem.starpilot_planner.starpilot_following.slower_lead = False - for _ in range(12): - cem.slow_lead(toggles, v_ego) - - assert not cem.slow_lead_detected - - -def test_tracked_following_slower_lead_still_triggers_slow_lead(): - v_ego = 65 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=36.0, - lead_v_lead=v_ego - 3.0, - lead_model_prob=1.0, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 0.0 - cem.slow_lead_detected = False - cem.starpilot_planner.starpilot_following.slower_lead = True - for _ in range(24): - cem.slow_lead(toggles, v_ego) - - assert cem.slow_lead_detected - - -def test_far_radar_slower_lead_waits_for_comfort_range_before_triggering(): - v_ego = 75 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=136.0, - lead_v_lead=v_ego - 5.0, - lead_model_prob=1.0, - lead_radar=True, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - cem.starpilot_planner.starpilot_following.slower_lead = True - - for _ in range(24): - cem.slow_lead(toggles, v_ego) - assert not cem.slow_lead_detected - - cem.starpilot_planner.lead_one.dRel = 80.0 - for _ in range(24): - cem.slow_lead(toggles, v_ego) - assert cem.slow_lead_detected - - -def test_far_vision_slower_lead_keeps_existing_trigger_range(): - v_ego = 75 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=110.0, - lead_v_lead=v_ego - 5.0, - lead_model_prob=1.0, - lead_radar=False, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - cem.starpilot_planner.starpilot_following.slower_lead = True - - for _ in range(24): - cem.slow_lead(toggles, v_ego) - assert cem.slow_lead_detected - - -def test_tracked_vision_slow_lead_continues_existing_experimental_mode(): - v_ego = 8.0 - cem = make_cem( - model_length=80.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=22.0, - lead_v_lead=6.4, - lead_model_prob=0.99, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.prev_experimental_mode = True - for _ in range(24): - cem.slow_lead(toggles, v_ego) - - assert cem.slow_lead_detected - - -def test_tracked_vision_slow_lead_does_not_start_experimental_mode(): - v_ego = 8.0 - cem = make_cem( - model_length=80.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=22.0, - lead_v_lead=6.4, - lead_model_prob=0.99, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - for _ in range(24): - cem.slow_lead(toggles, v_ego) - - assert not cem.slow_lead_detected - - -def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled(): - v_ego = 1.5 - cem = make_cem( - model_length=20.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=8.0, - lead_v_lead=1.2, - lead_model_prob=0.99, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 1.0 - cem.slow_lead_detected = True - cem.starpilot_planner.starpilot_following.slower_lead = False - cem.slow_lead(toggles, v_ego) - - assert not cem.slow_lead_detected - - -def test_far_untracked_slow_lead_does_not_trigger_slow_lead(): - v_ego = 50 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=False, - lead_status=True, - lead_d_rel=v_ego * 4.8, - lead_v_lead=v_ego - 2.0, - lead_model_prob=0.97, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 1.0 - cem.slow_lead_detected = True - cem.starpilot_planner.starpilot_following.slower_lead = False - cem.slow_lead(toggles, v_ego) - - assert not cem.slow_lead_detected - - -def test_pace_matched_lead_clears_slow_lead_quickly(): - v_ego = 30 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 4.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=24.0, - lead_v_lead=v_ego + 0.2, - lead_model_prob=0.99, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 1.0 - cem.slow_lead_detected = True - cem.starpilot_planner.starpilot_following.slower_lead = False - cem.slow_lead(toggles, v_ego) - - assert not cem.slow_lead_detected - - -def test_tracked_stale_slow_lead_clears_after_short_timeout(monkeypatch): - v_ego = 65 * CV.MPH_TO_MS - cem = make_cem( - model_length=v_ego * 5.0, - tracking_lead=True, - lead_status=True, - lead_d_rel=55.0, - lead_v_lead=v_ego - 0.8, - lead_model_prob=0.98, - ) - toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) - - cem.slow_lead_filter.x = 1.0 - cem.slow_lead_detected = True - cem.starpilot_planner.starpilot_following.slower_lead = False - - monotonic_values = iter([50.0, 50.3, 50.9]) - monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) - - cem.slow_lead(toggles, v_ego) - assert cem.slow_lead_detected - - cem.slow_lead(toggles, v_ego) - assert cem.slow_lead_detected - - cem.slow_lead(toggles, v_ego) - assert not cem.slow_lead_detected - - -class DummyThemeManager: - def update_wheel_image(self, *args, **kwargs): - pass - - -def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch): - planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) - - try: - monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 1.0)) - monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None) - monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None) - monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0) - monkeypatch.setattr(planner.starpilot_weather, "update_weather", lambda *args, **kwargs: None) - - def following_update(*args, **kwargs): - planner.starpilot_following.following_lead = planner.tracking_lead - planner.starpilot_following.slower_lead = planner.tracking_lead - - monkeypatch.setattr(planner.starpilot_following, "update", following_update) - - seen = {} - - def cem_update(v_ego, sm, starpilot_toggles): - seen.update({ - "tracking_lead": planner.tracking_lead, - "following_lead": planner.starpilot_following.following_lead, - "slower_lead": planner.starpilot_following.slower_lead, - "model_length": planner.model_length, - "driving_in_curve": planner.driving_in_curve, - "road_curvature_detected": planner.road_curvature_detected, - }) - - monkeypatch.setattr(planner.starpilot_cem, "update", cem_update) - - planner.model_length = 0.0 - planner.tracking_lead = False - planner.driving_in_curve = False - planner.lateral_acceleration = 0.0 - planner.road_curvature_detected = False - planner.starpilot_following.following_lead = False - planner.starpilot_following.slower_lead = False - - starpilot_toggles = SimpleNamespace( - set_speed_offset=0, - conditional_experimental_mode=True, - minimum_lane_change_speed=100.0, - pause_lateral_below_speed=0.0, - pause_lateral_below_signal=False, - weather_presets=False, - ) - - for controls_enabled, aol_enabled in ((True, False), (False, True)): - seen.clear() - - sm = { - "radarState": SimpleNamespace(leadOne=SimpleNamespace(status=True, dRel=25.0, vLead=0.5)), - "selfdriveState": SimpleNamespace(enabled=controls_enabled), - "carState": SimpleNamespace(vCruise=50.0, vEgo=20.0, standstill=False, leftBlinker=False, rightBlinker=False), - "controlsState": SimpleNamespace(curvature=0.02), - "modelV2": SimpleNamespace(position=SimpleNamespace(x=[0.0, 30.0]), laneLines=[None] * 4, roadEdges=[None] * 2), - "starpilotCarState": SimpleNamespace(pauseLateral=False, alwaysOnLateralEnabled=aol_enabled), - planner.gps_location_service: SimpleNamespace(latitude=1.0, longitude=1.0, bearingDeg=90.0), - } - - planner.tracking_lead_filter.x = 1.0 - planner.update(0.0, False, sm, starpilot_toggles) - - assert seen == { - "tracking_lead": True, - "following_lead": True, - "slower_lead": True, - "model_length": 30.0, - "driving_in_curve": True, - "road_curvature_detected": True, - } - finally: - planner.shutdown() diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 0fd543dd6..569e6fa77 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -30,7 +30,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personalit [log.LongitudinalPersonality.relaxed, # personality log.LongitudinalPersonality.standard, log.LongitudinalPersonality.aggressive], - [0,10,35])) # speed + [1,10,35])) # speed; 0 m/s cannot converge from an arbitrary initial gap class TestFollowingDistance: def test_following_distance(self): v_lead = float(self.speed) diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py deleted file mode 100644 index 2df853e93..000000000 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ /dev/null @@ -1,196 +0,0 @@ -from openpilot.selfdrive.controls.lib.lead_behavior import ( - get_tracked_lead_catchup_bias, - is_radarless_matched_follow_window, - should_hold_tracked_vision_lead, - should_track_lead, - should_disable_far_lead_throttle, -) - - -def test_tracked_lead_catchup_bias_for_hanging_gap(): - bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1) - assert bias > 10.0 - - -def test_tracked_lead_catchup_bias_ignores_near_desired_gap(): - bias = get_tracked_lead_catchup_bias(31.4, 50.0, 38.0, 0.1) - assert bias == 0.0 - - -def test_tracked_lead_catchup_bias_ignores_very_far_gap(): - bias = get_tracked_lead_catchup_bias(31.4, 110.0, 38.0, 0.1) - assert bias == 0.0 - - -def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap(): - bias = get_tracked_lead_catchup_bias(30.4, 63.0, 40.0, 0.4) - assert bias > 9.0 - - -def test_tracked_lead_catchup_bias_reduces_for_laterally_offset_lead(): - centered = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=0.2) - offset = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=1.95) - assert centered > 0.0 - assert offset == 0.0 - - -def test_tracked_lead_catchup_bias_fades_before_very_far_gap_cutoff(): - near_upper = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9) - smaller_gap = get_tracked_lead_catchup_bias(34.0, 96.0, 73.0, 1.9) - assert near_upper > 0.0 - assert near_upper < smaller_gap - - -def test_tracked_lead_catchup_bias_stays_off_once_at_set_speed(): - bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1, v_cruise=31.4) - assert bias == 0.0 - - -def test_tracked_lead_catchup_bias_fades_smoothly_near_set_speed(): - below_set = get_tracked_lead_catchup_bias(27.70, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8) - at_set = get_tracked_lead_catchup_bias(27.78, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8) - assert 0.0 < below_set < 0.25 - assert at_set == 0.0 - - -def test_tracked_lead_catchup_bias_dials_back_camry_bookmark_case(): - bias = get_tracked_lead_catchup_bias(27.40, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8) - assert 0.0 < bias < 2.0 - - -def test_tracked_lead_catchup_bias_fades_smoothly_at_closing_limit(): - below_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.75) - above_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.77) - assert abs(below_limit - above_limit) < 0.1 - - -def test_disable_far_lead_throttle_rejects_two_second_plus_gap(): - should_disable = should_disable_far_lead_throttle(31.4, 78.7, 38.0, 0.1, False) - assert not should_disable - - -def test_disable_far_lead_throttle_keeps_mild_coast_near_target_gap(): - should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 0.5, False) - assert should_disable - - -def test_disable_far_lead_throttle_rejects_fast_closing(): - should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 3.5, False) - assert not should_disable - - -def test_disable_far_lead_throttle_rejects_route_like_highway_stab_case(): - should_disable = should_disable_far_lead_throttle(34.69, 68.5, 63.0, 2.31, False) - assert not should_disable - - -def test_disable_far_lead_throttle_rejects_large_gap_near_pace_matched_case(): - should_disable = should_disable_far_lead_throttle(32.43, 72.4, 56.0, 1.30, False) - assert not should_disable - - -def test_should_track_lead_keeps_radar_leads_on_model_horizon(): - assert should_track_lead(True, 95.0, 100.0, 6.0, 30.0, v_lead=25.0, radar=True) - - -def test_should_track_lead_rejects_far_vision_only_highway_lead(): - assert not should_track_lead(True, 82.0, 140.0, 6.0, 29.0, v_lead=25.0, radar=False) - - -def test_should_track_lead_accepts_closer_vision_only_highway_lead(): - assert should_track_lead(True, 56.0, 140.0, 6.0, 29.0, v_lead=25.0, radar=False) - - -def test_should_track_lead_accepts_fast_closing_vision_lead_early(): - assert should_track_lead(True, 90.0, 140.0, 6.0, 20.0, v_lead=0.0, radar=False) - - -def test_should_hold_tracked_vision_lead_keeps_honda_bookmark_case(): - assert should_hold_tracked_vision_lead( - True, 44.5, 174.0, 6.0, 16.8, - model_prob=0.99, y_rel=-0.69, radar=False, - ) - - -def test_should_hold_tracked_vision_lead_does_not_expand_initial_tracking_gate(): - assert not should_track_lead(True, 44.5, 174.0, 6.0, 16.8, v_lead=16.7, radar=False) - - -def test_should_hold_tracked_vision_lead_releases_offcenter_lead(): - assert not should_hold_tracked_vision_lead( - True, 44.5, 174.0, 6.0, 16.8, - model_prob=0.99, y_rel=-1.7, radar=False, - ) - - -def test_should_hold_tracked_vision_lead_uses_path_relative_offset_on_curve(): - assert should_hold_tracked_vision_lead( - True, 27.8, 174.0, 6.0, 16.0, - model_prob=1.0, y_rel=-2.11, path_y=1.32, radar=False, - ) - - -def test_should_hold_tracked_vision_lead_releases_low_confidence_lead(): - assert not should_hold_tracked_vision_lead( - True, 44.5, 174.0, 6.0, 16.8, - model_prob=0.69, y_rel=0.0, radar=False, - ) - - -def test_should_hold_tracked_vision_lead_does_not_change_radar_tracking(): - assert not should_hold_tracked_vision_lead( - True, 44.5, 174.0, 6.0, 16.8, - model_prob=0.99, y_rel=0.0, radar=True, - ) - - -def test_should_hold_tracked_vision_lead_keeps_braking_lead(): - assert should_hold_tracked_vision_lead( - True, 44.5, 174.0, 6.0, 16.8, - model_prob=0.99, y_rel=0.0, radar=False, - ) - - -def test_should_hold_tracked_vision_lead_releases_beyond_exit_gap(): - assert not should_hold_tracked_vision_lead( - True, 57.0, 174.0, 6.0, 16.8, - model_prob=0.99, y_rel=0.0, radar=False, - ) - - -def test_should_hold_tracked_vision_lead_ignores_shortened_model_horizon_in_bolt_stutter_case(): - assert should_hold_tracked_vision_lead( - True, 54.6, 40.0, 6.0, 19.4, - model_prob=1.0, y_rel=0.05, radar=False, - ) - - -def test_should_hold_tracked_vision_lead_does_not_extend_low_confidence_short_horizon_case(): - assert not should_hold_tracked_vision_lead( - True, 54.6, 40.0, 6.0, 19.4, - model_prob=0.90, y_rel=0.05, radar=False, - ) - - -def test_radarless_matched_follow_window_accepts_pace_matched_highway_follow(): - assert is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95) - - -def test_radarless_matched_follow_window_rejects_large_relative_speed(): - assert not is_radarless_matched_follow_window(31.0, 48.0, 27.5, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95) - - -def test_radarless_matched_follow_window_rejects_far_headway(): - assert not is_radarless_matched_follow_window(31.0, 82.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95) - - -def test_radarless_matched_follow_window_rejects_low_confidence_lead(): - assert not is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.55) - - -def test_radarless_matched_follow_window_keeps_default_low_speed_guard(): - assert not is_radarless_matched_follow_window(14.4, 25.4, 16.2, 1.25, radar=False, lead_brake=0.0, lead_prob=1.0) - - -def test_radarless_matched_follow_window_accepts_lower_speed_when_requested(): - assert is_radarless_matched_follow_window(14.4, 25.4, 16.2, 1.25, radar=False, lead_brake=0.0, lead_prob=1.0, min_speed=12.0) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index cd81aff5c..bf94fd806 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1,6 +1,3 @@ -import time -import sys -import types from types import SimpleNamespace import numpy as np @@ -10,244 +7,78 @@ from cereal import log from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR from opendbc.car.gm.values import CAR as GM_CAR, GMFlags -import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module + +import openpilot.selfdrive.controls.lib.longitudinal_planner as planner_module from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, soften_far_radar_lead_accel, should_trigger_planner_fcw -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.modeld.constants import ModelConstants, Plan +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( + T_IDXS, + LongitudinalMpc, + should_trigger_planner_fcw, +) +from openpilot.selfdrive.controls.lib.longitudinal_planner import ( + MIN_PLAN_HORIZON, + LongitudinalPlanner, + PlanState, + get_vehicle_min_accel, +) +from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_longitudinal_planner_tune +from openpilot.selfdrive.modeld.constants import ModelConstants -def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0, - radar: bool = False, model_prob: float = 0.0, y_rel: float = 0.0): +def make_lead(*, status=False, d_rel=200.0, v_lead=0.0, a_lead=0.0, + radar=False, model_prob=1.0): lead = log.RadarState.LeadData.new_message() lead.status = status lead.dRel = d_rel lead.vLead = v_lead lead.vLeadK = v_lead - lead.aLeadK = a_lead lead.vRel = 0.0 - lead.aRel = 0.0 - lead.yRel = y_rel + lead.aLeadK = a_lead + lead.aLeadTau = 1.5 lead.modelProb = model_prob lead.radar = radar return lead -def test_mpc_duplicate_lead_filters_do_not_cross_contaminate_tracks(): - mpc = LongitudinalMpc() - mpc.set_cur_state(20.0, 0.0) - mpc.current_filter_time = 0.5 - lead_one = make_lead(status=True, d_rel=35.0, v_lead=12.0, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=35.0, v_lead=28.0, model_prob=1.0) - - mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True) - mpc.process_lead(lead_two, lead_index=1, smooth_duplicate_vision=True) - - assert mpc.duplicate_lead_v_filters[0].x == pytest.approx(12.0) - assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0) - - lead_one.vLead = 14.0 - mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True) - assert 12.0 < mpc.duplicate_lead_v_filters[0].x < 14.0 - assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0) - - -def test_mpc_duplicate_vision_filter_damps_low_speed_velocity_noise(): - mpc = LongitudinalMpc() - mpc.set_cur_state(18.0, 0.0) - mpc.current_filter_time = 0.0 - lead = make_lead(status=True, d_rel=38.0, v_lead=17.0, model_prob=1.0) - - mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True) - lead.vLead = 20.0 - mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True) - - assert 17.0 < mpc.duplicate_lead_v_filters[0].x < 20.0 - - -def test_mpc_distinct_vision_lead_uses_unchanged_baseline_filter(): - mpc = LongitudinalMpc() - baseline_mpc = LongitudinalMpc() - mpc.set_cur_state(18.0, 0.0) - baseline_mpc.set_cur_state(18.0, 0.0) - mpc.current_filter_time = 0.0 - baseline_mpc.current_filter_time = 0.0 - lead = make_lead(status=True, d_rel=38.0, v_lead=17.0, model_prob=1.0) - - mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False) - baseline_mpc.process_lead(lead) - lead.vLead = 20.0 - mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False) - baseline_mpc.process_lead(lead) - - assert mpc.lead_v_filter.x == pytest.approx(baseline_mpc.lead_v_filter.x) - - -def test_mpc_panic_bypass_immediately_removes_duplicate_vision_filter(): - mpc = LongitudinalMpc() - mpc.set_cur_state(18.0, 0.0) - mpc.current_filter_time = 0.0 - lead = make_lead(status=True, d_rel=25.0, v_lead=17.0, model_prob=1.0) - - mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True) - mpc.set_weights(v_ego=18.0, panic_bypass=True) - lead.vLead = 10.0 - mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False) - - assert mpc.filter_time_factor == 0.0 - assert mpc.lead_v_filter.x == pytest.approx(10.0) - - -def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow(): - v_ego = 24.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) - planner.lead_two = make_lead(status=False) - - initial = planner.get_vehicle_far_follow_slew_target( - v_ego, prev_target=0.0, target=-0.6, output_should_stop=False, panic_bypass=False, - ) - - assert initial == pytest.approx(-0.6) - - smoothed = planner.get_vehicle_far_follow_slew_target( - v_ego, prev_target=initial, target=0.4, output_should_stop=False, panic_bypass=False, - ) - assert smoothed == pytest.approx(-0.5) - - -@pytest.mark.parametrize("d_rel,v_lead,output_should_stop,panic_bypass", [ - (20.0, 20.0, False, False), - (35.0, 18.0, False, False), - (58.0, 20.0, True, False), - (58.0, 20.0, False, True), -]) -def test_hrv_far_follow_output_slew_bypasses_urgent_scenes(d_rel, v_lead, output_should_stop, panic_bypass): - v_ego = 24.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) - planner.lead_two = make_lead(status=False) - planner.far_follow_output_slew_active = True - planner.lead_one.dRel = d_rel - planner.lead_one.vLead = v_lead - - target = planner.get_vehicle_far_follow_slew_target( - v_ego, prev_target=0.4, target=-1.0, - output_should_stop=output_should_stop, panic_bypass=panic_bypass, - ) - - assert target == pytest.approx(-1.0) - assert not planner.far_follow_output_slew_active - - -def test_non_hrv_has_no_vehicle_far_follow_output_slew(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=24.0) - planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99) - planner.lead_two = make_lead(status=False) - - target = planner.get_vehicle_far_follow_slew_target( - 24.0, prev_target=0.4, target=-1.0, output_should_stop=False, panic_bypass=False, - ) - - assert target == pytest.approx(-1.0) - - -def test_depart_release_hold_rejects_nearby_stopped_lead_conflict(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP) - planner.lead_one = make_lead(status=True, d_rel=4.0, v_lead=0.8, a_lead=0.2, model_prob=1.0) - planner.lead_two = make_lead(status=True, d_rel=5.5, v_lead=0.0, model_prob=1.0) - - assert planner.get_safe_depart_release_hold_lead(0.0) is None - - -def test_depart_release_hold_survives_stale_stop_then_cancels_for_braking_lead(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP) - lead_one = make_lead(status=True, d_rel=4.0, v_lead=0.8, a_lead=0.2, model_prob=1.0) - lead_two = make_lead(status=False) - sm = make_sm( - 0.0, - desired_accel=0.4, - min_accel=-1.0, - experimental_mode=True, - tracking_lead=True, - lead_one=lead_one, - lead_two=lead_two, - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = True - sm["modelV2"].position.x = [0.0] * len(ModelConstants.T_IDXS) - sm["modelV2"].velocity.x = [0.0] * len(ModelConstants.T_IDXS) - sm["modelV2"].acceleration.x = [0.0] * len(ModelConstants.T_IDXS) - planner.lead_depart_release_candidate_elapsed = longitudinal_planner_module.LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME - planner.lead_depart_release_hold_remaining = 1.0 - - planner.update(sm, make_toggles()) - - assert not planner.output_should_stop - assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL - - lead_one.vLead = 0.1 - lead_one.aLeadK = -0.5 - planner.update(sm, make_toggles()) - assert planner.lead_depart_release_hold_remaining == 0.0 - assert planner.output_should_stop - - -def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0): +def make_model(v_ego, desired_accel=0.0): model = log.ModelDataV2.new_message() - model.init('leadsV3', 3) - t_idxs = ModelConstants.T_IDXS - - model.position.x = [float(v_ego * t) for t in t_idxs] - model.position.y = [0.0] * len(t_idxs) - model.position.z = [0.0] * len(t_idxs) - model.position.t = [float(t) for t in t_idxs] - - model.velocity.x = [float(v_ego)] * len(t_idxs) - model.velocity.y = [0.0] * len(t_idxs) - model.velocity.z = [0.0] * len(t_idxs) - model.velocity.t = [float(t) for t in t_idxs] - - model.acceleration.x = [0.0] * len(t_idxs) - model.acceleration.y = [0.0] * len(t_idxs) - model.acceleration.z = [0.0] * len(t_idxs) - model.acceleration.t = [float(t) for t in t_idxs] - - model.meta.disengagePredictions.gasPressProbs = [float(gas_press_prob)] * 6 - model.meta.disengagePredictions.brakePressProbs = [float(brake_press_prob)] * 6 + times = np.asarray(ModelConstants.T_IDXS) + model.position.x = (v_ego * times).tolist() + model.position.y = [0.0] * len(times) + model.position.z = [0.0] * len(times) + model.position.t = times.tolist() + model.velocity.x = [v_ego] * len(times) + model.velocity.y = [0.0] * len(times) + model.velocity.z = [0.0] * len(times) + model.velocity.t = times.tolist() + model.acceleration.x = [0.0] * len(times) + model.acceleration.y = [0.0] * len(times) + model.acceleration.z = [0.0] * len(times) + model.acceleration.t = times.tolist() + model.meta.disengagePredictions.gasPressProbs = [1.0] * 6 model.action.desiredAcceleration = desired_accel model.action.shouldStop = False return model -def set_model_lead(model, idx: int, *, prob: float, x0: float, y0: float, v0: float, a0: float = 0.0): - lead = model.leadsV3[idx] - lead.prob = float(prob) - lead.x = [float(x0)] - lead.y = [float(y0)] - lead.v = [float(v0)] - lead.a = [float(a0)] +def set_model_departure(model, accel=0.5): + times = np.asarray(ModelConstants.T_IDXS) + model.position.x = (2.0 * times + 0.5 * accel * times ** 2).tolist() + model.velocity.x = (2.0 + accel * times).tolist() + model.acceleration.x = [accel] * len(times) + model.action.desiredAcceleration = accel + model.action.shouldStop = False -def set_model_launch_trajectory(model, *, wait_time: float = 0.6, accel: float = 1.0): - times = np.asarray(ModelConstants.T_IDXS, dtype=float) - moving_time = np.maximum(times - wait_time, 0.0) - model.position.x = (0.5 * accel * moving_time ** 2).tolist() - model.velocity.x = (accel * moving_time).tolist() - model.acceleration.x = np.where(times >= wait_time, accel, 0.0).tolist() +def set_model_stop(model, distance=5.0): + count = len(ModelConstants.T_IDXS) + model.position.x = np.linspace(0.0, distance, count).tolist() + model.velocity.x = np.linspace(10.0, 0.0, count).tolist() + model.action.desiredAcceleration = -1.0 -def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True, - tracking_lead: bool = False, lead_one=None, lead_two=None, - gas_press_prob: float = 1.0, brake_press_prob: float = 0.0, disable_throttle: bool = False): +def make_sm(v_ego=20.0, *, v_cruise=None, model_accel=0.0, lead_one=None, lead_two=None): + v_cruise = v_ego + 5.0 if v_cruise is None else v_cruise return { "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]), "carState": SimpleNamespace( @@ -255,72 +86,233 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta vEgoCluster=v_ego, aEgo=0.0, vCruise=100.0, - standstill=False, + standstill=v_ego == 0.0, steeringAngleDeg=0.0, + brakePressed=False, + leftBlinker=False, + rightBlinker=False, ), - "controlsState": SimpleNamespace( - longControlState=LongCtrlState.pid, - forceDecel=False, - ), + "controlsState": SimpleNamespace(longControlState=LongCtrlState.pid, forceDecel=False), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), - "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob, brake_press_prob=brake_press_prob), + "modelV2": make_model(v_ego, model_accel), "radarState": SimpleNamespace( - leadOne=lead_one if lead_one is not None else make_lead(status=False), - leadTwo=lead_two if lead_two is not None else make_lead(status=False), + leadOne=lead_one or make_lead(), + leadTwo=lead_two or make_lead(), + ), + "selfdriveState": SimpleNamespace( + enabled=True, + personality=log.LongitudinalPersonality.standard, ), - "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=experimental_mode, personality=0), - "starpilotCarState": SimpleNamespace(accelPressed=False), "starpilotPlan": SimpleNamespace( - vCruise=v_ego + 5.0, - minAcceleration=min_accel, + vCruise=v_cruise, + minAcceleration=-3.5, maxAcceleration=2.0, - disableThrottle=disable_throttle, - trackingLead=tracking_lead, - accelerationJerk=5.0, - dangerJerk=5.0, + accelerationJerk=200.0, + dangerJerk=100.0, speedJerk=5.0, - dangerFactor=1.0, + dangerFactor=0.75, tFollow=1.45, + disableThrottle=False, forcingStop=False, redLight=False, - forcingStopLength=2, + stopSignConfirmed=False, + forcingStopLength=2.0, + increasedStoppedDistance=0.0, + roadCurvature=0.0, ), } -def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False): +def make_toggles(*, model_first=False, delay=0.2): return SimpleNamespace( - taco_tune=False, - classic_model=False, - tinygrad_model=True, - model_version=model_version, - vEgoStopping=0.5, - radar_takeoffs=radar_takeoffs, + longitudinal_model_preference=model_first, + longitudinalActuatorDelay=delay, + vEgoStopping=0.05, + stopAccel=-0.5, ) -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version): - v_ego = 18.0 - desired_accel = -1.0 - comfort_min_accel = -0.5 +def use_fixed_mpc(planner, accel): + def update(*_args, **_kwargs): + v_ego = planner.mpc.x0[1] + planner.mpc.a_solution = np.full(len(T_IDXS), accel) + planner.mpc.v_solution = np.maximum(v_ego + accel * T_IDXS, 0.0) + planner.mpc.j_solution = np.zeros(len(T_IDXS) - 1) + planner.mpc.source = "cruise" + planner.mpc.crash_cnt = 0 + planner.mpc.update = update + + +def test_set_speed_first_and_model_first_are_continuous_preferences(): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm(v_ego, desired_accel, comfort_min_accel) + sm = make_sm(v_ego=20.0, v_cruise=20.0, model_accel=-0.4) - vehicle_min_accel = get_vehicle_min_accel(CP, v_ego) - assert vehicle_min_accel < comfort_min_accel + set_speed = LongitudinalPlanner(CP, init_v=20.0) + use_fixed_mpc(set_speed, 0.5) + set_speed.update(sm, make_toggles(model_first=False)) - planner.update(sm, make_toggles(model_version)) + model_first = LongitudinalPlanner(CP, init_v=20.0) + use_fixed_mpc(model_first, 0.5) + model_first.update(sm, make_toggles(model_first=True)) - assert planner.mode == "blended" - assert planner.mlsim - assert planner.output_a_target == pytest.approx(desired_accel, abs=1e-3) - assert planner.output_a_target < comfort_min_accel + assert set_speed.model_authority == 0.0 + assert set_speed.output_a_target > 0.0 + assert model_first.model_authority >= 0.75 + assert model_first.output_a_target < 0.0 -def test_gm_pedal_vehicle_min_accel_uses_brand_when_car_name_is_missing(): +def test_model_stop_intent_brakes_without_binary_mode_switch(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=10.0) + use_fixed_mpc(planner, 0.4) + sm = make_sm(v_ego=10.0, model_accel=-1.0) + set_model_stop(sm["modelV2"]) + + planner.update(sm, make_toggles()) + + assert planner.model_authority == 1.0 + assert planner.state == PlanState.stopping + assert planner.output_a_target < 0.0 + assert planner.plan_source == "e2e" + + +def test_force_stop_is_a_hard_stop(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + use_fixed_mpc(planner, 0.5) + sm = make_sm(v_ego=0.0, model_accel=0.5) + set_model_departure(sm["modelV2"]) + sm["starpilotPlan"].forcingStop = True + + planner.update(sm, make_toggles()) + + assert planner.state == PlanState.stopped + assert planner.output_should_stop + assert planner.output_a_target <= 0.0 + + +def test_second_lead_slot_can_immediately_cap_acceleration(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=15.0) + use_fixed_mpc(planner, 0.8) + sm = make_sm( + v_ego=15.0, + model_accel=0.8, + lead_one=make_lead(status=True, d_rel=150.0, v_lead=15.0, radar=True), + lead_two=make_lead(status=True, d_rel=20.0, v_lead=0.0, radar=False), + ) + + planner.update(sm, make_toggles()) + + assert planner.output_a_target < 0.0 + + +def test_stopped_lead_holds_until_motion_and_gap_are_both_safe(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + use_fixed_mpc(planner, 0.5) + lead = make_lead(status=True, d_rel=5.9, v_lead=1.0) + sm = make_sm(v_ego=0.0, model_accel=0.5, lead_one=lead) + set_model_departure(sm["modelV2"]) + planner.state = PlanState.stopped + + planner.update(sm, make_toggles()) + assert planner.state == PlanState.stopped + assert planner.output_should_stop + assert planner.output_a_target <= 0.0 + + lead.dRel = 6.0 + planner.update(sm, make_toggles()) + assert planner.state == PlanState.departing + assert planner.output_a_target >= planner.tune.launch_accel + + +def test_departing_lead_that_brakes_aborts_launch_immediately(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + use_fixed_mpc(planner, 0.5) + lead = make_lead(status=True, d_rel=7.0, v_lead=1.0) + sm = make_sm(v_ego=0.0, model_accel=0.5, lead_one=lead) + set_model_departure(sm["modelV2"]) + planner.state = PlanState.stopped + planner.update(sm, make_toggles()) + assert planner.state == PlanState.departing + + sm["carState"].vEgo = 0.4 + sm["carState"].vEgoCluster = 0.4 + sm["carState"].standstill = False + lead.vLead = 0.1 + lead.aLeadK = -1.0 + planner.update(sm, make_toggles()) + + assert planner.state == PlanState.stopping + assert planner.output_a_target <= -0.5 + + +def test_output_smoothing_is_asymmetric_and_urgent_braking_bypasses_it(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP) + planner.output_a_target = 0.0 + + assert planner._smooth_output(1.0, urgent=False) == pytest.approx( + planner.tune.accel_slew_rate * planner.dt + ) + assert planner._smooth_output(-2.0, urgent=True) == -2.0 + + +def test_low_actuator_delay_uses_stable_evaluation_horizon(monkeypatch): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=20.0) + use_fixed_mpc(planner, 0.0) + captured = {} + + def capture_plan(_speeds, _accels, action_t, _v_ego_stopping): + captured["action_t"] = action_t + return 0.0, False + + monkeypatch.setattr(planner_module, "get_accel_from_plan", capture_plan) + planner.update(make_sm(v_ego=20.0), make_toggles(delay=0.05)) + + assert captured["action_t"] == pytest.approx(MIN_PLAN_HORIZON) + + +def test_mpc_tracks_both_lead_slots_independently_without_fusion(): + mpc = LongitudinalMpc() + mpc.set_cur_state(20.0, 0.0) + mpc.run = lambda: None + lead_one = make_lead(status=True, d_rel=100.0, v_lead=20.0) + lead_two = make_lead(status=True, d_rel=25.0, v_lead=5.0) + radar_state = SimpleNamespace(leadOne=lead_one, leadTwo=lead_two) + + mpc.update(radar_state, 30.0) + + assert mpc.source == "lead1" + assert mpc._lead_filter_state[0][0] == pytest.approx(20.0) + assert mpc._lead_filter_state[1][0] == pytest.approx(5.0) + + lead_one.vLead = 10.0 + mpc.process_lead(lead_one, 0) + assert mpc._lead_filter_state[1][0] == pytest.approx(5.0) + + +def test_fcw_considers_each_credible_closing_lead(): + safe = make_lead(status=True, d_rel=100.0, v_lead=20.0, model_prob=1.0) + dangerous = make_lead(status=True, d_rel=20.0, v_lead=0.0, model_prob=1.0) + + assert not should_trigger_planner_fcw(safe, 20.0) + assert should_trigger_planner_fcw(dangerous, 20.0) + + +def test_vehicle_tuning_is_declarative_and_honda_exception_is_conservative(): + civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + hrv = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G) + + assert get_longitudinal_planner_tune(hrv).lead_filter_tau > get_longitudinal_planner_tune(civic).lead_filter_tau + assert get_longitudinal_planner_tune(hrv).accel_slew_rate < get_longitudinal_planner_tune(civic).accel_slew_rate + + +def test_gm_pedal_min_accel_keeps_physical_vehicle_floor(): CP = SimpleNamespace( carName=None, brand="gm", @@ -330,4322 +322,3 @@ def test_gm_pedal_vehicle_min_accel_uses_brand_when_car_name_is_missing(): ) assert get_vehicle_min_accel(CP, 32.4) == pytest.approx(-2.95) - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_uses_close_raw_lead_when_tracking_lead_is_debounced(model_version): - v_ego = 5.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=-0.6, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=24.0, v_lead=0.3), - ) - sm["starpilotPlan"].vCruise = v_ego + 12.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.mode == "acc" - assert planner.raw_close_lead_needs_control(sm["radarState"].leadOne, v_ego) - assert planner.output_a_target == pytest.approx( - planner.get_close_lead_brake_cap(sm["radarState"].leadOne, v_ego, sm["starpilotPlan"].minAcceleration) - ) - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_tracking(model_version): - v_ego = 29.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) - planner_far_vision = LongitudinalPlanner(CP, init_v=v_ego) - sm_no_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - ) - sm_far_vision = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=82.0, v_lead=25.0, radar=False, model_prob=0.9), - ) - sm_no_lead["starpilotPlan"].vCruise = v_ego + 2.0 - sm_far_vision["starpilotPlan"].vCruise = v_ego + 2.0 - - no_lead_outputs = [] - far_vision_outputs = [] - for _ in range(8): - planner_no_lead.update(sm_no_lead, make_toggles(model_version)) - planner_far_vision.update(sm_far_vision, make_toggles(model_version)) - no_lead_outputs.append(planner_no_lead.output_a_target) - far_vision_outputs.append(planner_far_vision.output_a_target) - - assert planner_far_vision.mode == "acc" - assert not planner_far_vision.raw_close_lead_needs_control(sm_far_vision["radarState"].leadOne, v_ego) - np.testing.assert_allclose(far_vision_outputs, no_lead_outputs, atol=1e-6) - - -def test_cruise_accel_cap_does_not_manufacture_braking_after_set_speed_drop_with_lead(): - v_ego = 20.115 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead( - status=True, - d_rel=68.95, - v_lead=18.74, - a_lead=-0.32, - radar=True, - model_prob=0.991, - y_rel=-0.15, - ) - lead_two = make_lead( - status=True, - d_rel=68.95, - v_lead=18.74, - a_lead=-0.32, - radar=True, - model_prob=0.993, - y_rel=-0.15, - ) - sm = make_sm( - v_ego, - desired_accel=-0.05, - min_accel=-1.0, - experimental_mode=True, - tracking_lead=True, - lead_one=lead_one, - lead_two=lead_two, - ) - sm["starpilotPlan"].vCruise = 15.646 - sm["starpilotPlan"].tFollow = 1.0 - - planner.update(sm, make_toggles()) - - assert planner.output_a_target > -1.0 - assert planner.output_a_target <= 0.0 - - -def test_cruise_accel_cap_preserves_close_lead_braking_after_set_speed_drop(): - v_ego = 20.115 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=-0.05, - min_accel=-1.0, - experimental_mode=True, - tracking_lead=True, - lead_one=make_lead( - status=True, - d_rel=20.0, - v_lead=0.0, - a_lead=-1.0, - radar=True, - model_prob=0.99, - ), - ) - sm["starpilotPlan"].vCruise = 15.646 - sm["starpilotPlan"].tFollow = 1.0 - - planner.update(sm, make_toggles()) - - assert planner.output_a_target <= -3.0 - - -def test_soften_far_radar_lead_accel_reduces_gentle_far_brake(): - softened = soften_far_radar_lead_accel(114.8, 28.88, -0.75, 29.26, 1.45, radar=True) - assert softened > -0.35 - assert softened < 0.0 - - -def test_soften_far_radar_lead_accel_keeps_close_closing_brake(): - baseline = -0.76 - softened = soften_far_radar_lead_accel(68.0, 26.38, baseline, 29.38, 1.45, radar=True) - assert softened == pytest.approx(baseline) - - -def test_planner_fcw_suppresses_low_speed_opening_or_low_ttc_false_positives(): - assert not should_trigger_planner_fcw( - make_lead(status=True, d_rel=7.156, v_lead=0.798, a_lead=0.021, radar=False, model_prob=0.99), - 0.402, - ) - assert not should_trigger_planner_fcw( - make_lead(status=True, d_rel=9.311, v_lead=0.911, a_lead=-0.263, radar=False, model_prob=0.99), - 1.252, - ) - - -def test_planner_fcw_keeps_real_low_speed_closing_alerts(): - assert should_trigger_planner_fcw( - make_lead(status=True, d_rel=1.8, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.99), - 1.6, - ) - - -def test_publish_planner_fcw_suppresses_crawl_speed_false_positive(): - car_state = SimpleNamespace(vEgo=0.29, standstill=False) - radar_state = SimpleNamespace( - leadOne=make_lead(status=True, d_rel=7.55, v_lead=0.033, a_lead=0.0, radar=False, model_prob=0.99), - ) - assert not should_publish_planner_fcw(3, car_state, radar_state) - - -def test_publish_planner_fcw_keeps_real_current_close_closing_alert(): - car_state = SimpleNamespace(vEgo=1.6, standstill=False) - radar_state = SimpleNamespace( - leadOne=make_lead(status=True, d_rel=1.8, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.99), - ) - assert should_publish_planner_fcw(3, car_state, radar_state) - - -def test_vision_lead_approach_cap_brakes_before_hard_cap(): - v_ego = 21.535 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984) - - hard_cap = planner.get_close_lead_brake_cap(lead, v_ego, -1.0) - approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) - - assert hard_cap == pytest.approx(-0.212, abs=1e-2) - assert approach_cap is not None - assert approach_cap < hard_cap - assert approach_cap > -1.2 - - -def test_vision_lead_approach_cap_brakes_harder_when_inside_tight_gap(): - v_ego = 26.18 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=39.72, v_lead=22.46, a_lead=-0.15, radar=False, model_prob=0.97) - - approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.49) - - assert approach_cap is not None - assert approach_cap < -0.5 - - -def test_vision_lead_approach_cap_brakes_harder_for_braking_tracked_lead_inside_tight_gap(): - v_ego = 19.50 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98) - - hard_cap = planner.get_close_lead_brake_cap(lead, v_ego, -3.0) - approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -3.0, 1.45) - - assert hard_cap == pytest.approx(-1.01, abs=0.03) - assert approach_cap is not None - assert approach_cap < -1.35 - assert approach_cap < hard_cap - - -def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap(): - v_ego = 19.37 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=66.168, v_lead=20.751, a_lead=0.261, radar=False, model_prob=0.975) - - assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None - - -def test_vision_untracked_slow_lead_cap_triggers_only_for_meaningful_closing_case(): - route_v_ego = 23.23 - far_v_ego = 29.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=route_v_ego) - route_like_lead = make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92) - far_mild_lead = make_lead(status=True, d_rel=82.0, v_lead=25.0, a_lead=0.0, radar=False, model_prob=0.9) - - route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, route_v_ego, -1.0) - far_cap = planner.get_vision_untracked_slow_lead_cap(far_mild_lead, far_v_ego, -1.0) - - assert route_cap is not None - assert route_cap < -0.1 - assert far_cap is None - - -def test_vision_untracked_slow_lead_cap_starts_earlier_for_high_confidence_rav4_approach(): - v_ego = 21.4 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - route_like_lead = make_lead(status=True, d_rel=68.3, v_lead=18.3, radar=False, model_prob=0.95) - - route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) - - assert route_cap is not None - assert -0.6 < route_cap < -0.2 - - -def test_vision_untracked_slow_lead_cap_catches_near_rav4_lead_before_tracking(): - v_ego = 21.3 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - route_like_lead = make_lead(status=True, d_rel=41.1, v_lead=19.2, radar=False, model_prob=0.90) - - route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) - - assert route_cap is not None - assert -0.35 < route_cap <= -0.1 - - -def test_vision_untracked_slow_lead_relaxed_entry_requires_centered_lead(): - v_ego = 21.4 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - off_path_lead = make_lead(status=True, d_rel=68.3, v_lead=18.3, radar=False, model_prob=0.99, y_rel=1.5) - - assert planner.get_vision_untracked_slow_lead_cap(off_path_lead, v_ego, -1.0) is None - - -def test_vision_untracked_slow_lead_cap_reaches_high_confidence_far_slower_lead_before_raw_close_lead(): - v_ego = 21.48 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - route_like_lead = make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935) - - route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) - - assert route_cap is not None - assert route_cap < -0.5 - assert not planner.raw_close_lead_needs_control(route_like_lead, v_ego) - - -def test_vision_untracked_slow_lead_cap_relaxes_confidence_for_near_stopped_high_closure_lead(): - v_ego = 20.35 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - route_like_lead = make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70) - - route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) - - assert route_cap is not None - assert route_cap < -0.55 - - -def test_hrv_untracked_slow_lead_cap_prepares_more_for_high_speed_stop(): - v_ego = 21.8 - lead = make_lead(status=True, d_rel=86.5, v_lead=8.3, a_lead=0.0, radar=False, model_prob=0.93, y_rel=-0.14) - - civic_planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=v_ego) - hrv_planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G), init_v=v_ego) - civic_cap = civic_planner.get_vision_untracked_slow_lead_cap(lead, v_ego, -3.5) - hrv_cap = hrv_planner.get_vision_untracked_slow_lead_cap(lead, v_ego, -3.5) - - assert civic_cap is not None - assert hrv_cap is not None - assert hrv_cap < civic_cap - 0.15 - assert -1.2 < hrv_cap < -0.8 - - -def test_vision_untracked_slow_lead_cap_keeps_low_confidence_floor_for_less_threatening_lead(): - v_ego = 20.35 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - less_threatening_lead = make_lead(status=True, d_rel=115.4, v_lead=9.5, a_lead=0.0, radar=False, model_prob=0.75) - - assert planner.get_vision_untracked_slow_lead_cap(less_threatening_lead, v_ego, -1.0) is None - - -def test_vision_untracked_approach_lift_eases_throttle_without_braking(): - v_ego = 30.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.98) - - aggressive_cap = planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.2) - standard_cap = planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.4) - - assert aggressive_cap is not None - assert standard_cap is not None - assert 0.0 <= standard_cap <= aggressive_cap < 0.22 - - -@pytest.mark.parametrize("lead", [ - make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=True, model_prob=0.98), - make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.90), - make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.98, y_rel=1.5), - make_lead(status=True, d_rel=110.0, v_lead=30.0, radar=False, model_prob=0.98), -]) -def test_vision_untracked_approach_lift_ignores_unqualified_leads(lead): - v_ego = 30.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - - assert planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.4) is None - - -def test_vision_untracked_approach_lift_is_rate_limited_and_held(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=25.0, init_a=0.5) - now = 0.0 - - cap = None - for _ in range(6): - now += planner.dt - cap = planner.update_vision_untracked_approach_lift_cap(0.0, 0.5, 0.5, now, True) - - assert cap is not None - assert 0.45 < cap < 0.5 - - held_cap = planner.update_vision_untracked_approach_lift_cap(None, 0.5, 0.5, now + planner.dt, True) - assert held_cap is not None - assert held_cap < cap - - -def test_vision_untracked_approach_lift_releases_after_hold_or_tracking(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=25.0, init_a=0.5) - now = 0.0 - - for _ in range(6): - now += planner.dt - planner.update_vision_untracked_approach_lift_cap(0.0, 0.5, 0.5, now, True) - - previous_cap = planner.untracked_vision_approach_lift_cap - now += planner.dt - releasing_cap = planner.update_vision_untracked_approach_lift_cap(None, 0.5, 0.5, now, False) - - assert releasing_cap is not None - assert releasing_cap > previous_cap - - -def test_far_opening_radar_brake_guard_removes_only_harmless_pulse(): - v_ego = 28.9 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - opening_lead = make_lead(status=True, d_rel=96.5, v_lead=32.0, a_lead=-0.15, radar=True, model_prob=0.93) - - guard_target = planner.get_far_opening_radar_brake_guard_target( - opening_lead, v_ego, 1.25, 0.0, -0.41, 29.0, 0.0, "cruise", True, - ) - - assert guard_target == 0.0 - - -def test_far_opening_radar_brake_guard_preserves_close_or_requested_braking(): - v_ego = 28.9 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - close_slower_lead = make_lead(status=True, d_rel=36.9, v_lead=20.9, a_lead=-0.18, radar=True, model_prob=0.99) - far_opening_lead = make_lead(status=True, d_rel=96.5, v_lead=32.0, a_lead=-0.15, radar=True, model_prob=0.93) - - assert planner.get_far_opening_radar_brake_guard_target( - close_slower_lead, v_ego, 1.25, 0.0, -3.5, 29.0, 0.0, "lead0", True, - ) is None - assert planner.get_far_opening_radar_brake_guard_target( - far_opening_lead, v_ego, 1.25, 0.0, -0.41, 27.0, 0.0, "cruise", True, - ) is None - - -def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop(): - v_ego = 13.207 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=49.131, v_lead=1.837, a_lead=-0.312, radar=False, model_prob=0.942) - - slow_stop_cap = planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.75) - - assert slow_stop_cap is not None - assert slow_stop_cap < -0.9 - assert slow_stop_cap > -1.25 - - -def test_vision_slow_stopped_lead_cap_ignores_far_high_speed_stop_candidate(): - v_ego = 33.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=183.0, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.995) - - assert planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.45) is None - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version): - v_ego = 21.535 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-3.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984), - ) - sm["starpilotPlan"].vCruise = v_ego + 8.0 - - for _ in range(8): - planner.update(sm, make_toggles(model_version)) - - assert planner.effective_t_follow is not None - assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.15 - assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.45 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_dynamic_t_follow_stays_near_base_for_far_highway_lead(model_version): - v_ego = 29.26 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9), - ) - sm["starpilotPlan"].vCruise = v_ego + 3.0 - - for _ in range(12): - planner.update(sm, make_toggles(model_version)) - - assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_dynamic_t_follow_releases_toward_base_after_lead_opens(model_version): - v_ego = 21.535 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-3.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984), - ) - - for _ in range(8): - planner.update(sm, make_toggles(model_version)) - - boosted_t_follow = planner.effective_t_follow - sm["radarState"].leadOne = make_lead(status=True, d_rel=66.168, v_lead=20.751, a_lead=0.261, radar=False, model_prob=0.975) - for _ in range(12): - planner.update(sm, make_toggles(model_version)) - - assert boosted_t_follow is not None - assert planner.effective_t_follow < boosted_t_follow - assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_version): - approach_v_ego = 21.535 - close_v_ego = 21.435 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_approach = LongitudinalPlanner(CP, init_v=approach_v_ego) - planner_close = LongitudinalPlanner(CP, init_v=close_v_ego) - - sm_approach = make_sm( - approach_v_ego, - desired_accel=0.2, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984), - ) - sm_close = make_sm( - close_v_ego, - desired_accel=0.2, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=27.18, v_lead=15.76, a_lead=-0.824, radar=False, model_prob=0.988), - ) - sm_approach["starpilotPlan"].vCruise = approach_v_ego + 8.0 - sm_close["starpilotPlan"].vCruise = close_v_ego + 8.0 - - approach_outputs = [] - for _ in range(6): - planner_approach.update(sm_approach, make_toggles(model_version)) - approach_outputs.append(planner_approach.output_a_target) - - planner_close.update(sm_close, make_toggles(model_version)) - - assert planner_approach.mode == "acc" - assert planner_close.mode == "acc" - assert min(approach_outputs[:2]) > -0.55 - assert approach_outputs[-1] < -1.3 - assert planner_close.output_a_target < approach_outputs[0] - 0.8 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_tracked_vision_far_mild_closure_does_not_bypass_persistence(model_version): - v_ego = 37.45 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=42.8, v_lead=35.31, a_lead=0.18, radar=False, model_prob=0.98) - - approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) - - assert approach_cap is not None - assert approach_cap > -1.0 - assert not planner.tracked_vision_lead_approach_needs_immediate_brake(lead, v_ego, approach_cap) - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_tracked_vision_close_or_braking_lead_bypasses_persistence(model_version): - v_ego = 19.50 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98), - ) - sm["starpilotPlan"].vCruise = v_ego + 6.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_a_target < -1.3 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_version): - v_ego = 23.23 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) - planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) - sm_no_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - ) - sm_with_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92), - ) - sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 - sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 - - for _ in range(6): - planner_no_lead.update(sm_no_lead, make_toggles(model_version)) - planner_with_lead.update(sm_with_lead, make_toggles(model_version)) - - assert planner_with_lead.mode == "acc" - assert not planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) - assert planner_with_lead.output_a_target <= planner_no_lead.output_a_target - 0.04 - assert planner_with_lead.output_a_target < -0.2 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_keeps_close_slow_radar_lead_active_when_tracking_flaps(model_version): - v_ego = 0.96 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) - planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) - sm_no_lead = make_sm( - v_ego, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - ) - sm_with_lead = make_sm( - v_ego, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=8.15, v_lead=0.09, a_lead=-0.36, radar=True, model_prob=1.0, y_rel=0.1), - ) - sm_no_lead["starpilotPlan"].vCruise = v_ego + 8.0 - sm_with_lead["starpilotPlan"].vCruise = v_ego + 8.0 - - planner_no_lead.update(sm_no_lead, make_toggles(model_version)) - planner_with_lead.update(sm_with_lead, make_toggles(model_version)) - - assert planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) - assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.15 - assert planner_with_lead.output_a_target <= 0.22 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_low_speed_weak_departure_accel_cap_softens_voacc_follow_pulse(model_version): - v_ego = 2.8 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=16.6, v_lead=2.85, a_lead=0.05, radar=False, model_prob=0.99, y_rel=0.0), - ) - sm["starpilotPlan"].vCruise = v_ego + 10.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_a_target <= 0.22 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_tracking(model_version): - v_ego = 21.48 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) - planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) - sm_no_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - ) - sm_with_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935), - ) - sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 - sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 - - no_lead_outputs = [] - lead_outputs = [] - for _ in range(8): - planner_no_lead.update(sm_no_lead, make_toggles(model_version)) - planner_with_lead.update(sm_with_lead, make_toggles(model_version)) - no_lead_outputs.append(planner_no_lead.output_a_target) - lead_outputs.append(planner_with_lead.output_a_target) - - assert planner_with_lead.mode == "acc" - assert not planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) - assert all(lead_output <= no_lead_output + 1e-6 - for lead_output, no_lead_output in zip(lead_outputs[5:], no_lead_outputs[5:])) - assert min(lead_outputs[5:]) < min(no_lead_outputs[5:]) - 0.08 - assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_pretracking_vision_far_slower_lead_can_still_brake_immediately(model_version): - v_ego = 21.48 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935), - ) - sm["starpilotPlan"].vCruise = v_ego + 6.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.mode == "acc" - assert planner.output_a_target < -0.45 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_pretracking_closer_braking_vision_lead_bypasses_far_lead_persistence(model_version): - v_ego = 17.46 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=41.9, v_lead=14.86, a_lead=-0.03, radar=False, model_prob=1.0), - ) - sm["starpilotPlan"].vCruise = v_ego + 6.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.mode == "acc" - assert planner.output_a_target < -0.35 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_pretracking_flappy_far_lead_requires_persistence(model_version): - v_ego = 26.09 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) - planner_flappy = LongitudinalPlanner(CP, init_v=v_ego) - sm_no_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - ) - sm_flappy = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=74.75, v_lead=26.63, a_lead=0.01, radar=False, model_prob=0.989), - ) - sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 - sm_flappy["starpilotPlan"].vCruise = v_ego + 6.0 - - flappy_sequence = [ - (74.75, 26.63, 0.01, 0.989), - (68.17, 20.81, 0.094, 0.971), - (69.73, 24.12, 0.057, 0.981), - (62.15, 21.38, 0.064, 0.983), - (66.29, 23.19, 0.069, 0.985), - (70.58, 27.51, 0.036, 0.988), - ] - - no_lead_outputs = [] - flappy_outputs = [] - for d_rel, v_lead, a_lead, model_prob in flappy_sequence: - planner_no_lead.update(sm_no_lead, make_toggles(model_version)) - sm_flappy["radarState"].leadOne = make_lead( - status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, radar=False, model_prob=model_prob, - ) - planner_flappy.update(sm_flappy, make_toggles(model_version)) - no_lead_outputs.append(planner_no_lead.output_a_target) - flappy_outputs.append(planner_flappy.output_a_target) - - assert planner_flappy.mode == "acc" - assert min(flappy_outputs) > min(no_lead_outputs) - 0.12 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confidence_is_midrange(model_version): - v_ego = 20.35 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) - planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) - sm_no_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - ) - sm_with_lead = make_sm( - v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70), - ) - sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 - sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 - - for _ in range(8): - planner_no_lead.update(sm_no_lead, make_toggles(model_version)) - planner_with_lead.update(sm_with_lead, make_toggles(model_version)) - - assert planner_with_lead.mode == "acc" - assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.12 - assert planner_with_lead.output_a_target < -0.45 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version): - v_ego = 28.7 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) - planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) - sm_no_lead = make_sm( - v_ego, - desired_accel=0.5, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=False, - ) - sm_with_lead = make_sm( - v_ego, - desired_accel=0.5, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=22.0, v_lead=29.4, a_lead=0.0, radar=False, model_prob=0.995), - ) - sm_no_lead["starpilotPlan"].vCruise = v_ego + 4.0 - sm_with_lead["starpilotPlan"].vCruise = v_ego + 4.0 - - for _ in range(8): - planner_no_lead.update(sm_no_lead, make_toggles(model_version)) - planner_with_lead.update(sm_with_lead, make_toggles(model_version)) - - assert planner_with_lead.mode == "acc" - assert planner_with_lead.output_a_target <= planner_no_lead.output_a_target - 0.15 - assert planner_with_lead.output_a_target < 0.08 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version): - v_ego = 3.8 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.1, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=5.75, v_lead=0.58, a_lead=-0.1, radar=False, model_prob=0.99), - ) - sm["starpilotPlan"].vCruise = v_ego + 4.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.mode == "acc" - assert planner.output_should_stop - assert planner.output_a_target < -1.0 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_vision_lead(model_version): - v_ego = 6.2 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.1, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=9.35, v_lead=2.8, a_lead=-0.2, radar=False, model_prob=0.99), - ) - sm["starpilotPlan"].vCruise = v_ego + 4.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.mode == "acc" - assert planner.output_should_stop - assert planner.output_a_target <= -2.7 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_softens_near_stop(model_version, monkeypatch): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.55) - lead = make_lead(status=True, d_rel=2.1, v_lead=0.05, a_lead=-0.02, radar=False, model_prob=0.99) - - monotonic_values = iter([10.0, 10.1]) - monkeypatch.setattr(longitudinal_planner_module.time, "monotonic", lambda: next(monotonic_values)) - - cap_armed, active_armed = planner.get_vision_low_speed_stop_buffer_cap(lead, 0.55, -2.0) - cap_held, active_held = planner.get_vision_low_speed_stop_buffer_cap(lead, 0.34, -2.0) - - assert active_armed - assert cap_armed is not None - assert active_held - assert cap_held is not None - assert cap_held <= -1.25 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_close_moving_vision_lead_keeps_negative_output_while_should_stop(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.242) - toggles = make_toggles(model_version) - - stop_sequence = [ - (0.242, 2.062, 0.284, -0.081), - (0.221, 1.963, 0.338, -0.076), - (0.194, 2.100, 0.451, -0.076), - (0.180, 2.001, 0.447, -0.066), - (0.166, 1.964, 0.451, -0.066), - (0.151, 2.075, 0.451, -0.060), - ] - - outputs = [] - for v_ego, d_rel, v_lead, desired_accel in stop_sequence: - sm = make_sm( - v_ego, - desired_accel=desired_accel, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=0.0, radar=False, model_prob=1.0), - ) - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - - planner.update(sm, toggles) - outputs.append(planner.output_a_target) - - assert all(output <= -0.02 for output in outputs[2:]) - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_close_near_standstill_vision_lead_keeps_meaningful_brake_floor(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.017) - - sm = make_sm( - 0.017, - desired_accel=-0.06, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=2.83, v_lead=0.09, a_lead=0.10, radar=False, model_prob=0.9999), - ) - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = True - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert planner.output_a_target <= -0.20 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_close_near_standstill_moving_lead_keeps_brake_floor_while_should_stop(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.034) - - sm = make_sm( - 0.034, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=3.93, v_lead=1.61, a_lead=2.18, radar=False, model_prob=1.0), - ) - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = True - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert planner.output_a_target <= -0.20 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_close_opening_vision_lead_does_not_drop_to_zero_after_stop_release(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=1.49) - toggles = make_toggles(model_version) - - sm_stop = make_sm( - 1.488, - desired_accel=-1.64, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=3.433, v_lead=1.469, a_lead=0.58, radar=False, model_prob=0.9996), - ) - sm_stop["controlsState"].longControlState = LongCtrlState.stopping - sm_stop["starpilotPlan"].vCruise = 10.0 - sm_stop["modelV2"].action.shouldStop = True - planner.update(sm_stop, toggles) - - sm_release = make_sm( - 1.420, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=3.568, v_lead=1.679, a_lead=0.66, radar=False, model_prob=0.9994), - ) - sm_release["controlsState"].longControlState = LongCtrlState.pid - sm_release["starpilotPlan"].vCruise = 10.0 - sm_release["modelV2"].action.shouldStop = False - planner.update(sm_release, toggles) - - assert planner.output_a_target <= -0.18 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_close_near_standstill_departing_lead_keeps_small_brake_after_stop_release(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.034) - toggles = make_toggles(model_version) - - sm_stop = make_sm( - 0.034, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=3.93, v_lead=1.61, a_lead=2.18, radar=False, model_prob=1.0), - ) - sm_stop["controlsState"].longControlState = LongCtrlState.stopping - sm_stop["starpilotPlan"].vCruise = 10.0 - sm_stop["modelV2"].action.shouldStop = True - planner.update(sm_stop, toggles) - - sm_release = make_sm( - 0.449, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=4.02, v_lead=2.45, a_lead=2.26, radar=False, model_prob=1.0), - ) - sm_release["controlsState"].longControlState = LongCtrlState.pid - sm_release["starpilotPlan"].vCruise = 10.0 - sm_release["modelV2"].action.shouldStop = False - planner.update(sm_release, toggles) - - assert planner.output_a_target <= -0.18 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_tracked_vision_model_brake_floor_prevents_positive_output_on_slower_lead(model_version): - v_ego = 19.1 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=-1.18, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=36.5, v_lead=17.2, a_lead=-0.53, radar=False, model_prob=0.993), - ) - - for _ in range(8): - planner.update(sm, make_toggles(model_version)) - - assert planner.mode == "acc" - assert planner.output_a_target <= -0.35 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_tracked_vision_model_brake_cap_relaxes_mild_model_brake_slam_window(model_version): - v_ego = 20.56 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=48.0, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999) - - cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -0.35) - - assert cap is not None - assert cap > -1.2 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_tracked_vision_model_brake_cap_does_not_relax_strong_model_brake(model_version): - v_ego = 20.56 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=38.1, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999) - - cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -1.2) - - assert cap is None - - -def test_model_launch_accel_skips_hesitant_start_of_trajectory(): - model_v = np.maximum(T_IDXS_MPC - 0.6, 0.0) - model_a = np.where(T_IDXS_MPC >= 0.6, 1.0, 0.0) - - launch_accel = LongitudinalPlanner.get_model_launch_accel(model_v, model_a, action_t=0.2, v_ego=0.0) - - assert launch_accel is not None - assert launch_accel >= 0.8 - - -def test_green_light_model_launch_boosts_no_lead_experimental_takeoff(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=True) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - set_model_launch_trajectory(sm["modelV2"]) - - planner.update(sm, make_toggles()) - - assert not planner.output_should_stop - assert planner.output_a_target >= 0.8 - - -def test_green_light_model_launch_survives_cem_switch_back_to_chill(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm_red = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=True) - sm_red["carState"].standstill = True - sm_red["controlsState"].longControlState = LongCtrlState.stopping - sm_red["modelV2"].action.shouldStop = True - sm_red["starpilotPlan"].redLight = True - planner.update(sm_red, make_toggles()) - - sm_green = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False) - sm_green["carState"].standstill = True - sm_green["controlsState"].longControlState = LongCtrlState.stopping - set_model_launch_trajectory(sm_green["modelV2"]) - planner.update(sm_green, make_toggles()) - - assert planner.model_launch_stop_seen - assert not planner.output_should_stop - assert planner.output_a_target >= 0.8 - - -@pytest.mark.parametrize(("veto", "brake_pressed"), [("redLight", False), ("forcingStop", False), (None, True)]) -def test_green_light_model_launch_respects_stop_and_driver_vetoes(veto, brake_pressed): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=True) - sm["carState"].standstill = True - sm["carState"].brakePressed = brake_pressed - sm["controlsState"].longControlState = LongCtrlState.stopping - if veto is not None: - setattr(sm["starpilotPlan"], veto, True) - set_model_launch_trajectory(sm["modelV2"]) - - planner.update(sm, make_toggles()) - - assert planner.output_a_target < 0.3 - - -def test_model_launch_does_not_override_stationary_lead_guard(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=True, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=4.0, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - set_model_launch_trajectory(sm["modelV2"]) - - planner.update(sm, make_toggles()) - - assert planner.output_should_stop - assert planner.output_a_target <= 0.0 - - -def test_model_launch_boosts_only_after_lead_departure_is_confirmed(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm( - 0.0, - desired_accel=0.1, - min_accel=-0.5, - experimental_mode=True, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=7.0, v_lead=1.5, a_lead=0.8, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - set_model_launch_trajectory(sm["modelV2"]) - - planner.update(sm, make_toggles()) - - assert not planner.output_should_stop - assert planner.output_a_target >= 0.8 - - -def test_model_launch_is_cancelled_when_departing_lead_stops_again(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm_depart = make_sm( - 0.0, - desired_accel=0.1, - min_accel=-0.5, - experimental_mode=True, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=7.0, v_lead=1.5, a_lead=0.8, radar=True, model_prob=1.0), - ) - sm_depart["carState"].standstill = True - sm_depart["controlsState"].longControlState = LongCtrlState.stopping - set_model_launch_trajectory(sm_depart["modelV2"]) - planner.update(sm_depart, make_toggles()) - - sm_stop = make_sm( - 0.2, - desired_accel=0.1, - min_accel=-0.5, - experimental_mode=True, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=3.8, v_lead=0.0, a_lead=-0.6, radar=True, model_prob=1.0), - ) - sm_stop["controlsState"].longControlState = LongCtrlState.pid - set_model_launch_trajectory(sm_stop["modelV2"]) - - planner.update(sm_stop, make_toggles()) - - assert planner.output_should_stop - assert planner.output_a_target <= 0.0 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_manual_resume_override_clears_no_lead_model_stop_at_standstill(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = True - sm["starpilotPlan"].forcingStop = True - sm["starpilotCarState"] = SimpleNamespace(accelPressed=True) - - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target >= 0.2 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_manual_resume_override_does_not_clear_stopped_lead_stop(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - lead_one=make_lead(status=True, d_rel=4.0, v_lead=0.0, radar=False, model_prob=0.99), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = True - sm["starpilotPlan"].forcingStop = True - sm["starpilotCarState"] = SimpleNamespace(accelPressed=True) - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version): - v_ego = 0.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=7.1, v_lead=2.3, a_lead=1.8, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert planner.output_a_target < 0.1 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_accelerating_lead_keeps_small_nudge_while_stop_holds(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.40, radar=False, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_near_standstill_accelerating_lead_keeps_nudge_during_creep_frame(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.03) - - sm = make_sm( - 0.03, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=5.60, v_lead=0.06, a_lead=0.40, radar=False, model_prob=1.0), - ) - sm["carState"].standstill = False - sm["controlsState"].longControlState = LongCtrlState.pid - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_tiny_opening_lead_without_accel_does_not_get_nudge_floor(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.0, radar=False, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert planner.output_a_target <= 0.0 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_slow_creep_depart_releases_after_short_confirm(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.33, a_lead=0.28, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) - for _ in range(max(frames, 1)): - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_slow_creep_depart_releases_near_stop_gap_with_modest_accel(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=5.7, v_lead=1.0, a_lead=0.09, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) - for _ in range(max(frames, 1)): - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_slow_creep_depart_does_not_release_on_gap_without_motion_signal(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.20, a_lead=0.05, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) + 2 - for _ in range(max(frames, 1)): - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_stationary_radar_lead_settles_excess_stop_gap_after_confirmation(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm( - 0.0, - desired_accel=-0.12, - min_accel=-0.5, - experimental_mode=True, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = True - - frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) - for _ in range(max(frames - 1, 1)): - planner.update(sm, make_toggles(model_version)) - assert planner.output_should_stop - - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target == pytest.approx(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_ACCEL) - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_stationary_radar_gap_settle_reholds_at_target_gap(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm( - 0.0, - desired_accel=-0.12, - min_accel=-0.5, - experimental_mode=True, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = True - - frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) - for _ in range(frames): - planner.update(sm, make_toggles(model_version)) - assert planner.radar_standstill_gap_settle_active - - sm["carState"].vEgo = 0.15 - sm["carState"].standstill = False - sm["radarState"].leadOne.dRel = 5.6 - planner.update(sm, make_toggles(model_version)) - - assert not planner.radar_standstill_gap_settle_active - assert planner.output_should_stop - assert planner.output_a_target <= 0.0 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_stationary_gap_settle_never_uses_vision_only_lead(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - sm = make_sm( - 0.0, - desired_accel=-0.12, - min_accel=-0.5, - experimental_mode=True, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=False, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = True - - frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) + 2 - for _ in range(frames): - planner.update(sm, make_toggles(model_version)) - - assert not planner.radar_standstill_gap_settle_active - assert planner.output_should_stop - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version): - v_ego = 0.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.1, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=15.0, v_lead=2.2, a_lead=0.4, radar=False, model_prob=0.99), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = False - sm["starpilotPlan"].vCruise = 10.0 - - for _ in range(12): - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target >= 0.2 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_stopped_lead_guard_blocks_false_release_at_longer_gap(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.03, - desired_accel=1.85, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=8.15, v_lead=0.04, a_lead=0.0, radar=False, model_prob=0.99), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.pid - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert planner.output_a_target <= 0.0 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_stopped_lead_guard_does_not_block_radar_depart_at_longer_gap(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=8.2, v_lead=0.72, a_lead=0.32, radar=True, model_prob=0.999, y_rel=0.1), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_stopped_lead_guard_blocks_false_release_during_creep_frame(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.2) - - sm = make_sm( - 0.2, - desired_accel=1.15, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=7.9, v_lead=0.03, a_lead=0.0, radar=False, model_prob=0.99), - ) - sm["carState"].standstill = False - sm["controlsState"].longControlState = LongCtrlState.pid - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert planner.output_a_target <= 0.0 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_confident_departing_lead_clears_stop_without_waiting_for_model_accel(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=3.95, v_lead=0.62, a_lead=1.05, radar=False, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = True - sm["starpilotPlan"].vCruise = 10.0 - - for _ in range(6): - planner.update(sm, make_toggles(model_version)) - assert planner.output_should_stop - - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target >= 0.2 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_confident_departing_lead_gets_depart_floor_with_zero_model_accel(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=4.10, v_lead=1.05, a_lead=1.20, radar=False, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = False - sm["starpilotPlan"].vCruise = 10.0 - - for _ in range(6): - planner.update(sm, make_toggles(model_version)) - assert planner.output_should_stop - - planner.update(sm, make_toggles(model_version)) - - assert not planner.output_should_stop - assert planner.output_a_target >= 0.25 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_confident_departing_lead_does_not_release_on_first_creep_frame(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.38, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=4.12, v_lead=0.32, a_lead=1.12, radar=False, model_prob=1.0), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["modelV2"].action.shouldStop = False - sm["starpilotPlan"].vCruise = 10.0 - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - assert planner.output_a_target <= 0.2 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_moving_lead_holds_depart_accel_floor_after_stop_release(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - toggles = make_toggles(model_version) - - sequence = [ - (0.0, True, 15.0, 2.2, 0.10), - (0.0, True, 15.2, 2.4, 0.12), - (0.0, True, 15.5, 2.6, 0.15), - (0.10, False, 15.8, 2.8, 0.20), - (0.25, False, 16.2, 3.0, 0.25), - (0.45, False, 16.8, 3.2, 0.30), - ] - - outputs = [] - for v_ego, standstill, d_rel, v_lead, desired_accel in sequence: - sm = make_sm( - v_ego, - desired_accel=desired_accel, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=0.0, radar=False, model_prob=0.99), - ) - sm["carState"].standstill = standstill - sm["controlsState"].longControlState = LongCtrlState.starting if standstill else LongCtrlState.pid - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, toggles) - outputs.append(planner.output_a_target) - - assert outputs[2] >= 0.25 - assert outputs[3] >= 0.25 - assert outputs[4] >= 0.25 - - -def test_route_251682_rav4_confirmed_depart_adds_bounded_accel_assist(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - lead = make_lead( - status=True, - d_rel=7.7, - v_lead=2.0, - a_lead=1.79, - radar=False, - model_prob=1.0, - ) - - floor = planner.get_lead_depart_accel_floor(lead, v_ego=0.0, model_desired_accel=0.44) - - assert 0.52 <= floor <= 0.54 - assert floor <= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_depart_accel_hold_reuses_floor_through_softening_lead_delta(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - toggles = make_toggles(model_version) - - sm_release = make_sm( - 0.0, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=4.1, v_lead=1.05, a_lead=1.2, radar=False, model_prob=1.0), - ) - sm_release["carState"].standstill = True - sm_release["controlsState"].longControlState = LongCtrlState.stopping - sm_release["starpilotPlan"].vCruise = 10.0 - sm_release["modelV2"].action.shouldStop = False - - for _ in range(6): - planner.update(sm_release, toggles) - assert planner.output_should_stop - - planner.update(sm_release, toggles) - assert not planner.output_should_stop - assert planner.output_a_target >= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL - - sm_hold = make_sm( - 0.5, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=5.8, v_lead=1.25, a_lead=0.08, radar=False, model_prob=1.0), - ) - sm_hold["carState"].standstill = False - sm_hold["controlsState"].longControlState = LongCtrlState.pid - sm_hold["starpilotPlan"].vCruise = 10.0 - sm_hold["modelV2"].action.shouldStop = False - - planner.update(sm_hold, toggles) - - assert not planner.output_should_stop - assert planner.output_a_target >= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_moving_lead_depart_accel_hold_cancels_if_lead_brakes(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - toggles = make_toggles(model_version) - - sm_release = make_sm( - 0.0, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=4.8, v_lead=1.0, a_lead=0.0, radar=False, model_prob=0.99), - ) - sm_release["carState"].standstill = True - sm_release["controlsState"].longControlState = LongCtrlState.starting - sm_release["starpilotPlan"].vCruise = 10.0 - sm_release["modelV2"].action.shouldStop = False - planner.update(sm_release, toggles) - - sm_brake = make_sm( - 0.18, - desired_accel=0.18, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=3.9, v_lead=0.1, a_lead=-0.4, radar=False, model_prob=0.99), - ) - sm_brake["controlsState"].longControlState = LongCtrlState.pid - sm_brake["starpilotPlan"].vCruise = 10.0 - sm_brake["modelV2"].action.shouldStop = True - - planner.update(sm_brake, toggles) - - assert planner.output_should_stop - assert planner.output_a_target < 0.1 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_radar_depart_kept_when_radar_lead_is_centered(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=0.2), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_radar_depart_blocks_offcenter_radar_conflict(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=2.3), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_low_speed_radar_depart_hold_blocks_offcenter_radar_conflict(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=1.25) - - sm = make_sm( - 1.25, - desired_accel=0.20, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999, y_rel=2.2), - ) - sm["carState"].standstill = False - sm["controlsState"].longControlState = LongCtrlState.pid - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2) - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_radar_takeoffs_toggle_bypasses_offcenter_veto(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - - sm = make_sm( - 0.0, - desired_accel=0.45, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=2.3), - ) - sm["carState"].standstill = True - sm["controlsState"].longControlState = LongCtrlState.stopping - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) - - planner.update(sm, make_toggles(model_version, radar_takeoffs=True)) - - assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_low_speed_radar_takeoffs_toggle_bypasses_offcenter_veto(model_version): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=1.25) - - sm = make_sm( - 1.25, - desired_accel=0.20, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=False, - lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999, y_rel=2.2), - ) - sm["carState"].standstill = False - sm["controlsState"].longControlState = LongCtrlState.pid - sm["starpilotPlan"].vCruise = 10.0 - sm["modelV2"].action.shouldStop = False - set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2) - - planner.update(sm, make_toggles(model_version, radar_takeoffs=True)) - - assert planner.output_a_target >= 0.0 - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): - far_v_ego = 29.26 - far_v_cruise = 32.22 - close_v_ego = 29.38 - close_v_cruise = 32.22 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner_far = LongitudinalPlanner(CP, init_v=far_v_ego) - planner_close = LongitudinalPlanner(CP, init_v=close_v_ego) - - sm_far = make_sm( - far_v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9), - ) - sm_close = make_sm( - close_v_ego, - desired_accel=0.2, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=68.0, v_lead=26.38, a_lead=-0.76, radar=True, model_prob=0.9), - ) - sm_far["starpilotPlan"].vCruise = far_v_cruise - sm_close["starpilotPlan"].vCruise = close_v_cruise - - for _ in range(80): - planner_far.update(sm_far, make_toggles(model_version)) - planner_close.update(sm_close, make_toggles(model_version)) - - assert planner_far.mode == "acc" - assert planner_close.mode == "acc" - assert planner_far.output_a_target > -0.4 - assert planner_close.output_a_target < planner_far.output_a_target - 0.1 - - -def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch): - monkeypatch.setenv("DEBUG", "0") - fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") - fake_commonmodel.DrivingModelFrame = object - fake_commonmodel.CLContext = object - monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel) - - from openpilot.selfdrive.modeld import modeld - - captured = {} - - def fake_get_accel_from_plan(speeds, accels, t_idxs, *, action_t, vEgoStopping): - captured["speeds"] = speeds - captured["accels"] = accels - captured["t_idxs"] = t_idxs - captured["action_t"] = action_t - captured["vEgoStopping"] = vEgoStopping - return 0.4, True - - monkeypatch.setattr(modeld, "get_accel_from_plan_tomb_raider", fake_get_accel_from_plan) - - plan = np.zeros((1, ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH), dtype=np.float32) - plan[0, :, Plan.VELOCITY] = 3.0 - plan[0, :, Plan.ACCELERATION] = -0.1 - prev_action = log.ModelDataV2.Action.new_message() - toggles = SimpleNamespace(vEgoStopping=0.42) - - action = modeld.get_action_from_model( - {"plan": plan}, - prev_action, - lat_action_t=0.2, - long_action_t=0.73, - v_ego=5.0, - mlsim=True, - is_v9=True, - is_v14=False, - is_v15=False, - starpilot_toggles=toggles, - ) - - assert captured["action_t"] == pytest.approx(0.73) - assert captured["vEgoStopping"] == pytest.approx(0.42) - assert list(captured["t_idxs"]) == ModelConstants.T_IDXS - np.testing.assert_allclose(captured["speeds"], 3.0) - np.testing.assert_allclose(captured["accels"], -0.1) - assert action.shouldStop - - -def test_modeld_action_uses_direct_action_head_for_v14(monkeypatch): - monkeypatch.setenv("DEBUG", "0") - fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") - fake_commonmodel.DrivingModelFrame = object - fake_commonmodel.CLContext = object - monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel) - - from openpilot.selfdrive.modeld import modeld - - prev_action = log.ModelDataV2.Action.new_message() - prev_action.desiredCurvature = 0.05 - prev_action.desiredAcceleration = -0.2 - toggles = SimpleNamespace(vEgoStopping=0.42) - - action = modeld.get_action_from_model( - {"action": np.array([[12.0, -0.8]], dtype=np.float32)}, - prev_action, - lat_action_t=0.2, - long_action_t=0.73, - v_ego=5.0, - mlsim=True, - is_v9=False, - is_v14=True, - is_v15=False, - starpilot_toggles=toggles, - ) - - assert action.desiredCurvature == pytest.approx(0.12) - assert action.desiredAcceleration < -0.2 - assert not action.shouldStop - - -def test_modeld_action_uses_current_action_head_scaling_for_v15(monkeypatch): - monkeypatch.setenv("DEBUG", "0") - fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") - fake_commonmodel.DrivingModelFrame = object - fake_commonmodel.CLContext = object - monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel) - - from openpilot.selfdrive.modeld import modeld - - prev_action = log.ModelDataV2.Action.new_message() - prev_action.desiredCurvature = 0.05 - prev_action.desiredAcceleration = -0.2 - toggles = SimpleNamespace(vEgoStopping=0.42) - - action = modeld.get_action_from_model( - {"action": np.array([[12.0, -0.8]], dtype=np.float32)}, - prev_action, - lat_action_t=0.2, - long_action_t=0.73, - v_ego=5.0, - mlsim=True, - is_v9=False, - is_v14=False, - is_v15=True, - starpilot_toggles=toggles, - ) - - assert action.desiredCurvature == pytest.approx(0.48) - assert action.desiredAcceleration < -0.2 - assert not action.shouldStop - - -def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero(): - class FakePM: - def __init__(self): - self.sent = {} - - def send(self, name, msg): - self.sent[name] = msg - - class FakeSM(dict): - def all_checks(self, service_list=None): - return True - - logMonoTime = {"modelV2": int(1e9)} - - v_ego = 5.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.output_a_target = -0.5 - planner.output_should_stop = False - planner.v_desired_trajectory = np.zeros(CONTROL_N) - planner.a_desired_trajectory = np.zeros(CONTROL_N) - planner.j_desired_trajectory = np.zeros(CONTROL_N) - planner.fcw = False - planner.mpc.source = "cruise" - planner.mpc.solve_time = 0.0 - pm = FakePM() - - sm = FakeSM(make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False)) - sm["starpilotPlan"].forcingStop = True - sm["starpilotPlan"].forcingStopLength = 5.0 - sm["starpilotPlan"].vCruise = 0.0 - - planner.publish(sm, pm) - - assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop - - -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_force_stop_handoff_sets_output_should_stop_before_zero_vcruise(model_version): - v_ego = 1.25 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm(v_ego, desired_accel=-0.35, min_accel=-1.0, experimental_mode=False) - sm["starpilotPlan"].forcingStop = True - sm["starpilotPlan"].forcingStopLength = 6.5 - sm["starpilotPlan"].vCruise = 0.4 - sm["modelV2"].action.shouldStop = False - - planner.update(sm, make_toggles(model_version)) - - assert planner.output_should_stop - - -def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_low(): - class FakePM: - def __init__(self): - self.sent = {} - - def send(self, name, msg): - self.sent[name] = msg - - class FakeSM(dict): - def all_checks(self, service_list=None): - return True - - logMonoTime = {"modelV2": int(1e9)} - - v_ego = 1.25 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.output_a_target = -0.35 - planner.output_should_stop = False - planner.v_desired_trajectory = np.zeros(CONTROL_N) - planner.a_desired_trajectory = np.zeros(CONTROL_N) - planner.j_desired_trajectory = np.zeros(CONTROL_N) - planner.fcw = False - planner.mpc.source = "cruise" - planner.mpc.solve_time = 0.0 - pm = FakePM() - - sm = FakeSM(make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False)) - sm["starpilotPlan"].forcingStop = True - sm["starpilotPlan"].forcingStopLength = 6.5 - sm["starpilotPlan"].vCruise = 0.4 - - planner.publish(sm, pm) - - assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop - - -def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): - v_ego = 10.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, gas_press_prob=0.5) - toggles = make_toggles() - - planner.update(sm, toggles) - assert planner.model_allow_throttle - assert planner.allow_throttle - - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.37) - planner.update(sm, toggles) - assert planner.model_allow_throttle - assert planner.allow_throttle - - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.34) - for _ in range(4): - planner.update(sm, toggles) - assert planner.model_allow_throttle - assert planner.allow_throttle - planner.update(sm, toggles) - assert not planner.model_allow_throttle - assert not planner.allow_throttle - - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.43) - planner.update(sm, toggles) - assert not planner.model_allow_throttle - assert not planner.allow_throttle - - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.46) - for _ in range(4): - planner.update(sm, toggles) - assert not planner.model_allow_throttle - assert not planner.allow_throttle - planner.update(sm, toggles) - assert planner.model_allow_throttle - assert planner.allow_throttle - - -def test_allow_throttle_confirmation_filters_route_length_model_pulses(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, gas_press_prob=0.6) - toggles = make_toggles() - - planner.update(sm, toggles) - - # Representative gasPressProb runs from route b85c25a4c6f99d83/0000000c: - # repeated 0.05-0.20 second threshold crossings must not change the coast cap. - for probability, frames in ((0.30, 2), (0.50, 1), (0.32, 3), (0.48, 4), (0.29, 4)): - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=probability) - for _ in range(frames): - planner.update(sm, toggles) - assert planner.model_allow_throttle - assert planner.allow_throttle - - # A sustained model request still applies the physical coast cap. - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.2) - for _ in range(5): - planner.update(sm, toggles) - assert not planner.model_allow_throttle - assert not planner.allow_throttle - - -def test_no_throttle_cap_stays_at_coast_limit_until_throttle_returns(): - v_ego = 8.5 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm(v_ego, desired_accel=0.0, min_accel=-3.0, experimental_mode=False, gas_press_prob=0.0) - sm["carControl"].orientationNED = [0.0, 0.1, 0.0] - toggles = make_toggles() - - for _ in range(5): - planner.update(sm, toggles) - - accel_coast = max(get_vehicle_min_accel(CP, v_ego), get_coast_accel(sm["carControl"].orientationNED[1])) - - assert not planner.allow_throttle - assert planner.output_a_target == pytest.approx(accel_coast, abs=1e-3) - - -def test_low_speed_follow_catchup_accel_cap_limits_close_vision_catchup(): - v_ego = 7.8 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=18.4, v_lead=8.2, radar=False, model_prob=0.98) - - cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) - - assert cap is not None - assert 0.15 <= cap <= 0.45 - - -def test_route_8bc6_post_departure_catchup_cap_uses_continuous_allowance_for_accelerating_radar_lead(): - v_ego = 19.03 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, - ) - - cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) - - assert cap is not None - assert 0.1 < cap < 0.3 - - -def test_route_8bc6_cruise_tracking_cap_uses_continuous_allowance_for_accelerating_radar_follow(): - v_ego = 18.744474411010742 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.650001525878906, v_lead=19.049156188964844, - a_lead=0.4783128798007965, radar=True, model_prob=0.9989967942237854, y_rel=-0.2, - ) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.3 < cap < 0.5 - - -def test_route_687_voacc_catchup_cap_uses_continuous_spacious_allowance(): - v_ego = 12.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, - ) - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.15 < cap < 0.3 - - -def test_route_687_voacc_cruise_tracking_cap_uses_continuous_spacious_allowance(): - v_ego = 12.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, - ) - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.15 < cap < 0.3 - - -def test_low_speed_follow_catchup_uses_raw_vehicle_speed_when_cluster_runs_high(): - v_ego = 7.8 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.6, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=16.0, v_lead=8.4, radar=False, model_prob=0.99), - ) - sm["carState"].vEgoCluster = 9.2 - sm["starpilotPlan"].vCruise = v_ego + 4.0 - - for _ in range(6): - planner.update(sm, make_toggles()) - - assert planner.output_a_target <= 0.20 - - -def test_low_speed_follow_transition_brake_cap_softens_first_sign_flip(): - v_ego = 7.7 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=18.6, v_lead=7.6, radar=False, model_prob=0.98) - - cap = planner.get_low_speed_follow_transition_brake_cap(lead, v_ego, 1.45, 0.59, -0.24) - - assert cap is not None - assert -0.14 <= cap <= -0.08 - - -def test_low_speed_follow_transition_brake_cap_stays_off_when_gap_is_tight(): - v_ego = 7.7 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=13.0, v_lead=7.6, radar=False, model_prob=0.98) - - cap = planner.get_low_speed_follow_transition_brake_cap(lead, v_ego, 1.45, 0.59, -0.24) - - assert cap is None - - -def test_far_near_speed_follow_keeps_uncertainty_smoothing_active(): - v_ego = 30.0 - - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=78.0, v_lead=29.2, radar=False, model_prob=0.96), - ) - sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=1.0, brake_press_prob=0.52) - toggles = make_toggles() - - for _ in range(12): - planner.update(sm, toggles) - - assert planner.mpc.filter_time_factor > 0.75 - - -def test_near_speed_follow_keeps_some_smoothing_under_high_uncertainty(): - v_ego = 31.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=53.0, v_lead=29.8, radar=False, model_prob=0.96) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=lead, - brake_press_prob=0.85, - ) - - for _ in range(16): - planner.update(sm, make_toggles()) - - assert planner.mpc.filter_time_factor >= 0.24 - - -def test_near_speed_follow_soft_brake_cap_limits_matched_follow_pulse(): - v_ego = 31.4 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=44.0, v_lead=30.1, radar=False, model_prob=0.96) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-1.0, - experimental_mode=False, - tracking_lead=True, - lead_one=lead, - brake_press_prob=0.85, - ) - - for _ in range(16): - planner.update(sm, make_toggles()) - - assert planner.mpc.filter_time_factor >= 0.24 - assert planner.output_a_target >= -0.33 - - -def test_near_speed_follow_soft_brake_cap_covers_slightly_opening_lead(): - v_ego = 29.58 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=44.44, v_lead=30.65, radar=False, model_prob=0.98) - - cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45) - - assert cap is not None - assert cap >= -0.14 - assert cap <= -0.06 - - -def test_near_speed_follow_soft_brake_cap_extends_to_spacious_modest_closing(): - v_ego = 23.69 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=49.48, v_lead=21.64, a_lead=-0.014, radar=False, model_prob=0.998) - - cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45) - - assert cap is not None - assert cap >= -0.33 - assert cap <= -0.22 - - -def test_near_speed_follow_soft_brake_cap_uses_raw_vehicle_speed_when_cluster_runs_high(): - v_ego = 23.0073 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=make_lead(status=True, d_rel=47.52, v_lead=21.68, a_lead=-0.0646, radar=False, model_prob=0.998), - ) - sm["carState"].vEgoCluster = 23.6931 - sm["starpilotPlan"].maxAcceleration = 0.61 - - for _ in range(6): - planner.update(sm, make_toggles()) - - assert not planner.lead_is_matched_follow_window(sm["radarState"].leadOne, sm["carState"].vEgoCluster, 1.45) - assert planner.output_a_target > -0.35 - assert planner.output_a_target < -0.22 - - -def test_near_speed_follow_soft_brake_cap_rejects_close_gap_even_with_modest_closing(): - v_ego = 37.19 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=34.70, v_lead=35.88, radar=False, model_prob=0.99) - - cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.0) - - assert cap is None - - -def test_follow_control_lead_prefers_active_lead1_for_matched_follow(): - v_ego = 23.3 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6) - planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98) - planner.mpc.source = "lead1" - - follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45) - - assert follow_lead is planner.lead_two - - -def test_follow_control_lead_disables_optional_matched_follow_override(): - v_ego = 23.3 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6) - planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98) - planner.mpc.source = "lead1" - - follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45, allow_optional_far_lead_logic=False) - - assert follow_lead is planner.lead_one - - -def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch(): - v_ego = 27.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99) - - follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45) - - assert follow_lead is planner.lead_one - - -def test_follow_control_lead_requires_real_lead_control_when_optional_logic_disabled(): - v_ego = 27.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99) - - follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45, allow_optional_far_lead_logic=False) - - assert follow_lead is None - - -def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead(): - v_ego = 32.37 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=82.07, v_lead=30.63, a_lead=-0.01, radar=False, model_prob=0.99) - - cap = planner.get_far_lead_brake_cap(lead, v_ego, 1.70) - - assert cap is not None - assert cap > -0.2 - assert cap < -0.05 - - -def test_far_lead_soft_brake_cap_limits_spacious_mild_closing_radar_lead(): - v_ego = 25.61 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=55.1, v_lead=24.11, a_lead=0.22, radar=True, model_prob=0.99) - - cap = planner.get_far_lead_brake_cap(lead, v_ego, 1.13) - - assert cap is not None - assert -0.11 < cap < -0.04 - - -@pytest.mark.parametrize( - "v_lead,a_lead", - [ - (22.9, 0.0), - (24.11, -0.5), - ], -) -def test_far_lead_soft_brake_cap_rejects_urgent_radar_lead(v_lead, a_lead): - v_ego = 25.61 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=55.1, v_lead=v_lead, a_lead=a_lead, radar=True, model_prob=0.99) - - assert planner.get_far_lead_brake_cap(lead, v_ego, 1.13) is None - - -def test_experimental_release_state_arms_only_on_falling_edge(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP) - - planner.update_experimental_release_accel_state(True, 10.0, v_ego=23.96) - assert planner.experimental_release_accel_until == 0.0 - - planner.update_experimental_release_accel_state(False, 10.1, v_ego=23.96) - assert planner.experimental_release_accel_until == pytest.approx( - 10.1 + longitudinal_planner_module.EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME - ) - - planner.update_experimental_release_accel_state(True, 10.2) - assert planner.experimental_release_accel_until == 0.0 - - -def test_experimental_release_state_holds_longer_at_low_speed(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP) - - planner.update_experimental_release_accel_state(True, 10.0, v_ego=6.75) - planner.update_experimental_release_accel_state(False, 10.1, v_ego=6.75) - - assert planner.experimental_release_accel_until == pytest.approx( - 10.1 + longitudinal_planner_module.EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_HOLD_TIME - ) - - -def test_experimental_release_accel_transition_damps_low_speed_slow_lead_handoff(): - v_ego = 6.75 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=11.0, v_lead=6.8, a_lead=0.0, radar=False, model_prob=0.99) - - target = planner.get_experimental_release_accel_target( - lead, - v_ego, - 1.13, - prev_output_a_target=-0.25, - output_a_target=-0.03, - release_active=True, - ) - - assert target == pytest.approx(-0.19) - - -def test_experimental_release_accel_transition_damps_moving_lead_handoff(): - v_ego = 23.96 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=55.15, v_lead=23.73, a_lead=0.40, radar=True, model_prob=0.997) - - target = planner.get_experimental_release_accel_target( - lead, - v_ego, - 1.13, - prev_output_a_target=0.03, - output_a_target=0.44, - release_active=True, - ) - - assert target == pytest.approx(0.09) - - -def test_experimental_release_accel_transition_does_not_mask_stopped_lead(): - v_ego = 23.96 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=35.0, v_lead=0.0, a_lead=-1.0, radar=True, model_prob=0.997) - - target = planner.get_experimental_release_accel_target( - lead, - v_ego, - 1.13, - prev_output_a_target=-0.4, - output_a_target=0.4, - release_active=True, - ) - - assert target is None - - -def test_planner_arms_experimental_release_accel_only_on_mode_exit(): - v_ego = 23.96 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=55.15, v_lead=23.73, a_lead=0.40, radar=True, model_prob=0.997) - sm = make_sm( - v_ego, - desired_accel=0.0, - min_accel=-1.0, - experimental_mode=True, - tracking_lead=True, - lead_one=lead, - ) - - release_states = [] - original = planner.get_experimental_release_accel_target - - def record_release_state(self, *args, **kwargs): - release_states.append(bool(args[-1])) - return original(*args, **kwargs) - - planner.get_experimental_release_accel_target = types.MethodType(record_release_state, planner) - planner.update(sm, make_toggles()) - sm["selfdriveState"].experimentalMode = False - planner.update(sm, make_toggles()) - - assert release_states == [False, True] - - -def test_matched_follow_transition_target_damps_large_comfort_sign_flip(): - v_ego = 20.3 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=45.6, v_lead=19.19, a_lead=0.0, radar=False, model_prob=0.99) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.12, - output_a_target=-0.40, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed > -0.05 - assert smoothed < 0.12 - - -def test_matched_follow_transition_target_skips_urgent_closure(): - v_ego = 31.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=35.0, v_lead=28.0, a_lead=0.0, radar=False, model_prob=0.99) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.10, - output_a_target=-0.60, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_matched_follow_transition_target_damps_low_speed_tracking_cruise_throttle_jitter(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.08, - output_a_target=0.46, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(0.14, abs=1e-6) - - -def test_route_8bc6_opening_radar_catchup_cap_does_not_stab_out_throttle(): - v_ego = 15.60 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=24.3, v_lead=16.61, a_lead=0.82, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=1.31, - output_a_target=0.32, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert 1.20 < smoothed < 1.31 - - -def test_opening_radar_catchup_smoothing_yields_immediately_if_lead_brakes(): - v_ego = 15.60 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=24.3, v_lead=16.61, a_lead=-0.50, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=1.31, - output_a_target=0.32, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_opening_radar_catchup_smoothing_has_no_pullaway_boundary_jump(): - v_ego = 16.30 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=25.5, v_lead=17.54, a_lead=0.68, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=0.51, - output_a_target=0.87, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed == pytest.approx(0.56, abs=1e-6) - - -def test_route_8bc6_radar_lead_source_handoff_keeps_catchup_transition_smooth(): - v_ego = 24.44 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=37.1, v_lead=24.46, a_lead=0.33, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.15, - prev_output_a_target=0.42, - output_a_target=0.26, - current_source="lead0", - tracking_lead_active=True, - ) - - assert smoothed == pytest.approx(0.37, abs=1e-6) - - -def test_opening_radar_catchup_smoothing_stays_off_inside_requested_headway(): - v_ego = 15.60 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=21.0, v_lead=16.61, a_lead=0.82, radar=True, model_prob=1.0) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.25, - prev_output_a_target=1.31, - output_a_target=0.32, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_matched_follow_transition_target_skips_low_speed_without_tracking(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.08, - output_a_target=0.46, - current_source="cruise", - tracking_lead_active=False, - ) - - assert smoothed is None - - -def test_matched_follow_transition_target_skips_low_speed_real_braking(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=29.0, v_lead=13.6, a_lead=0.0, radar=False, model_prob=0.999) - - smoothed = planner.get_matched_follow_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.08, - output_a_target=-0.30, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_cruise_tracking_lead_accel_cap_limits_mid_speed_follow_nibble(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.3 <= cap <= 0.5 - - -def test_cruise_tracking_lead_accel_cap_blocks_unresolved_raw_close_lead_burst(): - v_ego = 17.6 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=41.9, v_lead=14.2, a_lead=0.0, radar=True, model_prob=0.99, y_rel=-0.97) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=False, - ) - - assert cap is not None - assert 0.0 <= cap <= 0.05 - - -def test_cruise_tracking_lead_accel_cap_skips_when_lead_clearly_pulls_away(): - v_ego = 14.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=35.0, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.1) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead(): - v_ego = 11.4 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=20.5, v_lead=12.4, a_lead=0.46, radar=True, model_prob=1.0, y_rel=0.1) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_cruise_tracking_lead_accel_cap_continuously_limits_spacious_tracking_only_follow(): - v_ego = 18.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=45.0, v_lead=17.4, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.1) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.3 < cap < 0.55 - - -def test_route_8bc6_radar_follow_caps_do_not_flip_between_bypass_and_hold(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=17.35) - route_states = ( - (17.35, 36.2, 16.55, 0.60), - (17.26, 34.8, 17.36, 0.40), - (18.78, 35.2, 18.58, 0.40), - (18.87, 35.0, 19.07, 0.40), - ) - - caps = [] - for v_ego, d_rel, v_lead, a_lead in route_states: - lead = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, - radar=True, model_prob=1.0, y_rel=0.2) - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - assert cap is not None - caps.append(cap) - - assert min(caps) > 0.25 - assert max(caps) < 0.65 - assert max(caps) - min(caps) < 0.35 - - -def test_inside_gap_closing_lead_cap_blocks_route_accel_burst(): - v_ego = 17.1 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=26.3, v_lead=16.5, a_lead=0.0, radar=True, model_prob=1.0) - - cap = planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25) - - assert cap is not None - assert cap == pytest.approx(0.0) - - -def test_inside_gap_closing_lead_cap_strengthens_with_route_closure(): - v_ego = 19.8 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=23.3, v_lead=17.3, a_lead=0.0, radar=True, model_prob=1.0) - - cap = planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25) - - assert cap is not None - assert -0.7 <= cap <= -0.5 - - -@pytest.mark.parametrize("lead", [ - make_lead(status=True, d_rel=36.0, v_lead=16.5, radar=True, model_prob=1.0), - make_lead(status=True, d_rel=26.3, v_lead=17.8, radar=True, model_prob=1.0), - make_lead(status=True, d_rel=26.3, v_lead=16.5, radar=False, model_prob=0.8), - make_lead(status=True, d_rel=26.3, v_lead=16.5, radar=True, model_prob=1.0, y_rel=2.0), -]) -def test_inside_gap_closing_lead_cap_ignores_normal_or_ambiguous_follow(lead): - v_ego = 17.1 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - - assert planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25) is None - - -def test_inside_gap_closing_lead_cap_does_not_touch_standstill_departure(): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=0.0) - lead = make_lead(status=True, d_rel=5.0, v_lead=1.0, a_lead=0.5, radar=True, model_prob=1.0) - - assert planner.get_inside_gap_closing_lead_accel_cap(lead, 0.0, -1.0, 1.25) is None - - -def test_route_8bc6_post_departure_cruise_cap_uses_continuous_allowance_for_accelerating_radar_lead(): - v_ego = 19.03 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, - ) - - cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.2 < cap < 0.35 - - -def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_radar_follow_outside_cap_window(): - v_ego = 24.108949661254883 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=36.75, v_lead=24.234233856201172, - a_lead=0.3050585687160492, radar=True, model_prob=0.9995405077934265, y_rel=-0.2, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.160530924797058, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelerates_away(): - v_ego = 22.293441772460938 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=36.5, v_lead=22.264942169189453, - a_lead=0.2780209183692932, radar=True, model_prob=0.999357283115387, y_rel=0.35, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.2014657258987427, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap is None - - -def test_route_8bc6_catchup_cap_continuously_limits_accelerating_lead_when_source_flips_to_lead0(): - v_ego = 24.361867904663086 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=38.5, v_lead=24.46365737915039, - a_lead=0.4625629186630249, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.3549551963806152, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap is not None - assert 0.03 < cap < 0.10 - - -def test_route_8bc6_radar_matched_follow_catchup_cap_continuously_limits_mild_pullaway_after_lead_lock(): - v_ego = 23.96 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.6, v_lead=24.23, a_lead=0.16, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.16, - current_source="lead0", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) - assert cap is not None - assert 0.1 < cap < 0.25 - - -def test_route_8bc6_radar_matched_follow_catchup_cap_keeps_cap_when_pullaway_is_not_confirmed(): - v_ego = 23.96 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.6, v_lead=24.23, a_lead=0.08, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.16, - current_source="lead0", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) - assert cap is not None - - -def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave(): - v_ego = 22.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=41.558, v_lead=22.3, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) - assert cap is None - - -def test_route_8bc6_radar_matched_follow_catchup_cap_holds_small_cap_for_slower_lead_on_cruise(): - v_ego = 22.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=39.108, v_lead=21.2, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) - assert cap == pytest.approx(0.04, abs=1e-6) - - -def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_catchup_cap(): - v_ego = 16.4023914337 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=25.75, v_lead=15.9339208603, - a_lead=0.1164954603, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap_without_latch = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - cap_with_latch = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap_without_latch == pytest.approx(0.03, abs=1e-6) - assert cap_with_latch is None - - -@pytest.mark.parametrize("v_ego,d_rel,v_lead,a_lead", [ - (8.05, 14.75, 8.56, 0.25), - (10.65, 14.35, 11.59, 1.38), - (8.34, 11.25, 8.33, 0.97), -]) -def test_route_8bc6_rolling_departure_arms_settle_latch(v_ego, d_rel, v_lead, a_lead): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=d_rel, v_lead=v_lead, - a_lead=a_lead, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap is None - assert planner.post_departure_follow_settle_until > time.monotonic() - - -def test_route_8bc6_radar_catchup_cap_enters_continuously_above_min_speed(): - v_ego = 8.69 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=11.25, v_lead=8.73, - a_lead=1.10, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert planner.post_departure_follow_settle_until == 0.0 - assert 1.20 < cap < 1.30 - - -def test_rolling_departure_settle_latch_stays_active_through_headway_hysteresis(): - v_ego = 10.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - departing_lead = make_lead( - status=True, d_rel=13.4, v_lead=10.3, - a_lead=0.3, radar=True, model_prob=1.0, y_rel=0.0, - ) - settling_lead = make_lead( - status=True, d_rel=13.2, v_lead=10.2, - a_lead=0.1, radar=True, model_prob=1.0, y_rel=0.0, - ) - - assert planner.post_departure_follow_settle_active(departing_lead, v_ego, 1.25) - assert planner.post_departure_follow_settle_active(settling_lead, v_ego, 1.25) - - -def test_rolling_departure_settle_latch_supports_confident_vision_lead(): - v_ego = 10.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=13.4, v_lead=10.3, - a_lead=0.3, radar=False, model_prob=0.95, y_rel=0.0, - ) - - assert planner.post_departure_follow_settle_active(lead, v_ego, 1.25) - - -def test_rolling_departure_settle_latch_clears_for_stopped_lead(): - v_ego = 10.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - departing_lead = make_lead( - status=True, d_rel=13.4, v_lead=10.3, - a_lead=0.3, radar=True, model_prob=1.0, y_rel=0.0, - ) - stopped_lead = make_lead( - status=True, d_rel=12.0, v_lead=0.0, - a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, - ) - - assert planner.post_departure_follow_settle_active(departing_lead, v_ego, 1.25) - assert not planner.post_departure_follow_settle_active(stopped_lead, v_ego, 1.25) - assert planner.post_departure_follow_settle_until == 0.0 - - -@pytest.mark.parametrize("v_ego,d_rel,v_lead,a_lead", [ - (10.0, 14.0, 10.5, -0.2), - (10.0, 12.5, 11.0, 0.5), - (20.0, 29.0, 20.5, 0.5), -]) -def test_rolling_departure_settle_latch_does_not_arm_without_safe_departure(v_ego, d_rel, v_lead, a_lead): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=d_rel, v_lead=v_lead, - a_lead=a_lead, radar=True, model_prob=1.0, y_rel=0.0, - ) - - assert not planner.post_departure_follow_settle_active(lead, v_ego, 1.25) - assert planner.post_departure_follow_settle_until == 0.0 - - -def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_cruise_cap(): - v_ego = 19.2975330353 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=28.9, v_lead=19.6019687653, - a_lead=0.1241656989, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap_without_latch = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - cap_with_latch = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - assert cap_without_latch is not None - assert 0.10 < cap_without_latch < 0.25 - assert cap_with_latch is None - - -def test_post_departure_settle_latch_does_not_bypass_when_lead_brakes_again(): - v_ego = 19.192998192 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - lead = make_lead( - status=True, d_rel=30.5, v_lead=19.147550216, - a_lead=-0.16, radar=True, model_prob=1.0, y_rel=0.2, - ) - - catchup_cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - cruise_cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.25, - current_source="cruise", - tracking_lead_active=True, - ) - - assert catchup_cap is not None - assert cruise_cap is not None - assert planner.post_departure_follow_settle_until == 0.0 - - -def test_post_departure_settle_latch_clears_once_follow_has_settled_near_target(): - v_ego = 16.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - planner.post_departure_follow_settle_until = time.monotonic() + 5.0 - lead = make_lead( - status=True, d_rel=20.48, v_lead=16.25, - a_lead=0.18, radar=True, model_prob=1.0, y_rel=0.0, - ) - - cap = planner.get_lead_catchup_accel_cap( - lead, - v_ego, - 1.25, - current_source="lead0", - tracking_lead_active=True, - ) - - assert cap is not None - assert planner.post_departure_follow_settle_until == 0.0 - - -def test_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again(): - v_ego = 19.03 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead( - status=True, d_rel=33.85, v_lead=18.90, a_lead=-0.35, radar=True, model_prob=1.0, y_rel=-0.30, - ) - - catchup_cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) - cruise_cap = planner.get_cruise_tracking_lead_accel_cap( - lead, - v_ego, - 1.45, - current_source="cruise", - tracking_lead_active=True, - ) - - assert catchup_cap is not None - assert cruise_cap is not None - - -def test_cruise_tracking_lead_accel_transition_target_damps_mid_speed_reacquisition_burst(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=20.0, v_lead=17.4, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.18) - - smoothed = planner.get_cruise_tracking_lead_accel_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.10, - output_a_target=1.10, - current_source="cruise", - ) - - assert smoothed is not None - assert smoothed == pytest.approx(0.23, abs=1e-2) - - -def test_cruise_tracking_lead_accel_transition_target_skips_clear_pullaway(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=34.0, v_lead=19.2, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) - - smoothed = planner.get_cruise_tracking_lead_accel_transition_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.10, - output_a_target=1.10, - current_source="cruise", - ) - - assert smoothed is None - - -def test_mild_follow_zero_cross_guard_coasts_on_cruise_sign_flip(): - v_ego = 19.8 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=42.0, v_lead=19.0, a_lead=-0.15, radar=False, model_prob=0.99, y_rel=0.10) - - guarded = planner.get_mild_follow_zero_cross_guard_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.18, - output_a_target=-0.12, - current_source="cruise", - tracking_lead_active=True, - ) - - assert guarded == pytest.approx(0.0, abs=1e-6) - - -def test_mild_follow_zero_cross_guard_coasts_on_near_duplicate_sign_flip(): - v_ego = 24.5 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=51.0, v_lead=21.7, a_lead=-0.25, radar=False, model_prob=0.99, y_rel=0.08) - - guarded = planner.get_mild_follow_zero_cross_guard_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.33, - output_a_target=-0.13, - current_source="lead1", - tracking_lead_active=True, - ) - - assert guarded == pytest.approx(0.0, abs=1e-6) - - -def test_mild_follow_zero_cross_guard_skips_urgent_close_follow(): - v_ego = 17.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=16.0, v_lead=12.0, a_lead=-0.45, radar=False, model_prob=0.99, y_rel=0.05) - - guarded = planner.get_mild_follow_zero_cross_guard_target( - lead, - v_ego, - 1.45, - prev_output_a_target=0.22, - output_a_target=-0.28, - current_source="cruise", - tracking_lead_active=True, - ) - - assert guarded is None - - -def test_post_097_follow_logic_is_always_active(): - v_ego = 16.2 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) - - planner = LongitudinalPlanner(CP, init_v=v_ego) - sm = make_sm( - v_ego, - desired_accel=0.8, - min_accel=-0.5, - experimental_mode=False, - tracking_lead=True, - lead_one=lead, - ) - sm["starpilotPlan"].vCruise = v_ego + 8.0 - - calls = [] - - def record(name, return_value=None): - def _inner(self, *args, **kwargs): - calls.append(name) - return return_value - return types.MethodType(_inner, planner) - - planner.get_follow_control_lead = record("get_follow_control_lead", lead) - planner.get_lead_catchup_accel_cap = record("get_lead_catchup_accel_cap", None) - planner.get_tracked_vision_model_brake_floor = record("get_tracked_vision_model_brake_floor", None) - planner.get_low_speed_follow_transition_brake_cap = record("get_low_speed_follow_transition_brake_cap", None) - planner.get_tracked_vision_model_brake_cap = record("get_tracked_vision_model_brake_cap", None) - planner.get_cruise_tracking_lead_accel_cap = record("get_cruise_tracking_lead_accel_cap", None) - planner.get_cruise_tracking_lead_accel_transition_target = record("get_cruise_tracking_lead_accel_transition_target", None) - - planner.update(sm, make_toggles()) - - expected_calls = { - "get_lead_catchup_accel_cap", - "get_follow_control_lead", - "get_tracked_vision_model_brake_floor", - "get_low_speed_follow_transition_brake_cap", - "get_tracked_vision_model_brake_cap", - "get_cruise_tracking_lead_accel_cap", - "get_cruise_tracking_lead_accel_transition_target", - } - - assert expected_calls.issubset(set(calls)) - - -def test_near_duplicate_lead_source_hysteresis_prefers_previous_source(): - v_ego = 27.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=46.2, v_lead=25.5, a_lead=-0.05, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=46.8, v_lead=25.55, a_lead=-0.03, radar=False, model_prob=0.99) - - lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego) - - assert lead_0_bias == 0.0 - assert lead_1_bias > 0.0 - - -def test_stable_follow_cruise_hysteresis_applies_for_radar_lead(): - v_ego = 27.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=40.5, v_lead=26.3, a_lead=-0.02, radar=True, model_prob=1.0) - - hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45) - - assert hysteresis > 0.0 - - -def test_stable_follow_cruise_hysteresis_holds_pullaway_lead_longer_near_target_gap(): - v_ego = 15.0 - t_follow = 1.45 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_matched = make_lead(status=True, d_rel=22.0, v_lead=15.0, a_lead=0.02, radar=True, model_prob=1.0) - lead_pullaway = make_lead(status=True, d_rel=22.0, v_lead=16.4, a_lead=0.02, radar=True, model_prob=1.0) - - matched_hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead_matched, v_ego, t_follow) - pullaway_hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead_pullaway, v_ego, t_follow) - - assert pullaway_hysteresis > matched_hysteresis - - -def test_stable_follow_cruise_hysteresis_skips_fast_closing_radar_lead(): - v_ego = 27.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead = make_lead(status=True, d_rel=40.5, v_lead=21.5, a_lead=-0.02, radar=True, model_prob=1.0) - - hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45) - - assert hysteresis == 0.0 - - -def test_vision_follow_cruise_hold_keeps_high_confidence_matched_lead_through_small_crossover(): - v_ego = 22.5 - t_follow = 1.20 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=-0.02, radar=False, model_prob=0.99) - lead_two = make_lead(status=False) - - sticky = planner.mpc.get_vision_follow_cruise_hold( - "lead0", - lead_one, - lead_two, - 101.0, - 200.0, - 100.0, - v_ego, - t_follow, - True, - ) - - assert sticky == "lead0" - - -@pytest.mark.parametrize("tracking_lead, radar, model_prob, cruise_obstacle", [ - (False, False, 0.99, 100.0), - (True, True, 1.0, 100.0), - (True, False, 0.80, 100.0), - (True, False, 0.99, 98.0), -]) -def test_vision_follow_cruise_hold_skips_nonmatching_or_clear_cruise_cases( - tracking_lead, radar, model_prob, cruise_obstacle, -): - v_ego = 22.5 - t_follow = 1.20 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=-0.02, radar=radar, model_prob=model_prob) - lead_two = make_lead(status=False) - - sticky = planner.mpc.get_vision_follow_cruise_hold( - "lead0", - lead_one, - lead_two, - 101.0, - 200.0, - cruise_obstacle, - v_ego, - t_follow, - tracking_lead, - ) - - assert sticky is None - - -@pytest.mark.parametrize("prev_source, lead_accel", [ - ("cruise", -0.02), - ("lead0", -0.60), -]) -def test_vision_follow_cruise_hold_never_delays_restrictive_transition(prev_source, lead_accel): - v_ego = 22.5 - t_follow = 1.20 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=lead_accel, radar=False, model_prob=0.99) - lead_two = make_lead(status=False) - - sticky = planner.mpc.get_vision_follow_cruise_hold( - prev_source, - lead_one, - lead_two, - 101.0, - 200.0, - 100.0, - v_ego, - t_follow, - True, - ) - - assert sticky is None - - -def test_near_duplicate_lead_source_hysteresis_prefers_previous_source_for_identical_radar_track(): - v_ego = 27.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=34.6, v_lead=24.2, a_lead=-0.04, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=34.7, v_lead=24.2, a_lead=-0.04, radar=True, model_prob=1.0) - lead_one.vRel = -0.8 - lead_two.vRel = -0.78 - lead_one.radarTrackId = 123 - lead_two.radarTrackId = 123 - - lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego) - - assert lead_0_bias == 0.0 - assert lead_1_bias > 0.0 - - -def test_near_duplicate_leads_detect_identical_radar_track_below_45_mph(): - v_ego = 14.31 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - - assert planner.mpc.leads_are_near_duplicates(lead_one, lead_two, v_ego) - - -def test_identical_radar_duplicate_source_hold_keeps_previous_label(): - v_ego = 21.6 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=33.5, v_lead=20.7, a_lead=-0.03, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=33.5, v_lead=20.7, a_lead=-0.03, radar=True, model_prob=1.0) - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - - sticky = planner.mpc.get_identical_radar_duplicate_source_hold("lead1", lead_one, lead_two, 33.52, 33.50) - - assert sticky == "lead1" - - -def test_identical_radar_duplicate_cruise_hold_keeps_previous_lead_through_route_like_crossover(): - v_ego = 22.57 - t_follow = 1.20 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=32.3, v_lead=23.74, a_lead=0.67, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=32.3, v_lead=23.74, a_lead=0.67, radar=True, model_prob=1.0) - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - - sticky = planner.mpc.get_identical_radar_duplicate_cruise_hold( - "lead0", - lead_one, - lead_two, - 144.92, - 144.91, - 135.10, - v_ego, - t_follow, - ) - - assert sticky == "lead0" - - -def test_identical_radar_duplicate_cruise_hold_skips_clear_pullaway(): - v_ego = 22.57 - t_follow = 1.20 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=48.0, v_lead=25.5, a_lead=0.45, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=48.0, v_lead=25.5, a_lead=0.45, radar=True, model_prob=1.0) - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - - sticky = planner.mpc.get_identical_radar_duplicate_cruise_hold( - "lead0", - lead_one, - lead_two, - 180.0, - 180.0, - 150.0, - v_ego, - t_follow, - ) - - assert sticky is None - - -def test_identical_radar_duplicate_cruise_bias_penalizes_near_target_follow(): - v_ego = 23.8 - t_follow = 1.15 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0) - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - - bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) - - assert bias > 0.0 - - -def test_identical_radar_duplicate_cruise_bias_skips_far_pullaway_follow(): - v_ego = 23.8 - t_follow = 1.15 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0) - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - - bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) - - assert bias == 0.0 - - -def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads(): - v_ego = 27.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=41.0, v_lead=23.8, a_lead=0.0, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=48.0, v_lead=25.4, a_lead=0.0, radar=False, model_prob=0.99) - - lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego) - - assert lead_0_bias == 0.0 - assert lead_1_bias == 0.0 - - -def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_offset_candidate(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) - lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - selected = planner.get_duplicate_vision_comfort_lead(v_ego) - - assert selected is lead_one - assert planner.duplicate_vision_comfort_lead_source == "lead0" - - -def test_duplicate_vision_comfort_lead_supports_mid_speed_follow_churn(): - v_ego = 16.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=30.0, v_lead=15.8, a_lead=0.02, radar=False, model_prob=1.0, y_rel=0.05) - lead_two = make_lead(status=True, d_rel=30.1, v_lead=15.82, a_lead=0.01, radar=False, model_prob=1.0, y_rel=0.08) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - selected = planner.get_duplicate_vision_comfort_lead(v_ego) - - assert selected is lead_one - - -def test_duplicate_vision_comfort_lead_latches_source_until_duplicate_cluster_resolves(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) - lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - initial = planner.get_duplicate_vision_comfort_lead(v_ego) - - lead_one.yRel = 1.25 - lead_two.yRel = 0.05 - lead_one.dRel = 44.4 - lead_two.dRel = 44.5 - latched = planner.get_duplicate_vision_comfort_lead(v_ego) - - assert initial is lead_one - assert latched is lead_one - assert planner.duplicate_vision_comfort_lead_source == "lead0" - - -def test_duplicate_vision_comfort_lead_resets_when_duplicate_cluster_clears(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) - lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - assert planner.get_duplicate_vision_comfort_lead(v_ego) is lead_one - - lead_two.dRel = 49.5 - assert planner.get_duplicate_vision_comfort_lead(v_ego) is None - assert planner.duplicate_vision_comfort_lead_source is None - - -def test_nonurgent_duplicate_vision_follow_keeps_comfort_smoothing(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=35.0, v_lead=20.5, a_lead=-0.05, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=35.2, v_lead=20.52, a_lead=-0.04, radar=False, model_prob=0.99) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - assert planner.is_nonurgent_duplicate_vision_follow(v_ego, 1.45) - - -@pytest.mark.parametrize("d_rel,v_lead,a_lead", [ - (24.0, 20.0, -0.05), # Low TTC. - (22.0, 23.0, -0.05), # Headway materially below the requested gap. - (35.0, 20.5, -0.50), # The lead is braking. -]) -def test_duplicate_vision_follow_preserves_urgent_panic_bypass(d_rel, v_lead, a_lead): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=d_rel + 0.2, v_lead=v_lead + 0.02, a_lead=a_lead, radar=False, model_prob=0.99) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - assert not planner.is_nonurgent_duplicate_vision_follow(v_ego, 1.45) - - -def test_radar_duplicates_preserve_panic_bypass(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=35.0, v_lead=20.5, a_lead=-0.05, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=35.0, v_lead=20.5, a_lead=-0.05, radar=True, model_prob=1.0) - lead_one.radarTrackId = 17 - lead_two.radarTrackId = 17 - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - assert not planner.is_nonurgent_duplicate_vision_follow(v_ego, 1.45) - - -def test_near_duplicate_lead_transition_target_damps_same_source_sign_flip(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) - lead_one.vRel = -0.95 - lead_two.vRel = -1.00 - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_two, - v_ego, - 1.45, - prev_output_a_target=-1.10, - output_a_target=0.13, - current_source="lead1", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(-0.92, abs=1e-6) - - -def test_near_duplicate_lead_transition_target_damps_tracking_cruise_sign_flip(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) - lead_one.vRel = -0.95 - lead_two.vRel = -1.00 - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_two, - v_ego, - 1.45, - prev_output_a_target=-1.10, - output_a_target=0.13, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(-0.92, abs=1e-6) - - -def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_handoff(): - v_ego = 17.61 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=41.9, v_lead=16.85, a_lead=0.0, radar=True, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=41.9, v_lead=16.85, a_lead=0.0, radar=True, model_prob=1.0) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - lead_one.radarTrackId = 2493 - lead_two.radarTrackId = 2493 - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_one, - v_ego, - 1.45, - prev_output_a_target=0.89, - output_a_target=0.05, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(0.57, abs=1e-6) - - -def test_near_duplicate_lead_transition_target_damps_mid_speed_duplicate_vision_handoff(): - v_ego = 17.6 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.8, v_lead=16.8, a_lead=0.0, radar=False, model_prob=1.0) - lead_two = make_lead(status=True, d_rel=44.9, v_lead=16.82, a_lead=0.0, radar=False, model_prob=1.0) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_one, - v_ego, - 1.25, - prev_output_a_target=0.49, - output_a_target=0.03, - current_source="cruise", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(0.17, abs=1e-6) - - -def test_near_duplicate_lead_transition_target_damps_generous_headway_duplicate_vision_sign_flip(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=54.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=54.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_two, - v_ego, - 1.45, - prev_output_a_target=0.70, - output_a_target=-0.61, - current_source="lead1", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(0.52, abs=1e-6) - - -def test_near_duplicate_lead_transition_target_skips_fast_duplicate_vision_sign_flip_when_headway_is_not_generous(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=43.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=43.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_two, - v_ego, - 1.45, - prev_output_a_target=0.70, - output_a_target=-0.61, - current_source="lead1", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_duplicate_slow_lead_brake_hold_prevents_zero_cross_from_duplicate_voacc_leads(): - v_ego = 24.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998) - lead_two = make_lead(status=True, d_rel=19.25, v_lead=20.02, a_lead=-0.41, radar=False, model_prob=0.996) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_duplicate_slow_lead_brake_hold_target( - lead_one, - v_ego, - 1.0, - prev_output_a_target=-3.50, - output_a_target=0.0, - current_source="lead0", - tracking_lead_active=True, - ) - - assert smoothed is not None - assert smoothed == pytest.approx(-3.28, abs=1e-6) - - -def test_duplicate_slow_lead_brake_hold_skips_distinct_leads(): - v_ego = 24.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998) - lead_two = make_lead(status=True, d_rel=24.0, v_lead=21.5, a_lead=-0.10, radar=False, model_prob=0.996) - lead_one.vRel = lead_one.vLead - v_ego - lead_two.vRel = lead_two.vLead - v_ego - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_duplicate_slow_lead_brake_hold_target( - lead_one, - v_ego, - 1.0, - prev_output_a_target=-3.50, - output_a_target=0.0, - current_source="lead0", - tracking_lead_active=True, - ) - - assert smoothed is None - - -def test_near_duplicate_lead_transition_target_skips_plain_cruise_without_tracking(): - v_ego = 25.0 - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - planner = LongitudinalPlanner(CP, init_v=v_ego) - lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) - lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) - lead_one.vRel = -0.95 - lead_two.vRel = -1.00 - planner.lead_one = lead_one - planner.lead_two = lead_two - - smoothed = planner.get_near_duplicate_lead_transition_target( - lead_two, - v_ego, - 1.45, - prev_output_a_target=-1.10, - output_a_target=0.13, - current_source="cruise", - tracking_lead_active=False, - ) - - assert smoothed is None diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py index 25ddb3112..611b4b99d 100644 --- a/selfdrive/controls/tests/test_starpilot_acceleration.py +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -18,7 +18,7 @@ from openpilot.starpilot.controls.lib.starpilot_acceleration import ( class FakePlanner: def __init__(self, *, v_cruise=0.0, slc_target=0.0, slc_offset=0.0, overridden_speed=0.0, - red_light=False, forcing_stop=False, disable_throttle=False): + red_light=False, forcing_stop=False): self.v_cruise = v_cruise self.starpilot_weather = SimpleNamespace(weather_id=0, reduce_acceleration=0.0) self.starpilot_vcruise = SimpleNamespace( @@ -27,8 +27,7 @@ class FakePlanner: forcing_stop=forcing_stop, slc=SimpleNamespace(overridden_speed=overridden_speed), ) - self.starpilot_cem = SimpleNamespace(stop_light_detected=red_light) - self.starpilot_following = SimpleNamespace(disable_throttle=disable_throttle) + self.longitudinal_intent = SimpleNamespace(stop_detected=red_light) def make_toggles(**overrides): diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 5e3243aaa..a93751be8 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -21,7 +21,6 @@ class FakeSM(dict): def make_toggles(**overrides): defaults = { "compass": False, - "conditional_experimental_mode": False, "minimum_lane_change_speed": 100.0, "pause_lateral_below_speed": 10.0, "pause_lateral_below_signal": True, @@ -61,8 +60,7 @@ def make_planner(monkeypatch): monkeypatch.setattr(planner.starpilot_following, "update", lambda *args, **kwargs: None) monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0) monkeypatch.setattr(planner.starpilot_weather, "update_weather", lambda *args, **kwargs: None) - monkeypatch.setattr(planner.starpilot_cem, "stop_sign_and_light", lambda *args, **kwargs: None) - monkeypatch.setattr(planner, "update_lead_status", lambda *args, **kwargs: False) + monkeypatch.setattr(planner.longitudinal_intent, "update", lambda *args, **kwargs: None) return planner @@ -81,7 +79,6 @@ def test_lateral_resume_delay_zero_keeps_immediate_resume(monkeypatch): finally: planner.shutdown() - def test_lateral_resume_delay_holds_resume_after_low_speed_turn(monkeypatch): planner = make_planner(monkeypatch) @@ -116,68 +113,3 @@ def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monke assert planner.blinker_delay_active is False finally: planner.shutdown() - - -def test_radarless_follow_hold_applies_to_tracked_vision_lead(monkeypatch): - planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) - - try: - monkeypatch.setattr(starpilot_planner_module.time, "monotonic", lambda: 100.0) - planner.model_length = 30.0 - planner.tracking_lead = True - planner.starpilot_following.t_follow = 1.45 - planner.lead_one = SimpleNamespace( - status=True, - dRel=46.0, - vLead=27.0, - aLeadK=0.0, - modelProb=0.98, - radar=False, - ) - - planner.update_lead_status(27.5) - assert planner.radarless_follow_hold_until > 100.0 - finally: - planner.shutdown() - - -def test_tracked_vision_lead_uses_exit_hysteresis_at_mid_speed(): - planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) - - try: - planner.model_length = 174.0 - planner.tracking_lead = True - planner.tracking_lead_filter.x = 1.0 - planner.lead_one = SimpleNamespace( - status=True, - dRel=44.5, - vLead=16.7, - yRel=-0.69, - aLeadK=0.0, - modelProb=0.99, - radar=False, - ) - - assert planner.update_lead_status(16.8) - finally: - planner.shutdown() - - -def test_untracked_vision_lead_still_uses_strict_entry_gate(): - planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) - - try: - planner.model_length = 174.0 - planner.lead_one = SimpleNamespace( - status=True, - dRel=44.5, - vLead=16.7, - yRel=-0.69, - aLeadK=0.0, - modelProb=0.99, - radar=False, - ) - - assert not planner.update_lead_status(16.8) - finally: - planner.shutdown() diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 78670af9f..510ce8b5c 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -34,7 +34,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False params=FakeParams(), params_memory=FakeParams({"NavInstructionState": nav_state or {}}), lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), - starpilot_cem=SimpleNamespace(stop_light_detected=red_light), + longitudinal_intent=SimpleNamespace(stop_detected=red_light), tracking_lead=False, driving_in_curve=False, model_length=60.0, @@ -432,9 +432,9 @@ def test_stop_then_turn_override_releases_after_stop_seen_window_expires(): sm["carState"].steeringAngleDeg = 30.0 toggles = make_toggles() - planner.starpilot_cem.stop_light_detected = True + planner.longitudinal_intent.stop_detected = True update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=7.0) - planner.starpilot_cem.stop_light_detected = False + planner.longitudinal_intent.stop_detected = False now = FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME + 0.5 for frame in range(12): @@ -521,7 +521,7 @@ def test_standstill_seeded_force_stop_hold_requires_clear_window_before_release( assert first == pytest.approx(0.0) assert vcruise.standstill_force_stop_hold - planner.starpilot_cem.stop_light_detected = False + planner.longitudinal_intent.stop_detected = False second = vcruise.update( controls_enabled=True, now=0.4, @@ -567,7 +567,7 @@ def test_standstill_seeded_force_stop_hold_accepts_datetime_now_without_crashing assert first == pytest.approx(0.0) assert vcruise.standstill_force_stop_hold - planner.starpilot_cem.stop_light_detected = False + planner.longitudinal_intent.stop_detected = False second = vcruise.update( controls_enabled=True, now=base + datetime.timedelta(seconds=0.4), diff --git a/selfdrive/controls/tests/test_unified_longitudinal_intent.py b/selfdrive/controls/tests/test_unified_longitudinal_intent.py new file mode 100644 index 000000000..77c57df64 --- /dev/null +++ b/selfdrive/controls/tests/test_unified_longitudinal_intent.py @@ -0,0 +1,159 @@ +from types import SimpleNamespace + +import pytest + +from openpilot.starpilot.common.experimental_state import CEStatus +from openpilot.starpilot.controls.lib.unified_longitudinal_intent import ( + MODEL_STOP_ENTER, + UnifiedLongitudinalIntent, +) + + +class FakeParamsMemory: + def __init__(self): + self.values = {} + + def put_int(self, key, value): + self.values[key] = value + + +def make_controller(): + planner = SimpleNamespace( + params_memory=FakeParamsMemory(), + driving_in_curve=False, + road_curvature_detected=False, + starpilot_vcruise=SimpleNamespace( + forcing_stop=False, + stop_sign_confirmed=False, + slc=SimpleNamespace(experimental_mode=False), + ), + ) + return planner, UnifiedLongitudinalIntent(planner) + + +def make_sm(*, v_ego=10.0, should_stop=False, model_distance=100.0, + end_speed=10.0, traffic=False, lead_one=None, lead_two=None): + model = SimpleNamespace( + action=SimpleNamespace(shouldStop=should_stop), + position=SimpleNamespace(x=[0.0, model_distance]), + velocity=SimpleNamespace(x=[v_ego, end_speed]), + ) + empty_lead = SimpleNamespace(status=False) + return { + "modelV2": model, + "carState": SimpleNamespace( + standstill=v_ego == 0.0, + leftBlinker=False, + rightBlinker=False, + steeringAngleDeg=0.0, + ), + "radarState": SimpleNamespace( + leadOne=lead_one or empty_lead, + leadTwo=lead_two or empty_lead, + ), + "starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic), + } + + +def toggles(model_first=False): + return SimpleNamespace(longitudinal_model_preference=model_first) + + +def settle(controller, sm, *, v_ego=10.0, frames=20): + for _ in range(frames): + controller.update(v_ego, sm, toggles()) + + +def test_open_road_has_no_stop_intent(): + _, controller = make_controller() + sm = make_sm() + + settle(controller, sm) + + assert not controller.stop_detected + assert controller.status_value == CEStatus["OFF"] + + +def test_model_stop_is_filtered_then_latched(): + _, controller = make_controller() + sm = make_sm(model_distance=5.0, end_speed=0.0) + + while controller.stop_filter.x < MODEL_STOP_ENTER: + controller.update(10.0, sm, toggles()) + + assert controller.stop_detected + assert controller.status_value == CEStatus["STOP_LIGHT"] + + +def test_stop_intent_releases_with_hysteresis(): + _, controller = make_controller() + stop_scene = make_sm(should_stop=True) + settle(controller, stop_scene) + assert controller.stop_detected + + clear_scene = make_sm() + controller.update(10.0, clear_scene, toggles()) + assert controller.stop_detected + settle(controller, clear_scene) + assert not controller.stop_detected + + +@pytest.mark.parametrize("hard_stop", ["forcing_stop", "stop_sign_confirmed"]) +def test_explicit_stop_sources_pin_intent(hard_stop): + planner, controller = make_controller() + setattr(planner.starpilot_vcruise, hard_stop, True) + + controller.update(5.0, make_sm(v_ego=5.0), toggles()) + + assert controller.stop_detected + assert controller.status_value == CEStatus["STOP_LIGHT"] + + +def test_committed_turn_vetoes_model_stop_false_positive(): + planner, controller = make_controller() + planner.driving_in_curve = True + sm = make_sm(v_ego=4.0, should_stop=True) + sm["carState"].leftBlinker = True + sm["carState"].steeringAngleDeg = 60.0 + + settle(controller, sm, v_ego=4.0) + + assert not controller.stop_detected + + +def test_traffic_mode_vetoes_model_stop_but_not_explicit_force_stop(): + planner, controller = make_controller() + sm = make_sm(should_stop=True, traffic=True) + settle(controller, sm) + assert not controller.stop_detected + + planner.starpilot_vcruise.forcing_stop = True + controller.update(10.0, sm, toggles()) + assert controller.stop_detected + + +def test_either_credible_slow_lead_slot_sets_diagnostic_status(): + _, controller = make_controller() + slow_lead = SimpleNamespace( + status=True, + dRel=25.0, + vLead=4.0, + modelProb=0.95, + radar=False, + ) + sm = make_sm(v_ego=10.0, lead_two=slow_lead) + + settle(controller, sm) + + assert not controller.stop_detected + assert controller.status_value == CEStatus["LEAD"] + + +def test_model_first_preference_is_status_only_not_a_separate_controller(): + planner, controller = make_controller() + + controller.update(10.0, make_sm(), toggles(model_first=True)) + + assert not controller.stop_detected + assert controller.status_value == CEStatus["USER_OVERRIDDEN"] + assert planner.params_memory.values["CEStatus"] == CEStatus["USER_OVERRIDDEN"] diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 626bbba04..2029595c8 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -736,10 +736,7 @@ class SelfdriveD: self.starpilot_events.add_from_msg(self.sm['starpilotPlan'].starpilotEvents) - if self.starpilot_toggles.conditional_experimental_mode or getattr(self.starpilot_toggles, "conditional_chill_mode", False): - self.experimental_mode = self.sm['starpilotPlan'].experimentalMode - else: - self.experimental_mode |= self.sm['starpilotPlan'].experimentalMode + self.experimental_mode = False if self.safe_mode else self.sm['starpilotPlan'].experimentalMode def data_sample(self): _car_state = messaging.recv_one(self.car_state_sock) @@ -885,10 +882,6 @@ class SelfdriveD: self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") - if self.safe_mode: - self.experimental_mode = False - elif not self.starpilot_toggles.conditional_experimental_mode: - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl self.personality = log.LongitudinalPersonality.relaxed if self.safe_mode else self.params.get("LongitudinalPersonality", return_default=True) time.sleep(0.1) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 2d1e8e5b2..127797063 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -25,6 +25,7 @@ class Maneuver: self.e2e = kwargs.get("e2e", False) self.personality = kwargs.get("personality", 0) self.force_decel = kwargs.get("force_decel", False) + self.lead_move_started_at = None self.duration = duration self.title = title @@ -68,7 +69,13 @@ class Maneuver: print("Crashed!!!!") valid = False - if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] < 1e-3: + if self.ensure_start and speed_lead > 0 and self.lead_move_started_at is None: + self.lead_move_started_at = plant.current_time + start_wait_expired = ( + self.lead_move_started_at is not None and + plant.current_time - self.lead_move_started_at > 0.5 + ) + if self.ensure_start and start_wait_expired and log['v_rel'] > 0 and log['acceleration'] < 1e-3: print('LongitudinalPlanner not starting!') valid = False diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 222df45ff..878d616dc 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -10,15 +10,11 @@ from types import SimpleNamespace from cereal import log import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN -from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import Ratekeeper, DT_MDL from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.lead_behavior import should_hold_tracked_vision_lead, should_track_lead from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU -from openpilot.starpilot.common.starpilot_variables import THRESHOLD class Plant: @@ -91,7 +87,6 @@ class Plant: self.e2e = e2e self.personality = personality self.force_decel = force_decel - self.tracking_lead_filter = FirstOrderFilter(0.0, 0.5, DT_MDL) self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate @@ -185,33 +180,8 @@ class Plant: car_state.carState.vCruise = float(v_cruise * 3.6) car_control.carControl.orientationNED = [0., float(pitch), 0.] - tracking_lead = bool(status) - if self.track_lead_with_gate: - tracking_candidate = should_track_lead( - status, - float(d_rel), - float(position.x[-1]) if len(position.x) else 0.0, - STOP_DISTANCE, - float(self.speed), - v_lead=float(v_lead), - radar=bool(self.only_radar), - ) - continuity_candidate = self.tracking_lead_filter.x >= THRESHOLD * 0.6 - if not tracking_candidate and continuity_candidate: - tracking_candidate = should_hold_tracked_vision_lead( - status, - float(d_rel), - float(position.x[-1]) if len(position.x) else 0.0, - STOP_DISTANCE, - float(self.speed), - model_prob=float(prob_lead), - y_rel=float(lead.yRel), - radar=bool(self.only_radar), - ) - self.tracking_lead_filter.update(tracking_candidate) - tracking_lead = self.tracking_lead_filter.x >= THRESHOLD - else: - self.tracking_lead_filter.update(float(status)) + # The planner must remain safe even if a legacy tracking diagnostic is false. + tracking_lead = bool(status and not self.track_lead_with_gate) # ******** get controlsState messages for plotting *** starpilot_plan = SimpleNamespace( diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 40a18a180..3dd83becd 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -50,7 +50,6 @@ from openpilot.starpilot.common.accel_profile import ( normalize_acceleration_profile, normalize_deceleration_profile, ) -from openpilot.starpilot.common.experimental_state import sync_persist_experimental_state, sync_persist_chill_state from openpilot.common.constants import CV @@ -94,8 +93,8 @@ class AdaptiveSpeedView(CardHubManagerView): def _build_cards(self): return [ { - "title": tr("Conditional Drive Mode"), - "desc": tr("Configure automated switching between Experimental and Chill Modes based on set conditions."), + "title": tr("Longitudinal Preference"), + "desc": tr("Choose whether set speed or the driving model leads on open roads. Lead and stop safety are identical."), "icon": "steering", "on_click": lambda: self._controller._navigate_to("ce"), }, @@ -138,7 +137,7 @@ class LongitudinalManagerView(CardHubManagerView): }, { "title": tr("Adaptive Speed Controls"), - "desc": tr("Configure Curve Speed Controller and Conditional Experimental Mode triggers."), + "desc": tr("Configure Curve Speed Controller and the unified longitudinal preference."), "icon": "display", "on_click": lambda: self._controller._navigate_to("adaptive_speed"), }, @@ -164,17 +163,15 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView): def __init__(self, controller: StarPilotLongitudinalLayout): super().__init__() - self._header_title = tr("Conditional Drive Mode") + self._header_title = tr("Longitudinal Preference") self._controller = controller - self._init_segmented_control() - self._init_adjustors() self._init_toggles() def _init_segmented_control(self): self._drive_mode_control = self._child( AetherSegmentedControl( - [tr("OFF"), tr("Experimental"), tr("Chill")], + [tr("Set-Speed First"), tr("Model First")], self._get_drive_mode_index, self._on_drive_mode_change, style=PANEL_STYLE, @@ -183,67 +180,24 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView): ) def _get_drive_mode_index(self): - if self._controller._params.get_bool("ConditionalExperimental"): - return 1 - elif self._controller._params.get_bool("ConditionalChill"): - return 2 - return 0 + return int(self._controller._params.get_int("LongitudinalModelPreference")) def _on_drive_mode_change(self, idx): - if idx == 0: - self._controller._params.put_bool("ConditionalExperimental", False) - self._controller._params.put_bool("ConditionalChill", False) - elif idx == 1: - self._controller._params.put_bool("ConditionalExperimental", True) - self._controller._params.put_bool("ConditionalChill", False) - elif idx == 2: - self._controller._params.put_bool("ConditionalExperimental", False) - self._controller._params.put_bool("ConditionalChill", True) - self._update_pagination() + self._controller._params.put_int("LongitudinalModelPreference", int(idx)) + self._controller._params_memory.put_int("LongitudinalModelPreferenceOverride", -1) def _init_toggles(self): - if PANEL_STYLE.toggle_row_mode: - self._toggle_grid = TileGrid(columns=1, padding=12, min_tile_height=TOGGLE_MIN_HEIGHT) - else: - self._toggle_grid = TileGrid(columns=2, padding=12, min_tile_height=130.0) + self._toggle_grid = TileGrid(columns=1, padding=12, min_tile_height=TOGGLE_MIN_HEIGHT) self._child(self._toggle_grid) self.register_page_grid(self._toggle_grid) - - cem_defs = [ - {"title": tr("Curves"), "subtitle": tr("Switch to Experimental Mode on open-road curves."), "get_state": lambda: self._controller._params.get_bool("CECurves"), "set_state": lambda v: self._controller._params.put_bool("CECurves", v)}, - {"title": tr("Curves w/ Lead"), "subtitle": tr("Switch on curves even when following a lead."), "get_state": lambda: self._controller._params.get_bool("CECurvesLead"), "set_state": lambda v: self._controller._params.put_bool("CECurvesLead", v), "is_enabled": lambda: self._controller._params.get_bool("CECurves"), "disabled_label": tr("Turn on Curves first")}, - {"title": tr("Stop Lights/Signs"), "subtitle": tr("Switch when openpilot detects a stop."), "get_state": lambda: self._controller._params.get_bool("CEStopLights"), "set_state": lambda v: self._controller._params.put_bool("CEStopLights", v)}, - {"title": tr("Lead Ahead"), "subtitle": tr("Switch when a slower/stopped vehicle is ahead."), "get_state": lambda: self._controller._params.get_bool("CELead"), "set_state": lambda v: self._controller._params.put_bool("CELead", v)}, - {"title": tr("Slower Lead"), "subtitle": tr("Switch specifically for slower leads."), "get_state": lambda: self._controller._params.get_bool("CESlowerLead"), "set_state": lambda v: self._controller._params.put_bool("CESlowerLead", v), "is_enabled": lambda: self._controller._params.get_bool("CELead"), "disabled_label": tr("Turn on Lead first")}, - {"title": tr("Stopped Lead"), "subtitle": tr("Switch specifically for stopped leads."), "get_state": lambda: self._controller._params.get_bool("CEStoppedLead"), "set_state": lambda v: self._controller._params.put_bool("CEStoppedLead", v), "is_enabled": lambda: self._controller._params.get_bool("CELead"), "disabled_label": tr("Turn on Lead first")}, - {"title": tr("Signal Lane Detect"), "subtitle": tr("Don't trigger on turn signal if lines are clear."), "get_state": lambda: self._controller._params.get_bool("CESignalLaneDetection"), "set_state": lambda v: self._controller._params.put_bool("CESignalLaneDetection", v), "is_enabled": lambda: self._controller._params.get_int("CESignalSpeed") > 0, "disabled_label": tr("Needs Turn Signal speed > 0")}, - {"title": tr("Status Widget"), "subtitle": tr("Show condition trigger on the drive screen."), "get_state": lambda: self._controller._params.get_bool("ShowCEMStatus"), "set_state": lambda v: self._controller._params.put_bool("ShowCEMStatus", v)}, - {"title": tr("Persist Exp State"), "subtitle": tr("Keep manual Experimental override through reboots."), "get_state": lambda: self._controller._params.get_bool("PersistExperimentalState"), "set_state": self._controller._set_persist_experimental_state}, - ] - - ccm_defs = [ - {"title": tr("Stable Lead Ahead"), "subtitle": tr("Switch to Chill Mode when following a steady lead."), "get_state": lambda: self._controller._params.get_bool("CCMLead"), "set_state": lambda v: self._controller._params.put_bool("CCMLead", v)}, - {"title": tr("Launch Assist"), "subtitle": tr("Temporarily switch to Chill from a stop."), "get_state": lambda: self._controller._params.get_bool("CCMLaunchAssist"), "set_state": lambda v: self._controller._params.put_bool("CCMLaunchAssist", v)}, - {"title": tr("Status Widget"), "subtitle": tr("Show condition trigger on the drive screen."), "get_state": lambda: self._controller._params.get_bool("ShowCCMStatus"), "set_state": lambda v: self._controller._params.put_bool("ShowCCMStatus", v)}, - {"title": tr("Persist Chill State"), "subtitle": tr("Keep manual Chill override through reboots."), "get_state": lambda: self._controller._params.get_bool("PersistChillState"), "set_state": self._controller._set_persist_chill_state}, - ] - - self._cem_toggle_defs = cem_defs - self._ccm_toggle_defs = ccm_defs - - self._update_pagination() - - def _update_pagination(self): - mode = self._get_drive_mode_index() - page_size = self._compute_page_size(TOGGLE_ROW_HEIGHT) - if mode == 1: - pages = [self._cem_toggle_defs[i:i+page_size] for i in range(0, len(self._cem_toggle_defs), page_size)] - self._set_toggle_pages(pages) - elif mode == 2: - pages = [self._ccm_toggle_defs[i:i+page_size] for i in range(0, len(self._ccm_toggle_defs), page_size)] - self._set_toggle_pages(pages) - else: - self._set_toggle_pages([]) + self._set_toggle_pages([[ + { + "title": tr("Status Widget"), + "subtitle": tr("Show the active longitudinal constraint on the drive screen."), + "get_state": lambda: self._controller._params.get_bool("ShowCEMStatus"), + "set_state": lambda value: self._controller._params.put_bool("ShowCEMStatus", value), + }, + ]]) def _make_toggle_tile(self, info: dict) -> ToggleTile: cls = RowToggleTile if PANEL_STYLE.toggle_row_mode else ToggleTile @@ -261,189 +215,28 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView): return cls(**kwargs) - def _init_adjustors(self): - speed_unit = self._controller._speed_unit() - is_metric = self._controller._is_metric() - max_speed = 150.0 if is_metric else 100.0 - - specs = { - "CESpeed": {"title": tr("Below Speed"), "subtitle": tr("Switch to Experimental Mode below this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 20, 35, 55, 75], "labels": {}, "get": lambda: float(self._controller._params.get_int("CESpeed"))}, - "CESpeedLead": {"title": tr("Speed w/ Lead"), "subtitle": tr("Switch below this speed when following a lead."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 20, 35, 55, 75], "labels": {}, "get": lambda: float(self._controller._params.get_int("CESpeedLead"))}, - "CESignalSpeed": {"title": tr("Turn Signal Below"), "subtitle": tr("Switch when turn signal is on below this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 20, 35, 55, 75], "labels": {0.0: tr("Off")}, "get": lambda: float(self._controller._params.get_int("CESignalSpeed"))}, - "CEModelStopTime": {"title": tr("Predicted Stop In"), "subtitle": tr("Switch when openpilot predicts a stop within time."), "min": 0, "max": 10.0, "step": 1.0, "unit": "s", "presets": [0, 3, 5, 7, 10], "labels": {0.0: tr("Off")}, "get": lambda: float(self._controller._params.get_int("CEModelStopTime"))}, - "CCMSpeed": {"title": tr("Above Speed"), "subtitle": tr("Switch to Chill Mode on open roads above this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 35, 55, 65, 80], "labels": {}, "get": lambda: float(self._controller._params.get_int("CCMSpeed"))}, - "CCMSpeedLead": {"title": tr("Speed w/ Lead"), "subtitle": tr("Switch when following a stable lead above this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 35, 55, 65, 80], "labels": {}, "get": lambda: float(self._controller._params.get_int("CCMSpeedLead"))}, - "CCMSetSpeedMargin": {"title": tr("Set Speed Margin"), "subtitle": tr("How far below set speed before Chill engages."), "min": 0, "max": 30.0 if is_metric else 15.0, "step": 1.0, "unit": speed_unit, "presets": [0, 5, 10, 15], "labels": {}, "get": lambda: float(self._controller._params.get_int("CCMSetSpeedMargin"))}, - } - - self._cem_keys = ["CESpeed", "CESpeedLead", "CESignalSpeed", "CEModelStopTime"] - self._ccm_keys = ["CCMSpeed", "CCMSpeedLead", "CCMSetSpeedMargin"] - - for key, spec in specs.items(): - adjustor = self._child(AetherAdjustorRow( - spec["title"], spec["subtitle"], spec["min"], spec["max"], spec["step"], - get_value=spec["get"], - on_change=lambda _v: None, - on_commit=None, - unit=spec["unit"], - labels=spec["labels"], - presets=spec["presets"], - is_active=lambda: False, - set_active=lambda active, k=key: self._show_slider(k) if active else None, - style=PANEL_STYLE, color=PANEL_STYLE.accent - )) - adjustor.set_touch_valid_callback(lambda: self._scroll_panel.is_touch_valid()) - self._adjustor_rows[key] = adjustor - - def _show_slider(self, key: str): - is_metric = self._controller._is_metric() - speed_unit = self._controller._speed_unit() - max_speed = 150.0 if is_metric else 100.0 - - specs = { - "CESpeed": {"title": tr("Below Speed"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 20, 35, 55, 75]}, - "CESpeedLead": {"title": tr("Speed w/ Lead"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 20, 35, 55, 75]}, - "CESignalSpeed": {"title": tr("Turn Signal Below"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {0.0: tr("Off")}, "presets": [0, 20, 35, 55, 75]}, - "CEModelStopTime": {"title": tr("Predicted Stop In"), "min": 0, "max": 10.0, "unit": "s", "labels": {0.0: tr("Off")}, "presets": [0, 3, 5, 7, 10]}, - "CCMSpeed": {"title": tr("Above Speed"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 35, 55, 65, 80]}, - "CCMSpeedLead": {"title": tr("Speed w/ Lead"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 35, 55, 65, 80]}, - "CCMSetSpeedMargin": {"title": tr("Set Speed Margin"), "min": 0, "max": 30.0 if is_metric else 15.0, "unit": speed_unit, "labels": {}, "presets": [0, 5, 10, 15]}, - } - - spec = specs[key] - original_val = float(self._controller._params.get_int(key)) - - def on_close(res, val): - if res == DialogResult.CONFIRM: - self._controller._params.put_int(key, int(val)) - - gui_app.push_widget(AetherSliderDialog( - title=spec["title"], - min_val=float(spec["min"]), max_val=float(spec["max"]), step=1.0, - current_val=original_val, - on_close=on_close, presets=[float(p) for p in spec["presets"]], - unit=spec["unit"], labels=spec["labels"], color=PANEL_STYLE.accent - )) - def _draw_header(self, rect: rl.Rectangle): pass def _measure_content_height(self, content_width: float) -> float: - mode = self._get_drive_mode_index() - if mode == 0: - return self.TAB_HEIGHT + self.TAB_BOTTOM_GAP - - keys = self._cem_keys if mode == 1 else self._ccm_keys - grid = self._toggle_grid - - col_width = (content_width - SECTION_GAP) / 2 if self._uses_two_columns(content_width) else content_width - - for key in keys: - self._adjustor_rows[key].custom_row_height = None - - default_adjustor_h = float(AETHER_LIST_METRICS.adjustor_row_height) - left_h = len(keys) * default_adjustor_h + 16.0 - - num_tiles = 4 if self._has_pagination else len(grid.tiles) - if PANEL_STYLE.toggle_row_mode: - rows = num_tiles - tile_h = TOGGLE_ROW_HEIGHT - else: - rows = (num_tiles + 1) // 2 if self._uses_two_columns(content_width) else num_tiles - tile_h = grid.min_tile_height - - pagination_space = 32.0 if self._has_pagination else 0.0 - tiles_h = rows * tile_h + (rows - 1) * grid.gap + grid.gap * 2 + pagination_space - - right_h = tiles_h - - if self._uses_two_columns(content_width): - max_natural_h = max(left_h, right_h) - section_overhead = SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP - - if self._scroll_rect: - available_h = self._scroll_rect.height - section_overhead - self.TAB_HEIGHT - self.TAB_BOTTOM_GAP - 6.0 - else: - available_h = max_natural_h - - max_container_h = available_h - - left_row_h = max(95.0, (max_container_h - 16.0) / max(1, len(keys))) - for key in keys: - self._adjustor_rows[key].custom_row_height = left_row_h - - self._left_container_h = max_container_h - self._tiles_container_h = max_container_h - - return self._compute_two_column_height(section_overhead + max_container_h) + self.TAB_HEIGHT + self.TAB_BOTTOM_GAP - else: - self._left_container_h = left_h - self._tiles_container_h = right_h - total = left_h + SECTION_GAP + right_h + SECTION_HEADER_HEIGHT * 2 + SECTION_HEADER_GAP * 2 - return total + self.TAB_HEIGHT + self.TAB_BOTTOM_GAP + self._tiles_container_h = TOGGLE_ROW_HEIGHT + 24.0 + return self.TAB_HEIGHT + self.TAB_BOTTOM_GAP + SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP + self._tiles_container_h def _draw_scroll_content(self, rect: rl.Rectangle, content_width: float): y = rect.y + self._scroll_offset - - header_w = content_width - bar_rect = rl.Rectangle(rect.x, y, header_w, self.TAB_HEIGHT) + bar_rect = rl.Rectangle(rect.x, y, content_width, self.TAB_HEIGHT) draw_list_group_shell(bar_rect, style=PANEL_STYLE) self._drive_mode_control.render(bar_rect) - y += self.TAB_HEIGHT + self.TAB_BOTTOM_GAP - mode = self._get_drive_mode_index() - if mode == 0: - return - - keys = self._cem_keys if mode == 1 else self._ccm_keys - grid = self._toggle_grid - - col_width = (content_width - SECTION_GAP) / 2 if self._uses_two_columns(content_width) else content_width - - draw_section_header(rl.Rectangle(rect.x, y, col_width, SECTION_HEADER_HEIGHT), tr("Values"), style=PANEL_STYLE) - if self._uses_two_columns(content_width): - draw_section_header(rl.Rectangle(rect.x + col_width + SECTION_GAP, y, col_width, SECTION_HEADER_HEIGHT), tr("Triggers"), style=PANEL_STYLE) - + draw_section_header(rl.Rectangle(rect.x, y, content_width, SECTION_HEADER_HEIGHT), tr("Diagnostics"), style=PANEL_STYLE) y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP - - self._draw_adjustors(y, rect.x, col_width, keys) - - tg_columns = 1 if PANEL_STYLE.toggle_row_mode else 2 - if self._uses_two_columns(content_width): - self._draw_two_column_tile_grid( - grid, rect.x + col_width + SECTION_GAP, y, col_width, - self._tiles_container_h, title=None, style=PANEL_STYLE, - columns=tg_columns) - else: - y += self._left_container_h + SECTION_GAP - draw_section_header(rl.Rectangle(rect.x, y, col_width, SECTION_HEADER_HEIGHT), tr("Triggers"), style=PANEL_STYLE) - y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP - self._draw_two_column_tile_grid( - grid, rect.x, y, col_width, - self._tiles_container_h, title=None, style=PANEL_STYLE, - columns=tg_columns) - - def _draw_adjustors(self, y: float, x: float, width: float, keys: list[str]): - draw_list_group_shell(rl.Rectangle(x, y, width, self._left_container_h), style=PANEL_STYLE) - current_y = y + 4 - for index, key in enumerate(keys): - adjustor = self._adjustor_rows[key] - row_h = adjustor.measure_height(width) - row_rect = rl.Rectangle(x, current_y, width, row_h) - adjustor.set_is_last(index == len(keys) - 1) - adjustor.set_parent_rect(self._scroll_rect) - adjustor.render(row_rect) - current_y += row_h + self._draw_two_column_tile_grid( + self._toggle_grid, rect.x, y, content_width, self._tiles_container_h, + title=None, style=PANEL_STYLE, columns=1, + ) def _get_active_elements(self): - mode = self._get_drive_mode_index() - elems = [self._drive_mode_control] - if mode == 1: - elems += [self._adjustor_rows[k] for k in self._cem_keys] - elif mode == 2: - elems += [self._adjustor_rows[k] for k in self._ccm_keys] - elems.append(self._toggle_grid) - return elems + return [self._drive_mode_control, self._toggle_grid] # ═══════════════════════════════════════════════════════════════ @@ -479,9 +272,6 @@ class StarPilotLongitudinalLayout(_SettingsPage): def _build_view(self): ol = lambda: starpilot_state.car_state.hasOpenpilotLongitudinal - ce_on = lambda: self._params.get_bool("ConditionalExperimental") - cc_on = lambda: self._params.get_bool("ConditionalChill") - ce_lead = lambda: ce_on() and self._params.get_bool("CELead") csc_on = lambda: self._params.get_bool("CurveSpeedController") confirmation_on = lambda: self._params.get_bool("SLCConfirmation") @@ -659,7 +449,7 @@ class StarPilotLongitudinalLayout(_SettingsPage): on_click=lambda k=key: self._show_slider(k, *self._speed_range(), unit=self._speed_unit()), )) - # ── 4. Adaptive Speed Controls Rows (CES + CSC + CCM) ── + # ── 4. Adaptive Speed Controls Rows ── self._curve_speed_controller_rows = [ SettingRow("CalibratedLatAccel", "value", tr_noop("Calibrated Lateral Accel"), subtitle=tr_noop("The learned lateral acceleration from collected driving data. Higher values allow faster cornering."), @@ -945,37 +735,19 @@ class StarPilotLongitudinalLayout(_SettingsPage): if state: self._params.put_bool("EVTuning", False) - def _set_persist_experimental_state(self, state: bool): - sync_persist_experimental_state(self._params, self._params_memory, state) - - def _set_persist_chill_state(self, state: bool): - sync_persist_chill_state(self._params, self._params_memory, state) - def _get_conditional_mode_label(self) -> str: - if self._params.get_bool("ConditionalExperimental"): - return tr("Conditional Experimental") - elif self._params.get_bool("ConditionalChill"): - return tr("Conditional Chill") - else: - return tr("OFF") + return tr("Model First") if self._params.get_int("LongitudinalModelPreference") else tr("Set-Speed First") def _show_conditional_mode_selector(self): - options = ["OFF", "Conditional Experimental", "Conditional Chill"] + options = ["Set-Speed First", "Model First"] current = self._get_conditional_mode_label() def on_select(res): if res == DialogResult.CONFIRM and dialog.selection: - if dialog.selection == "OFF": - self._params.put_bool("ConditionalExperimental", False) - self._params.put_bool("ConditionalChill", False) - elif dialog.selection == "Conditional Experimental": - self._params.put_bool("ConditionalExperimental", True) - self._params.put_bool("ConditionalChill", False) - elif dialog.selection == "Conditional Chill": - self._params.put_bool("ConditionalExperimental", False) - self._params.put_bool("ConditionalChill", True) + self._params.put_int("LongitudinalModelPreference", int(dialog.selection == "Model First")) + self._params_memory.put_int("LongitudinalModelPreferenceOverride", -1) - dialog = MultiOptionDialog(tr("Conditional Drive Mode"), options, current, callback=on_select) + dialog = MultiOptionDialog(tr("Longitudinal Preference"), options, current, callback=on_select) gui_app.push_widget(dialog) def _reset_curve_data(self): @@ -1003,8 +775,6 @@ class StarPilotLongitudinalLayout(_SettingsPage): _SPEED_RESCALE_KEYS = ( "Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7", "CustomCruise", "CustomCruiseLong", "SetSpeedOffset", - "CESpeed", "CESpeedLead", "CESignalSpeed", - "CCMSpeed", "CCMSpeedLead", "CCMSetSpeedMargin", ) # Distance-typed int params stored in the current unit (ft or m); rescaled diff --git a/selfdrive/ui/lib/mode_banner.py b/selfdrive/ui/lib/mode_banner.py index 8bee4cbc5..b822de805 100644 --- a/selfdrive/ui/lib/mode_banner.py +++ b/selfdrive/ui/lib/mode_banner.py @@ -8,17 +8,11 @@ from openpilot.starpilot.common.experimental_state import requested_experimental class ModeBannerVariant(StrEnum): CHILL = "chill" EXPERIMENTAL = "experimental" - CONDITIONAL_EXPERIMENTAL = "conditional_experimental" - CONDITIONAL_CHILL = "conditional_chill" def get_mode_banner_variant(params, params_memory=None) -> ModeBannerVariant: if params.get_bool("SafeMode"): return ModeBannerVariant.CHILL - if params.get_bool("ConditionalExperimental"): - return ModeBannerVariant.CONDITIONAL_EXPERIMENTAL - if params.get_bool("ConditionalChill"): - return ModeBannerVariant.CONDITIONAL_CHILL if requested_experimental_mode(params, params_memory): return ModeBannerVariant.EXPERIMENTAL return ModeBannerVariant.CHILL @@ -38,29 +32,11 @@ def _lerp_color(start: rl.Color, end: rl.Color, progress: float, alpha: int) -> ) -def _conditional_colors(variant: ModeBannerVariant, alpha: int) -> tuple[rl.Color, rl.Color, rl.Color, rl.Color]: - blue = _color(35, 149, 255, alpha) - mint = _color(20, 255, 171, alpha) - orange = _color(255, 138, 22, alpha) - red = _color(219, 56, 34, alpha) - if variant == ModeBannerVariant.CONDITIONAL_EXPERIMENTAL: - return blue, mint, orange, red - return orange, red, blue, mint - - def mode_banner_color(variant: ModeBannerVariant, progress: float, alpha: int = 255) -> rl.Color: progress = max(0.0, min(1.0, progress)) if variant == ModeBannerVariant.CHILL: return _lerp_color(_color(20, 255, 171, alpha), _color(35, 149, 255, alpha), progress, alpha) - if variant == ModeBannerVariant.EXPERIMENTAL: - return _lerp_color(_color(255, 155, 63, alpha), _color(219, 56, 34, alpha), progress, alpha) - - dominant_start, dominant_end, target_start, target_end = _conditional_colors(variant, alpha) - if progress <= 0.58: - return _lerp_color(dominant_start, dominant_end, progress / 0.58, alpha) - if progress <= 0.80: - return _lerp_color(dominant_end, target_start, (progress - 0.58) / 0.22, alpha) - return _lerp_color(target_start, target_end, ((progress - 0.80) / 0.20) * 0.67, alpha) + return _lerp_color(_color(255, 155, 63, alpha), _color(219, 56, 34, alpha), progress, alpha) def mode_atom_color(variant: ModeBannerVariant, progress: float, alpha: int = 255) -> rl.Color: @@ -71,25 +47,7 @@ def mode_atom_color(variant: ModeBannerVariant, progress: float, alpha: int = 25 def draw_mode_banner_gradient(rect: rl.Rectangle, variant: ModeBannerVariant, alpha: int = 255) -> None: - if variant in (ModeBannerVariant.CHILL, ModeBannerVariant.EXPERIMENTAL): - rl.draw_rectangle_gradient_h( - int(rect.x), int(rect.y), int(rect.width), int(rect.height), - mode_banner_color(variant, 0.0, alpha), mode_banner_color(variant, 1.0, alpha), - ) - return - - transition_start = int(rect.x + rect.width * 0.58) - transition_end = int(rect.x + rect.width * 0.80) - right = int(rect.x + rect.width) rl.draw_rectangle_gradient_h( - int(rect.x), int(rect.y), transition_start - int(rect.x), int(rect.height), - mode_banner_color(variant, 0.0, alpha), mode_banner_color(variant, 0.58, alpha), - ) - rl.draw_rectangle_gradient_h( - transition_start, int(rect.y), transition_end - transition_start, int(rect.height), - mode_banner_color(variant, 0.58, alpha), mode_banner_color(variant, 0.80, alpha), - ) - rl.draw_rectangle_gradient_h( - transition_end, int(rect.y), right - transition_end, int(rect.height), - mode_banner_color(variant, 0.80, alpha), mode_banner_color(variant, 1.0, alpha), + int(rect.x), int(rect.y), int(rect.width), int(rect.height), + mode_banner_color(variant, 0.0, alpha), mode_banner_color(variant, 1.0, alpha), ) diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index 98ac23486..37e9b813a 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -64,33 +64,21 @@ def get_screen_edge_color(state: UIState): def get_experimental_mode_banner_text(state: UIState): - conditional_enabled = state.params.get_bool("ConditionalExperimental") - - # With CEM enabled, only surface banner text for explicit manual override states. - # Automatic CEM transitions should only be reflected by path/border coloring. - if conditional_enabled: - if state.conditional_status in CEM_MANUAL_OVERRIDE_STATUSES: - return "OVERRIDDEN" - return None - if state.sm["selfdriveState"].experimentalMode: - return "EXPERIMENTAL" - return "CHILL" + return "MODEL FIRST" + return "SET-SPEED FIRST" def get_mode_transition_banner_text(state: UIState): enabled = state.sm["selfdriveState"].enabled lateral_active = enabled or state.always_on_lateral_active - conditional_enabled = state.params.get_bool("ConditionalExperimental") if state.status == UIStatus.OVERRIDE: return "OVERRIDE" if state.switchback_mode_enabled and lateral_active: return "SWITCHBACK" - if conditional_enabled and enabled and state.conditional_status in CEM_MANUAL_OVERRIDE_STATUSES: - return "OVERRIDDEN" if enabled and state.sm["selfdriveState"].experimentalMode: - return "EXPERIMENTAL" + return "MODEL FIRST" if enabled: - return "CHILL" + return "SET-SPEED FIRST" return None diff --git a/selfdrive/ui/onroad/exp_button.py b/selfdrive/ui/onroad/exp_button.py index d2476c2b3..08581f552 100644 --- a/selfdrive/ui/onroad/exp_button.py +++ b/selfdrive/ui/onroad/exp_button.py @@ -6,9 +6,8 @@ from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.widgets import Widget from openpilot.common.filter_simple import FirstOrderFilter from openpilot.starpilot.common.experimental_state import ( - CEStatus, - next_manual_ce_status, - sync_manual_ce_state, + requested_experimental_mode, + toggle_longitudinal_model_preference, ) @@ -35,7 +34,6 @@ class ExpButton(Widget): "disengaged": rl.Color(0, 0, 0, 166), "switchback": rl.Color(0x8b, 0x6c, 0xc5, 255), "aol": rl.Color(0x0a, 0xba, 0xb5, 255), - "cem_disabled": rl.Color(0xff, 0xff, 0x00, 255), "experimental": rl.Color(0xda, 0x6f, 0x25, 255), "traffic": rl.Color(0xc9, 0x22, 0x31, 255), } @@ -50,7 +48,7 @@ class ExpButton(Widget): def _update_state(self) -> None: selfdrive_state = ui_state.sm["selfdriveState"] - self._experimental_mode = selfdrive_state.experimentalMode + self._experimental_mode = requested_experimental_mode(self._params, ui_state.params_memory) self._engageable = selfdrive_state.engageable or selfdrive_state.enabled or ui_state.always_on_lateral_active # Smooth steering angle for rotating wheel @@ -65,8 +63,6 @@ class ExpButton(Widget): self._bg_color = self._bg_colors["switchback"] elif ui_state.always_on_lateral_active: self._bg_color = self._bg_colors["aol"] - elif ui_state.conditional_status == 1: - self._bg_color = self._bg_colors["cem_disabled"] elif self._held_or_actual_mode(): self._bg_color = self._bg_colors["experimental"] elif ui_state.traffic_mode_enabled: @@ -77,20 +73,9 @@ class ExpButton(Widget): def _handle_mouse_release(self, _): super()._handle_mouse_release(_) if self._is_toggle_allowed(): - if self._params.get_bool("ConditionalExperimental"): - current_status = ui_state.params_memory.get_int("CEStatus", default=CEStatus["OFF"]) - override_value = next_manual_ce_status(current_status, self._experimental_mode) - ui_state.params_memory.put_int("CEStatus", override_value) - sync_manual_ce_state(self._params, override_value) - self._held_mode = None - self._hold_end_time = None - else: - new_mode = not self._experimental_mode - self._params.put_bool("ExperimentalMode", new_mode) - - # Hold new state temporarily - self._held_mode = new_mode - self._hold_end_time = time.monotonic() + self._hold_duration + new_mode = toggle_longitudinal_model_preference(self._params, ui_state.params_memory) + self._held_mode = new_mode + self._hold_end_time = time.monotonic() + self._hold_duration def _render(self, rect: rl.Rectangle) -> None: center_x = int(self._rect.x + self._rect.width // 2) diff --git a/selfdrive/ui/tests/test_mode_banner.py b/selfdrive/ui/tests/test_mode_banner.py index c2046eaa8..c932c5581 100644 --- a/selfdrive/ui/tests/test_mode_banner.py +++ b/selfdrive/ui/tests/test_mode_banner.py @@ -17,26 +17,10 @@ def _rgb(color): return color.r, color.g, color.b -def test_mode_banner_variant_tracks_fixed_and_conditional_modes(): +def test_mode_banner_variant_tracks_longitudinal_preference(): assert get_mode_banner_variant(FakeParams()) == ModeBannerVariant.CHILL - assert get_mode_banner_variant(FakeParams({"ExperimentalMode": True})) == ModeBannerVariant.EXPERIMENTAL - assert get_mode_banner_variant(FakeParams({"ConditionalExperimental": True})) == ModeBannerVariant.CONDITIONAL_EXPERIMENTAL - assert get_mode_banner_variant(FakeParams({"ConditionalChill": True})) == ModeBannerVariant.CONDITIONAL_CHILL - assert get_mode_banner_variant(FakeParams({"SafeMode": True, "ConditionalExperimental": True})) == ModeBannerVariant.CHILL - - -def test_conditional_gradients_keep_full_major_and_partial_minor_colors(): - conditional_experimental = ModeBannerVariant.CONDITIONAL_EXPERIMENTAL - assert _rgb(mode_banner_color(conditional_experimental, 0.0)) == (35, 149, 255) - assert _rgb(mode_banner_color(conditional_experimental, 0.58)) == (20, 255, 171) - assert _rgb(mode_banner_color(conditional_experimental, 0.80)) == (255, 138, 22) - assert _rgb(mode_banner_color(conditional_experimental, 1.0)) == (231, 83, 30) - - conditional_chill = ModeBannerVariant.CONDITIONAL_CHILL - assert _rgb(mode_banner_color(conditional_chill, 0.0)) == (255, 138, 22) - assert _rgb(mode_banner_color(conditional_chill, 0.58)) == (219, 56, 34) - assert _rgb(mode_banner_color(conditional_chill, 0.80)) == (35, 149, 255) - assert _rgb(mode_banner_color(conditional_chill, 1.0)) == (25, 220, 199) + assert get_mode_banner_variant(FakeParams(ints={"LongitudinalModelPreference": 1})) == ModeBannerVariant.EXPERIMENTAL + assert get_mode_banner_variant(FakeParams({"SafeMode": True}, {"LongitudinalModelPreference": 1})) == ModeBannerVariant.CHILL def test_atom_gradients_use_compact_icon_directions(): @@ -44,7 +28,3 @@ def test_atom_gradients_use_compact_icon_directions(): assert _rgb(mode_atom_color(ModeBannerVariant.CHILL, 1.0)) == (20, 255, 171) assert _rgb(mode_atom_color(ModeBannerVariant.EXPERIMENTAL, 0.0)) == (255, 155, 63) assert _rgb(mode_atom_color(ModeBannerVariant.EXPERIMENTAL, 1.0)) == (219, 56, 34) - - for variant in (ModeBannerVariant.CONDITIONAL_EXPERIMENTAL, ModeBannerVariant.CONDITIONAL_CHILL): - assert _rgb(mode_atom_color(variant, 0.0)) == _rgb(mode_banner_color(variant, 0.0)) - assert _rgb(mode_atom_color(variant, 1.0)) == _rgb(mode_banner_color(variant, 1.0)) diff --git a/selfdrive/ui/widgets/exp_mode_button.py b/selfdrive/ui/widgets/exp_mode_button.py index b1fd40f14..6715740dc 100644 --- a/selfdrive/ui/widgets/exp_mode_button.py +++ b/selfdrive/ui/widgets/exp_mode_button.py @@ -40,12 +40,7 @@ class ExperimentalModeButton(Widget): rl.draw_line_ex(rl.Vector2(line_x, rect.y), rl.Vector2(line_x, rect.y + rect.height), 3, separator_color) # Draw text label (left aligned) - if self.mode_variant == ModeBannerVariant.CONDITIONAL_EXPERIMENTAL: - text = tr("CONDITIONAL EXPERIMENTAL") - elif self.mode_variant == ModeBannerVariant.CONDITIONAL_CHILL: - text = tr("CONDITIONAL CHILL") - else: - text = tr("EXPERIMENTAL MODE ON") if self.experimental_mode else tr("CHILL MODE ON") + text = tr("MODEL FIRST") if self.experimental_mode else tr("SET-SPEED FIRST") text_x = rect.x + self.horizontal_padding font = gui_app.font(FontWeight.NORMAL) diff --git a/starpilot/common/experimental_state.py b/starpilot/common/experimental_state.py index df7b356ac..4d1e708a6 100644 --- a/starpilot/common/experimental_state.py +++ b/starpilot/common/experimental_state.py @@ -1,15 +1,13 @@ #!/usr/bin/env python3 -from __future__ import annotations - from openpilot.common.params import Params -PERSIST_EXPERIMENTAL_STATE_PARAM = "PersistExperimentalState" -PERSISTED_CE_STATUS_PARAM = "PersistedCEStatus" -CE_STATUS_PARAM = "CEStatus" -PERSIST_CHILL_STATE_PARAM = "PersistChillState" -PERSISTED_CC_STATUS_PARAM = "PersistedCCStatus" -CC_STATUS_PARAM = "CCStatus" +CE_STATUS_PARAM = "CEStatus" +CC_STATUS_PARAM = "CCStatus" +LONGITUDINAL_PREFERENCE_PARAM = "LongitudinalModelPreference" +LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM = "LongitudinalModelPreferenceOverride" + +# Status values stay schema-compatible with the existing onroad widgets. CEStatus = { "OFF": 0, "USER_DISABLED": 1, @@ -22,6 +20,7 @@ CEStatus = { "STOP_LIGHT": 8, } +# Kept only so older/mici status renderers can safely read a stale CCStatus. CCStatus = { "OFF": 0, "USER_EXPERIMENTAL": 1, @@ -30,161 +29,17 @@ CCStatus = { "SPEED": 6, } -MANUAL_CE_STATUSES = { - CEStatus["USER_DISABLED"], - CEStatus["USER_OVERRIDDEN"], -} - -MANUAL_CC_STATUSES = { - CCStatus["USER_EXPERIMENTAL"], - CCStatus["USER_CHILL"], -} - - -def is_manual_ce_status(status: int) -> bool: - return int(status) in MANUAL_CE_STATUSES - - -def is_manual_cc_status(status: int) -> bool: - return int(status) in MANUAL_CC_STATUSES - - -def normalize_persisted_ce_status(status: int) -> int: - status = int(status) - return status if status in MANUAL_CE_STATUSES else CEStatus["OFF"] - - -def normalize_persisted_cc_status(status: int) -> int: - status = int(status) - return status if status in MANUAL_CC_STATUSES else CCStatus["OFF"] - - -def get_persisted_ce_status(params: Params) -> int: - return normalize_persisted_ce_status(params.get_int(PERSISTED_CE_STATUS_PARAM, default=CEStatus["OFF"])) - - -def get_persisted_cc_status(params: Params) -> int: - return normalize_persisted_cc_status(params.get_int(PERSISTED_CC_STATUS_PARAM, default=CCStatus["OFF"])) - - -def set_persisted_ce_status(params: Params, status: int) -> int: - normalized = normalize_persisted_ce_status(status) - params.put_int(PERSISTED_CE_STATUS_PARAM, normalized) - return normalized - - -def set_persisted_cc_status(params: Params, status: int) -> int: - normalized = normalize_persisted_cc_status(status) - params.put_int(PERSISTED_CC_STATUS_PARAM, normalized) - return normalized - - -def clear_persisted_ce_status(params: Params) -> None: - params.put_int(PERSISTED_CE_STATUS_PARAM, CEStatus["OFF"]) - - -def clear_persisted_cc_status(params: Params) -> None: - params.put_int(PERSISTED_CC_STATUS_PARAM, CCStatus["OFF"]) - - -def sync_persist_experimental_state(params: Params, params_memory: Params | None, enabled: bool) -> None: - params.put_bool(PERSIST_EXPERIMENTAL_STATE_PARAM, enabled) - if enabled: - current_status = params_memory.get_int(CE_STATUS_PARAM, default=CEStatus["OFF"]) if params_memory is not None else CEStatus["OFF"] - set_persisted_ce_status(params, current_status) - else: - clear_persisted_ce_status(params) - - -def sync_persist_chill_state(params: Params, params_memory: Params | None, enabled: bool) -> None: - params.put_bool(PERSIST_CHILL_STATE_PARAM, enabled) - if enabled: - current_status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"]) if params_memory is not None else CCStatus["OFF"] - set_persisted_cc_status(params, current_status) - else: - clear_persisted_cc_status(params) - - -def sync_manual_ce_state(params: Params, status: int) -> int: - return set_persisted_ce_status(params, status) if params.get_bool(PERSIST_EXPERIMENTAL_STATE_PARAM) else clear_and_return_off(params) - - -def sync_manual_cc_state(params: Params, status: int) -> int: - return set_persisted_cc_status(params, status) if params.get_bool(PERSIST_CHILL_STATE_PARAM) else clear_cc_and_return_off(params) - - -def clear_and_return_off(params: Params) -> int: - clear_persisted_ce_status(params) - return CEStatus["OFF"] - - -def clear_cc_and_return_off(params: Params) -> int: - clear_persisted_cc_status(params) - return CCStatus["OFF"] - - -def next_manual_ce_status(current_status: int, experimental_mode: bool) -> int: - if is_manual_ce_status(current_status): - return CEStatus["OFF"] - return CEStatus["USER_DISABLED"] if experimental_mode else CEStatus["USER_OVERRIDDEN"] - - -def next_manual_cc_status(current_status: int, experimental_mode: bool) -> int: - if is_manual_cc_status(current_status): - return CCStatus["OFF"] - return CCStatus["USER_CHILL"] if experimental_mode else CCStatus["USER_EXPERIMENTAL"] - def requested_experimental_mode(params: Params, params_memory: Params | None = None) -> bool: if params.get_bool("SafeMode"): return False - if params.get_bool("ConditionalExperimental"): - status = params_memory.get_int(CE_STATUS_PARAM, default=CEStatus["OFF"]) if params_memory is not None else CEStatus["OFF"] - if not is_manual_ce_status(status): - status = get_persisted_ce_status(params) - return status == CEStatus["USER_OVERRIDDEN"] - - if params.get_bool("ConditionalChill"): - status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"]) if params_memory is not None else CCStatus["OFF"] - if not is_manual_cc_status(status): - status = get_persisted_cc_status(params) - if not is_manual_cc_status(status): - return True - return status == CCStatus["USER_EXPERIMENTAL"] - - return params.get_bool("ExperimentalMode") + persistent = params.get_int(LONGITUDINAL_PREFERENCE_PARAM, default=0) + override = params_memory.get_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM, default=-1) if params_memory is not None else -1 + return bool(override if override in (0, 1) else persistent) -def restore_persisted_ce_state(params: Params, params_memory: Params) -> int: - current_status = params_memory.get_int(CE_STATUS_PARAM, default=CEStatus["OFF"]) - if is_manual_ce_status(current_status): - sync_manual_ce_state(params, current_status) - return current_status - - if not params.get_bool(PERSIST_EXPERIMENTAL_STATE_PARAM): - return current_status - - restored_status = get_persisted_ce_status(params) - if restored_status != CEStatus["OFF"]: - params_memory.put_int(CE_STATUS_PARAM, restored_status) - return restored_status - - return current_status - - -def restore_persisted_cc_state(params: Params, params_memory: Params) -> int: - current_status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"]) - if is_manual_cc_status(current_status): - sync_manual_cc_state(params, current_status) - return current_status - - if not params.get_bool(PERSIST_CHILL_STATE_PARAM): - return current_status - - restored_status = get_persisted_cc_status(params) - if restored_status != CCStatus["OFF"]: - params_memory.put_int(CC_STATUS_PARAM, restored_status) - return restored_status - - return current_status +def toggle_longitudinal_model_preference(params: Params, params_memory: Params) -> bool: + requested = not requested_experimental_mode(params, params_memory) + params_memory.put_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM, int(requested)) + return requested diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 422fa06f9..c9dcfe2af 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -107,26 +107,8 @@ SAFE_MODE_MANAGED_KEYS = ( "ReduceLateralAccelerationRain", "ReduceLateralAccelerationRainStorm", "ReduceLateralAccelerationSnow", - "ConditionalExperimental", - "ConditionalChill", - "CECurves", - "CECurvesLead", - "CELead", - "CESlowerLead", - "CEStoppedLead", - "CESpeed", - "CESpeedLead", - "CCMLead", - "CCMLaunchAssist", - "CCMSetSpeedMargin", - "CCMSpeed", - "CCMSpeedLead", - "CEModelStopTime", - "CEStopLights", - "CESignalSpeed", - "CESignalLaneDetection", - "PersistChillState", - "ShowCCMStatus", + "LongitudinalModelPreference", + "ShowCEMStatus", "CurveSpeedController", "SpeedLimitController", "SetSpeedLimit", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 68ff27724..e4ac6b19b 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -802,28 +802,12 @@ class StarPilotVariables: self.migrate_prius_cluster_offset(str(toggle.car_model)) toggle.cluster_offset = self.get_value("ClusterOffset", cast=float, condition=toggle.car_make == "toyota") - toggle.conditional_experimental_mode = toggle.openpilot_longitudinal and self.get_value("ConditionalExperimental") - toggle.conditional_chill_mode = toggle.openpilot_longitudinal and not toggle.conditional_experimental_mode and self.get_value("ConditionalChill") - toggle.conditional_curves = self.get_value("CECurves", condition=toggle.conditional_experimental_mode) - toggle.conditional_curves_lead = self.get_value("CECurvesLead", condition=toggle.conditional_curves) - toggle.conditional_lead = self.get_value("CELead", condition=toggle.conditional_experimental_mode) - toggle.conditional_slower_lead = self.get_value("CESlowerLead", condition=toggle.conditional_lead) - toggle.conditional_stopped_lead = self.get_value("CEStoppedLead", condition=toggle.conditional_lead) - toggle.conditional_limit = self.get_value("CESpeed", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion) - toggle.conditional_limit_lead = self.get_value("CESpeedLead", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion) - toggle.conditional_model_stop_time = self.get_value("CEModelStopTime", cast=float, condition=toggle.conditional_experimental_mode and self.get_value("CEStopLights")) - toggle.conditional_signal = self.get_value("CESignalSpeed", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion) - toggle.conditional_signal_lane_detection = self.get_value("CESignalLaneDetection", condition=toggle.conditional_signal != 0) - toggle.conditional_chill_speed = self.get_value("CCMSpeed", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion) - toggle.conditional_chill_speed_lead = self.get_value("CCMSpeedLead", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion) - toggle.conditional_chill_speed_margin = self.get_value("CCMSetSpeedMargin", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion) - toggle.conditional_chill_lead = self.get_value("CCMLead", condition=toggle.conditional_chill_mode) - toggle.conditional_chill_launch_assist = self.get_value("CCMLaunchAssist", condition=toggle.conditional_chill_mode) - toggle.cem_status = ( - self.get_value("ShowCEMStatus", condition=toggle.conditional_experimental_mode) or - self.get_value("ShowCCMStatus", condition=toggle.conditional_chill_mode) or - toggle.debug_mode + persistent_preference = self.get_value("LongitudinalModelPreference", cast=int, min=0, max=1) + drive_override = self.params_memory.get_int("LongitudinalModelPreferenceOverride", default=-1) + toggle.longitudinal_model_preference = not toggle.safe_mode and bool( + drive_override if drive_override in (0, 1) else persistent_preference ) + toggle.cem_status = self.get_value("ShowCEMStatus") or toggle.debug_mode toggle.curve_speed_controller = toggle.openpilot_longitudinal and self.get_value("CurveSpeedController") toggle.csc_status = self.get_value("ShowCSCStatus", condition=toggle.curve_speed_controller) or toggle.debug_mode diff --git a/starpilot/common/tests/test_accel_profile.py b/starpilot/common/tests/test_accel_profile.py index e2ecc127e..e6bfcaa0a 100644 --- a/starpilot/common/tests/test_accel_profile.py +++ b/starpilot/common/tests/test_accel_profile.py @@ -17,12 +17,12 @@ def test_truck_curves_match_expected_values(): assert get_accel_profile_curve_values(ACCELERATION_PROFILES["SPORT_PLUS"], ev_tuning=False, truck_tuning=True) == A_CRUISE_MAX_VALS_SPORT_PLUS_TRUCK -def test_standard_truck_curve_keeps_mid_high_speed_authority(): +def test_standard_truck_curve_relaxes_at_highway_speed(): values = get_accel_profile_curve_values(ACCELERATION_PROFILES["STANDARD"], ev_tuning=False, truck_tuning=True) - assert interpolate_accel_profile(20.0, values) >= 1.18 - assert interpolate_accel_profile(25.0, values) >= 0.98 - assert interpolate_accel_profile(30.0, values) >= 0.90 + assert 0.5 <= interpolate_accel_profile(20.0, values) <= 0.6 + assert interpolate_accel_profile(25.0, values) == 0.45 + assert 0.35 < interpolate_accel_profile(30.0, values) < 0.45 def test_truck_profiles_remain_ordered(): @@ -32,4 +32,7 @@ def test_truck_profiles_remain_ordered(): sport_plus = get_accel_profile_curve_values(ACCELERATION_PROFILES["SPORT_PLUS"], ev_tuning=False, truck_tuning=True) for e, s, sp, spp in zip(eco, standard, sport, sport_plus, strict=True): - assert e < s < sp < spp + assert e <= s <= sp <= spp + assert any(e < s for e, s in zip(eco, standard, strict=True)) + assert any(s < sp for s, sp in zip(standard, sport, strict=True)) + assert any(sp < spp for sp, spp in zip(sport, sport_plus, strict=True)) diff --git a/starpilot/common/tests/test_experimental_state.py b/starpilot/common/tests/test_experimental_state.py index b1ae1939d..5a7628d53 100644 --- a/starpilot/common/tests/test_experimental_state.py +++ b/starpilot/common/tests/test_experimental_state.py @@ -1,10 +1,7 @@ from openpilot.starpilot.common.experimental_state import ( - CC_STATUS_PARAM, - CCStatus, - CE_STATUS_PARAM, - CEStatus, + LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM, requested_experimental_mode, - restore_persisted_cc_state, + toggle_longitudinal_model_preference, ) @@ -16,9 +13,6 @@ class FakeParams: def get_bool(self, key): return bool(self.bools.get(key, False)) - def put_bool(self, key, value): - self.bools[key] = bool(value) - def get_int(self, key, default=0): return int(self.ints.get(key, default)) @@ -26,54 +20,34 @@ class FakeParams: self.ints[key] = int(value) -def test_requested_experimental_mode_defaults_to_experimental_in_ccm_auto(): - params = FakeParams(bools={"ConditionalChill": True}) - params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["OFF"]}) - - assert requested_experimental_mode(params, params_memory) is True +def test_set_speed_first_is_default(): + assert not requested_experimental_mode(FakeParams(), FakeParams()) -def test_requested_experimental_mode_respects_ccm_manual_override(): - params = FakeParams(bools={"ConditionalChill": True}) - - params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_CHILL"]}) - assert requested_experimental_mode(params, params_memory) is False - - params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"]}) - assert requested_experimental_mode(params, params_memory) is True +def test_persistent_model_first_preference_is_respected(): + params = FakeParams(ints={"LongitudinalModelPreference": 1}) + assert requested_experimental_mode(params, FakeParams()) -def test_requested_experimental_mode_prefers_cem_if_both_conditional_modes_are_enabled(): - params = FakeParams(bools={"ConditionalExperimental": True, "ConditionalChill": True}) +def test_drive_override_takes_priority_without_changing_persistent_default(): + params = FakeParams(ints={"LongitudinalModelPreference": 0}) + params_memory = FakeParams(ints={LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM: 1}) - params_memory = FakeParams(ints={ - CE_STATUS_PARAM: CEStatus["OFF"], - CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"], - }) - assert requested_experimental_mode(params, params_memory) is False - - params_memory = FakeParams(ints={ - CE_STATUS_PARAM: CEStatus["USER_OVERRIDDEN"], - CC_STATUS_PARAM: CCStatus["USER_CHILL"], - }) - assert requested_experimental_mode(params, params_memory) is True + assert requested_experimental_mode(params, params_memory) + assert params.get_int("LongitudinalModelPreference") == 0 -def test_requested_experimental_mode_safe_mode_overrides_ccm(): - params = FakeParams(bools={"SafeMode": True, "ConditionalChill": True}) - params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"]}) +def test_exp_button_flips_preference_for_current_drive(): + params = FakeParams(ints={"LongitudinalModelPreference": 0}) + params_memory = FakeParams() - assert requested_experimental_mode(params, params_memory) is False + assert toggle_longitudinal_model_preference(params, params_memory) + assert params_memory.get_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM) == 1 + assert not toggle_longitudinal_model_preference(params, params_memory) + assert params_memory.get_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM) == 0 -def test_restore_persisted_cc_state_rehydrates_manual_override(): - params = FakeParams( - bools={"PersistChillState": True}, - ints={"PersistedCCStatus": CCStatus["USER_CHILL"]}, - ) - params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["OFF"]}) - - restored_status = restore_persisted_cc_state(params, params_memory) - - assert restored_status == CCStatus["USER_CHILL"] - assert params_memory.get_int(CC_STATUS_PARAM) == CCStatus["USER_CHILL"] +def test_safe_mode_forces_set_speed_first(): + params = FakeParams(bools={"SafeMode": True}, ints={"LongitudinalModelPreference": 1}) + params_memory = FakeParams(ints={LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM: 1}) + assert not requested_experimental_mode(params, params_memory) diff --git a/starpilot/controls/lib/conditional_chill_mode.py b/starpilot/controls/lib/conditional_chill_mode.py deleted file mode 100644 index 9fa91e768..000000000 --- a/starpilot/controls/lib/conditional_chill_mode.py +++ /dev/null @@ -1,350 +0,0 @@ -#!/usr/bin/env python3 -import time - -from openpilot.common.constants import CV - -from openpilot.starpilot.common.experimental_state import ( - CCStatus, - is_manual_cc_status, - restore_persisted_cc_state, -) - - -class ConditionalChillMode: - CCM_STOP_MODEL_TIME = 7.0 - CHILL_SPEED_ENTRY_CONFIRM_TIME = 0.35 - CHILL_LEAD_ENTRY_CONFIRM_TIME = 1.0 - CHILL_LAUNCH_ENTRY_CONFIRM_TIME = 0.0 - CHILL_EXIT_BUFFER_TIME = 0.35 - CHILL_MIN_DWELL_TIME = 1.2 - - CHILL_LAUNCH_EXIT_SPEED = 15 * CV.MPH_TO_MS - CHILL_LAUNCH_MAX_ENTRY_SPEED = 1.0 - CHILL_LAUNCH_MAX_BRAKE = 0.2 - CHILL_LAUNCH_MAX_CLOSING_SPEED = 0.75 - CHILL_LAUNCH_MIN_LEAD_SPEED = 0.35 - CHILL_LAUNCH_MIN_LEAD_DELTA = 0.2 - CHILL_LAUNCH_MIN_LEAD_VREL = 0.2 - CHILL_LAUNCH_MIN_LEAD_ACCEL = 0.08 - - STABLE_LEAD_MIN_MODEL_PROB = 0.9 - STABLE_LEAD_MAX_BRAKE = 0.2 - STABLE_LEAD_MIN_SPEED = 1.5 - STABLE_LEAD_MAX_DISTANCE = 90.0 - STABLE_LEAD_MAX_DISTANCE_TIME = 4.5 - STABLE_LEAD_MAX_CLOSING_SPEED = 1.25 - STABLE_LEAD_MAX_CLOSING_RATIO = 0.05 - - ADJACENT_LEAD_VETO_MIN_SPEED = 1.0 - ADJACENT_LEAD_VETO_MAX_DISTANCE = 65.0 - ADJACENT_LEAD_VETO_MAX_DISTANCE_TIME = 3.5 - ADJACENT_LEAD_VETO_MAX_LATERAL_OFFSET = 5.5 - - LOW_SPEED_STOP_SCENE_MAX_SPEED = 18 * CV.MPH_TO_MS - - def __init__(self, StarPilotPlanner, detector): - self.starpilot_planner = StarPilotPlanner - self.detector = detector - self.params = self.starpilot_planner.params - self.params_memory = self.starpilot_planner.params_memory - - self.experimental_mode = True - self.status_value = CCStatus["OFF"] - self._active_auto_status = CCStatus["OFF"] - self._candidate_since = 0.0 - self._soft_exit_since = 0.0 - self._chill_hold_until = 0.0 - self._prev_cc_status = None - self._launch_active = False - self._launch_forced_exit = False - - def update(self, v_ego, v_cruise, sm, starpilot_toggles): - now = time.monotonic() - safe_mode = self.params.get_bool("SafeMode") - - self.status_value = CCStatus["OFF"] if safe_mode else restore_persisted_cc_state(self.params, self.params_memory) - - if is_manual_cc_status(self.status_value): - self._reset_timers() - self.experimental_mode = self.status_value == CCStatus["USER_EXPERIMENTAL"] - self._write_status(self.status_value) - return - - self._refresh_detector(v_ego, sm) - auto_status, launch_candidate = self._get_chill_status(v_ego, v_cruise, sm, starpilot_toggles) - - if safe_mode or self._has_hard_veto(v_ego, sm, allow_launch=launch_candidate): - self._reset_timers() - self.experimental_mode = False if safe_mode else True - self.status_value = CCStatus["OFF"] - self._write_status(CCStatus["OFF"]) - return - - chill_candidate = auto_status != CCStatus["OFF"] - entry_confirm_time = self._get_entry_confirm_time(auto_status, launch_candidate) - - if chill_candidate: - if self._candidate_since == 0.0: - self._candidate_since = now - self._soft_exit_since = 0.0 - - if not self.experimental_mode or (now - self._candidate_since) >= entry_confirm_time: - self.experimental_mode = False - self._active_auto_status = auto_status - self._chill_hold_until = max(self._chill_hold_until, now + self.CHILL_MIN_DWELL_TIME) - self.status_value = self._active_auto_status if not self.experimental_mode else CCStatus["OFF"] - else: - self._candidate_since = 0.0 - if not self.experimental_mode: - if self._launch_forced_exit: - self.experimental_mode = True - self._active_auto_status = CCStatus["OFF"] - self.status_value = CCStatus["OFF"] - self._soft_exit_since = 0.0 - self._chill_hold_until = 0.0 - self._launch_forced_exit = False - self._write_status(CCStatus["OFF"]) - return - - if self._soft_exit_since == 0.0: - self._soft_exit_since = now - - hold_active = now < self._chill_hold_until - exit_buffer_active = (now - self._soft_exit_since) < self.CHILL_EXIT_BUFFER_TIME - if hold_active or exit_buffer_active: - self.status_value = self._active_auto_status - else: - self.experimental_mode = True - self._active_auto_status = CCStatus["OFF"] - self.status_value = CCStatus["OFF"] - else: - self._soft_exit_since = 0.0 - self.status_value = CCStatus["OFF"] - - self._write_status(self.status_value if not self.experimental_mode else CCStatus["OFF"]) - - def _reset_timers(self): - self._active_auto_status = CCStatus["OFF"] - self._candidate_since = 0.0 - self._soft_exit_since = 0.0 - self._chill_hold_until = 0.0 - self._launch_active = False - self._launch_forced_exit = False - - def _refresh_detector(self, v_ego, sm): - detector_toggles = type("DetectorToggles", (), { - "conditional_curves": True, - "conditional_curves_lead": True, - "conditional_lead": True, - "conditional_slower_lead": True, - "conditional_stopped_lead": True, - })() - self.detector.curve_detection(v_ego, detector_toggles) - self.detector.slow_lead(detector_toggles, v_ego) - self.detector.stop_sign_and_light(v_ego, sm, self.CCM_STOP_MODEL_TIME) - - def _has_hard_veto(self, v_ego, sm, allow_launch=False): - if sm["carState"].standstill and not allow_launch: - return True - - if sm["carState"].leftBlinker or sm["carState"].rightBlinker: - return True - - if sm["starpilotCarState"].trafficModeEnabled: - return True - - if self.starpilot_planner.starpilot_vcruise.slc.experimental_mode: - return True - - if self.detector.curve_detected or self.detector.slow_lead_detected or self.detector.stop_light_detected: - return True - - if self.starpilot_planner.starpilot_vcruise.stop_sign_confirmed or self.starpilot_planner.starpilot_vcruise.forcing_stop: - return True - - if self._adjacent_lead_ambiguous(sm, v_ego): - return True - - return self._low_speed_stop_scene(v_ego) and not allow_launch - - def _low_speed_stop_scene(self, v_ego): - if v_ego >= self.LOW_SPEED_STOP_SCENE_MAX_SPEED: - return False - - if self.starpilot_planner.raw_model_stopped or self.starpilot_planner.model_stopped or self.detector.stop_light_model_detected: - return True - - lead = self.starpilot_planner.lead_one - if not getattr(lead, "status", False): - return False - - lead_distance = float(getattr(lead, "dRel", float("inf"))) - lead_speed = float(getattr(lead, "vLead", float("inf"))) - lead_distance_limit = max(40.0, v_ego * self.STABLE_LEAD_MAX_DISTANCE_TIME) - return lead_distance < lead_distance_limit and lead_speed < max(6.0, v_ego + 0.5) - - def _get_chill_status(self, v_ego, v_cruise, sm, starpilot_toggles): - if getattr(starpilot_toggles, "conditional_chill_launch_assist", False): - launch_status = self._get_launch_status(v_ego, sm) - if launch_status != CCStatus["OFF"]: - return launch_status, True - - lead = self.starpilot_planner.lead_one - lead_status = bool(getattr(lead, "status", False)) - tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False)) - set_speed_error = max(0.0, v_cruise - v_ego) - - if (not lead_status and not tracking_lead and - v_ego >= starpilot_toggles.conditional_chill_speed and - set_speed_error >= starpilot_toggles.conditional_chill_speed_margin): - return CCStatus["SPEED"], False - - if not starpilot_toggles.conditional_chill_lead: - return CCStatus["OFF"], False - - if v_ego < starpilot_toggles.conditional_chill_speed_lead or not lead_status or not tracking_lead: - return CCStatus["OFF"], False - - lead_distance = float(getattr(lead, "dRel", float("inf"))) - lead_speed = float(getattr(lead, "vLead", 0.0)) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - lead_prob = float(getattr(lead, "modelProb", 0.0)) - closing_speed = max(0.0, v_ego - lead_speed) - max_closing_speed = max(self.STABLE_LEAD_MAX_CLOSING_SPEED, self.STABLE_LEAD_MAX_CLOSING_RATIO * v_ego) - max_distance = min(self.STABLE_LEAD_MAX_DISTANCE, max(35.0, v_ego * self.STABLE_LEAD_MAX_DISTANCE_TIME)) - lead_confident = bool(getattr(lead, "radar", False)) or lead_prob >= self.STABLE_LEAD_MIN_MODEL_PROB - - if not lead_confident: - return CCStatus["OFF"], False - - if lead_distance >= max_distance or lead_speed <= self.STABLE_LEAD_MIN_SPEED: - return CCStatus["OFF"], False - - if lead_brake > self.STABLE_LEAD_MAX_BRAKE or closing_speed > max_closing_speed: - return CCStatus["OFF"], False - - return CCStatus["LEAD"], False - - def _get_launch_status(self, v_ego, sm): - if self._launch_active and self._launch_exit_required(v_ego, sm): - self._launch_active = False - self._launch_forced_exit = True - return CCStatus["OFF"] - - if self._launch_active: - return self._get_launch_cc_status() - - if not self._launch_scene_eligible(v_ego, sm): - return CCStatus["OFF"] - - self._launch_forced_exit = False - self._launch_active = True - return self._get_launch_cc_status() - - def _launch_scene_eligible(self, v_ego, sm): - if v_ego > self.CHILL_LAUNCH_MAX_ENTRY_SPEED and not self._launch_active: - return False - - selfdrive_state = self._get_sm_service(sm, "selfdriveState") - longitudinal_plan = self._get_sm_service(sm, "longitudinalPlan") - starpilot_plan = self._get_sm_service(sm, "starpilotPlan") - if selfdrive_state is None or longitudinal_plan is None: - return False - - if not bool(getattr(selfdrive_state, "enabled", False)): - return False - - if ( - self.detector.stop_light_detected or - self.detector.stop_light_model_detected or - self.starpilot_planner.raw_model_stopped or - self.starpilot_planner.model_stopped or - self.starpilot_planner.starpilot_vcruise.stop_sign_confirmed or - self.starpilot_planner.starpilot_vcruise.forcing_stop or - bool(getattr(starpilot_plan, "redLight", False)) or - bool(getattr(starpilot_plan, "forcingStop", False)) - ): - return False - - if bool(getattr(longitudinal_plan, "shouldStop", False)) or not bool(getattr(longitudinal_plan, "allowThrottle", False)): - return False - - lead = getattr(self.starpilot_planner, "lead_one", None) - lead_status = bool(getattr(lead, "status", False)) - tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False)) - if not lead_status and not tracking_lead: - return True - - lead_speed = float(getattr(lead, "vLead", 0.0)) - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - lead_accel = float(getattr(lead, "aLeadK", 0.0)) - lead_delta = lead_speed - float(v_ego) - lead_vrel = float(getattr(lead, "vRel", lead_delta)) - closing_speed = max(0.0, v_ego - lead_speed) - - return ( - lead_speed >= self.CHILL_LAUNCH_MIN_LEAD_SPEED and - lead_delta >= self.CHILL_LAUNCH_MIN_LEAD_DELTA and - lead_vrel >= self.CHILL_LAUNCH_MIN_LEAD_VREL and - lead_accel >= self.CHILL_LAUNCH_MIN_LEAD_ACCEL and - lead_brake <= self.CHILL_LAUNCH_MAX_BRAKE and - closing_speed <= self.CHILL_LAUNCH_MAX_CLOSING_SPEED - ) - - def _launch_exit_required(self, v_ego, sm): - if v_ego >= self.CHILL_LAUNCH_EXIT_SPEED: - return True - - if not self._launch_scene_eligible(v_ego, sm): - return True - - return False - - def _get_launch_cc_status(self): - lead = getattr(self.starpilot_planner, "lead_one", None) - lead_status = bool(getattr(lead, "status", False)) - tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False)) - return CCStatus["LEAD"] if lead_status or tracking_lead else CCStatus["SPEED"] - - def _get_entry_confirm_time(self, auto_status, launch_candidate): - if launch_candidate: - return self.CHILL_LAUNCH_ENTRY_CONFIRM_TIME - - if auto_status == CCStatus["LEAD"]: - return self.CHILL_LEAD_ENTRY_CONFIRM_TIME - - return self.CHILL_SPEED_ENTRY_CONFIRM_TIME - - def _adjacent_lead_ambiguous(self, sm, v_ego): - radar_state = self._get_sm_service(sm, "starpilotRadarState") - if radar_state is None: - return False - - max_distance = min(self.ADJACENT_LEAD_VETO_MAX_DISTANCE, max(25.0, v_ego * self.ADJACENT_LEAD_VETO_MAX_DISTANCE_TIME)) - for lead in (getattr(radar_state, "leadLeft", None), getattr(radar_state, "leadRight", None)): - if lead is None or not getattr(lead, "status", False): - continue - - lateral_offset = abs(float(getattr(lead, "yRel", 0.0))) - if lateral_offset > self.ADJACENT_LEAD_VETO_MAX_LATERAL_OFFSET: - continue - - if float(getattr(lead, "dRel", float("inf"))) < max_distance and float(getattr(lead, "vLead", 0.0)) > self.ADJACENT_LEAD_VETO_MIN_SPEED: - return True - - return False - - @staticmethod - def _get_sm_service(sm, key): - if isinstance(sm, dict): - return sm.get(key) - - try: - return sm[key] - except (KeyError, IndexError, TypeError, AttributeError): - return None - - def _write_status(self, status_value): - if status_value != self._prev_cc_status: - self.params_memory.put_int("CCStatus", status_value) - self._prev_cc_status = status_value diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py deleted file mode 100644 index b13fec29e..000000000 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ /dev/null @@ -1,521 +0,0 @@ -#!/usr/bin/env python3 -import time -import numpy as np - -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import DT_MDL -from openpilot.common.constants import CV - -from openpilot.starpilot.common.experimental_state import ( - CEStatus, - is_manual_ce_status, - restore_persisted_ce_state, -) -from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, THRESHOLD - -def interp(x, xp, fp): - return float(np.interp(x, xp, fp)) - - -def scale_threshold(v_ego): - # Speed-based lead threshold behavior (v_ego in m/s) - return interp(v_ego, [0.0, 17.9, 26.8, 35.8, 44.7], [0.58, 0.60, 0.62, 0.75, 0.90]) - - -class ConditionalExperimentalMode: - # ===== CONDITIONAL EXPERIMENTAL MODE SPEED-BASED TUNING ===== - # Speed ranges: [0-35, 35-55, 55-70, 70+ mph] - - # FILTER TIME CONSTANTS (Lower = More responsive, Higher = Smoother) - # [City, Urban Hwy, Rural Hwy, High Speed] - FILTER_TIME_CURVES = [0.9, 0.8, 0.6, 0.5] # Faster detection at highway speeds - FILTER_TIME_LEADS = [0.9, 0.8, 0.7, 0.5] # Less sensitive at 70+ mph for slow leads - FILTER_TIME_LIGHTS = [0.9, 0.8, 0.75, 0.55] # Less sensitive at 60+ mph for stoplights - - # HIGHWAY LIGHT DETECTION MULTIPLIERS - # How much to increase model stop time at highway speeds - LIGHT_BOOSTS = [1.0, 1.2, 1.1, 1.0] # Keep conservative boost for highest speeds - LIGHT_SPEED_LOW = 50 * CV.MPH_TO_MS # 50 mph threshold - LIGHT_SPEED_HIGH = 60 * CV.MPH_TO_MS # 60 mph threshold - LIGHT_MAX_TIME = 9 # Balanced max time preserving city performance - LOW_SPEED_LIGHT_FILTER_TIME = 0.35 - LEAD_CLEAR_FILTER_TIME_LOW = 0.6 - LEAD_CLEAR_FILTER_TIME_HIGH = 0.35 - STOP_LIGHT_ON_MARGIN = 2.5 - STOP_LIGHT_OFF_MARGIN = 4.0 - STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN = 10.0 - STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0 - STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0 - STOP_LIGHT_DETECTED_HOLD_TIME = 4.0 - STOP_APPROACH_LATCH_TIME = 1.0 - STOP_APPROACH_MAX_LEAD_SPEED = 4.5 - STOP_APPROACH_MIN_MODEL_PROB = 0.9 - SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB = 0.85 - SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME = 4.0 - SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5 - SLOW_LEAD_CONTINUITY_HOLD_TIME = 1.25 - SLOW_LEAD_FORCE_CLEAR_TIME = 0.75 - SLOW_LEAD_MODE_RELEASE_HOLD_TIME = 1.5 - SLOW_LEAD_MIN_CLOSING_SPEED = 0.75 - SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5 - SLOW_RADAR_LEAD_TRIGGER_MAX_DISTANCE_TIME = 2.5 - SLOW_RADAR_LEAD_TRIGGER_MIN_DISTANCE = 40.0 - POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0 - TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS - TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0 - - # ===== END TUNING PARAMETERS ===== - - # Current active values - FILTER_TIME_CURVE = 0.8 - FILTER_TIME_LEAD = 0.8 - FILTER_TIME_LIGHT = 0.8 - LIGHT_BOOST_LOW = 1.15 - LIGHT_BOOST_HIGH = 1.2 - - # Small latch to avoid frame-to-frame mode chatter. - CEM_TRANSITION_GUARD_TIME = 0.50 - CEM_TRANSITION_BUFFER_TIME = 0.25 - - @staticmethod - def get_speed_based_param(speed_mph, param_array): - """Get parameter value based on current speed using smooth interpolation between breakpoints [0, 35, 55, 70]""" - return interp(speed_mph, [0, 35, 55, 70], param_array) - - def __init__(self, StarPilotPlanner): - self.starpilot_planner = StarPilotPlanner - self.params = self.starpilot_planner.params - self.params_memory = self.starpilot_planner.params_memory - - # Faster filters with hysteresis for better responsiveness - self.curvature_filter = FirstOrderFilter(0, self.FILTER_TIME_CURVE, DT_MDL) - self.slow_lead_filter = FirstOrderFilter(0, self.FILTER_TIME_LEAD, DT_MDL) - self.stop_light_filter = FirstOrderFilter(0, self.FILTER_TIME_LIGHT, DT_MDL) - self.lead_clear_filter = FirstOrderFilter(0, self.LEAD_CLEAR_FILTER_TIME_LOW, DT_MDL) - - self.curve_detected = False - self.slow_lead_detected = False - self.prev_tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False)) - self.slow_lead_clear_since = 0.0 - self.slow_lead_continuity_until = 0.0 - self.experimental_mode = False - self.stop_light_detected = False - self.stop_light_model_detected = False - self.stop_light_detected_hold_until = 0.0 - self.stop_approach_hold_until = 0.0 - self.standstill_stop_reason = None - self.prev_experimental_mode = False # For hysteresis - self.mode_hold_until = 0.0 - self.mode_false_since = 0.0 - self.slow_lead_mode_hold_until = 0.0 - self._prev_ce_status = None - self.prev_standstill = False - self.prev_standstill_stop_hold = False - self.standstill_stop_release_pending = False - self.post_stop_launch_trigger_suppress_until = 0.0 - - def update(self, v_ego, sm, starpilot_toggles): - now = time.monotonic() - standstill = bool(sm["carState"].standstill) - current_standstill_stop_hold = False - released_standstill_stop_hold = self.prev_standstill and self.prev_standstill_stop_hold and not standstill - completed_pending_stop_release = self.standstill_stop_release_pending and not standstill - - if released_standstill_stop_hold or completed_pending_stop_release: - self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME - self.mode_hold_until = 0.0 - self.mode_false_since = 0.0 - self.slow_lead_mode_hold_until = 0.0 - self.prev_experimental_mode = False - self.standstill_stop_release_pending = False - - if not standstill: - self.standstill_stop_reason = None - - self.status_value = CEStatus["OFF"] if self.params.get_bool("SafeMode") else restore_persisted_ce_state(self.params, self.params_memory) - - if not is_manual_ce_status(self.status_value) and not standstill: - self.update_conditions(v_ego, sm, starpilot_toggles) - - triggered = self.check_conditions(v_ego, sm, starpilot_toggles) - if triggered: - self.mode_hold_until = now + self.CEM_TRANSITION_GUARD_TIME - self.mode_false_since = 0.0 - if self.status_value == CEStatus["LEAD"]: - self.slow_lead_mode_hold_until = now + self.SLOW_LEAD_MODE_RELEASE_HOLD_TIME - else: - self.slow_lead_mode_hold_until = 0.0 - elif self.prev_experimental_mode and self.mode_false_since == 0.0: - self.mode_false_since = now - elif not self.prev_experimental_mode: - self.mode_false_since = 0.0 - - hold_active = now < self.mode_hold_until - transition_buffer_active = self.mode_false_since != 0.0 and (now - self.mode_false_since) < self.CEM_TRANSITION_BUFFER_TIME - slow_lead_hold_active = bool( - starpilot_toggles.conditional_lead and - now < self.slow_lead_mode_hold_until and - self.has_credible_slow_lead_context(v_ego) - ) - if slow_lead_hold_active and not triggered: - self.status_value = CEStatus["LEAD"] - elif not slow_lead_hold_active: - self.slow_lead_mode_hold_until = 0.0 - - self.experimental_mode = triggered or slow_lead_hold_active or hold_active or transition_buffer_active - self.prev_experimental_mode = self.experimental_mode - ce_write_value = self.status_value if self.experimental_mode else CEStatus["OFF"] - if ce_write_value != self._prev_ce_status: - self.params_memory.put_int("CEStatus", ce_write_value) - self._prev_ce_status = ce_write_value - elif not is_manual_ce_status(self.status_value): - self.mode_hold_until = 0.0 - self.mode_false_since = 0.0 - self.slow_lead_mode_hold_until = 0.0 - - # Keep the stop-light path live at standstill so EXP stays pinned for a red - # light / stop sign. Stop signs latch until pedal, while stop lights can - # immediately release to CHILL when the model clears the stop. - self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time) - standstill_stop_hold = self.get_standstill_stop_hold(sm) - current_standstill_stop_hold = standstill_stop_hold - - if current_standstill_stop_hold: - self.standstill_stop_release_pending = False - elif self.prev_standstill_stop_hold: - self.standstill_stop_release_pending = True - - if self.standstill_stop_release_pending: - self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME - - self.experimental_mode = standstill_stop_hold - self.prev_experimental_mode = self.experimental_mode - self.status_value = CEStatus["STOP_LIGHT"] if self.experimental_mode else CEStatus["OFF"] - ce_write_value = self.status_value - if ce_write_value != self._prev_ce_status: - self.params_memory.put_int("CEStatus", ce_write_value) - self._prev_ce_status = ce_write_value - else: - self.mode_hold_until = 0.0 - self.mode_false_since = 0.0 - self.slow_lead_mode_hold_until = 0.0 - self._prev_ce_status = None - self.standstill_stop_release_pending = False - self.experimental_mode = self.status_value == CEStatus["USER_OVERRIDDEN"] - self.prev_experimental_mode = self.experimental_mode - self.stop_light_detected &= not is_manual_ce_status(self.status_value) - self.stop_light_filter.x = 0 - - self.prev_standstill = standstill - self.prev_standstill_stop_hold = current_standstill_stop_hold - - def has_credible_slow_lead_context(self, v_ego): - lead = self.starpilot_planner.lead_one - if lead is None or not bool(getattr(lead, "status", False)): - return False - - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - if not lead_radar and lead_prob < self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB: - return False - - lead_distance = float(getattr(lead, "dRel", float("inf"))) - return lead_distance < max(40.0, float(v_ego) * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) - - def get_standstill_stop_hold(self, sm): - dash_stop_sign = ( - bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or - bool(getattr(sm["starpilotCarState"], "dashboardStopSign", 0) > 0) - ) - force_stop_active = bool(getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False)) - model_stopped = bool(getattr(self.starpilot_planner, "model_stopped", False)) - pedal_override = bool(getattr(sm["carState"], "gasPressed", False) or getattr(sm["starpilotCarState"], "accelPressed", False)) - - if pedal_override or not bool(sm["carState"].standstill): - self.standstill_stop_reason = None - return False - - if dash_stop_sign: - self.standstill_stop_reason = "sign" - elif self.stop_light_detected or force_stop_active or model_stopped: - if self.standstill_stop_reason is None: - self.standstill_stop_reason = "light" - elif self.standstill_stop_reason == "light": - self.standstill_stop_reason = None - - if self.standstill_stop_reason == "sign": - return True - - return bool(self.stop_light_detected or force_stop_active or model_stopped) - - def check_conditions(self, v_ego, sm, starpilot_toggles): - launch_trigger_suppressed = time.monotonic() < self.post_stop_launch_trigger_suppress_until - below_speed = not launch_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead - below_speed_with_lead = not launch_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead - if below_speed or below_speed_with_lead: - self.status_value = CEStatus["SPEED"] - return True - - desired_lane = self.starpilot_planner.lane_width_left if sm["carState"].leftBlinker else self.starpilot_planner.lane_width_right - lane_available = desired_lane >= starpilot_toggles.lane_detection_width or not starpilot_toggles.conditional_signal_lane_detection - if v_ego < starpilot_toggles.conditional_signal and (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and not lane_available: - self.status_value = CEStatus["SIGNAL"] - return True - - if starpilot_toggles.conditional_curves and self.curve_detected and (starpilot_toggles.conditional_curves_lead or not self.starpilot_planner.starpilot_following.following_lead): - self.status_value = CEStatus["CURVATURE"] - return True - - if not launch_trigger_suppressed and starpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31: - self.status_value = CEStatus["LEAD"] - return True - - if starpilot_toggles.conditional_model_stop_time != 0 and self.stop_light_detected: - self.status_value = CEStatus["STOP_LIGHT"] - return True - - if self.starpilot_planner.starpilot_vcruise.slc.experimental_mode: - self.status_value = CEStatus["SPEED_LIMIT"] - return True - - return False - - def update_conditions(self, v_ego, sm, starpilot_toggles): - self.curve_detection(v_ego, starpilot_toggles) - self.slow_lead(starpilot_toggles, v_ego) - self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time) - - def curve_detection(self, v_ego, starpilot_toggles): - self.curvature_filter.update(self.starpilot_planner.road_curvature_detected or self.starpilot_planner.driving_in_curve) - self.curve_detected = bool(self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED) - - def slow_lead(self, starpilot_toggles, v_ego): - now = time.monotonic() - lead = self.starpilot_planner.lead_one - tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False)) - lead_status = bool(getattr(lead, "status", False)) - lead_distance = float(getattr(lead, "dRel", float("inf"))) - lead_speed = float(getattr(lead, "vLead", float("inf"))) - lead_prob = float(getattr(lead, "modelProb", 1.0)) - lead_radar = bool(getattr(lead, "radar", False)) - closing_speed = max(0.0, v_ego - lead_speed) - min_closing_speed = max(self.SLOW_LEAD_MIN_CLOSING_SPEED, 0.04 * v_ego) - - if not starpilot_toggles.conditional_stopped_lead and v_ego < self.SLOW_LEAD_CONTINUITY_MIN_EGO: - self.clear_slow_lead_state(tracking_lead) - return - - radar_slow_lead_in_range = bool( - not lead_radar or - lead_distance < max(self.SLOW_RADAR_LEAD_TRIGGER_MIN_DISTANCE, - v_ego * self.SLOW_RADAR_LEAD_TRIGGER_MAX_DISTANCE_TIME) - ) - slower_lead = bool( - starpilot_toggles.conditional_slower_lead and - self.starpilot_planner.starpilot_following.slower_lead and - radar_slow_lead_in_range - ) - stopped_lead = bool( - starpilot_toggles.conditional_stopped_lead and - lead_status and - lead_speed < 1 and - lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) - ) - vision_slow_lead_candidate = bool( - lead_status and - not lead_radar and - lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and - lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) and - closing_speed >= min_closing_speed and - lead_speed < max(v_ego - 0.5, 2.0) - ) - - lead_threshold = scale_threshold(v_ego) - adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence - - if lead_status and not slower_lead and not stopped_lead and closing_speed < (min_closing_speed * self.SLOW_LEAD_CLEAR_FASTER_FACTOR): - self.clear_slow_lead_state(tracking_lead) - return - - if tracking_lead and (slower_lead or stopped_lead or vision_slow_lead_candidate): - self.slow_lead_continuity_until = now + self.SLOW_LEAD_CONTINUITY_HOLD_TIME - elif self.prev_tracking_lead and not tracking_lead and self.slow_lead_detected and vision_slow_lead_candidate: - self.slow_lead_continuity_until = now + self.SLOW_LEAD_CONTINUITY_HOLD_TIME - - raw_vision_slow_lead = bool( - starpilot_toggles.conditional_slower_lead and - not tracking_lead and - now < self.slow_lead_continuity_until and - vision_slow_lead_candidate - ) - tracked_vision_mode_continuation = bool( - starpilot_toggles.conditional_slower_lead and - tracking_lead and - self.prev_experimental_mode and - vision_slow_lead_candidate - ) - - slow_lead_active = bool(slower_lead or raw_vision_slow_lead or stopped_lead or tracked_vision_mode_continuation) - if slow_lead_active: - self.slow_lead_clear_since = 0.0 - self.slow_lead_filter.update(True) - self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold) - elif tracking_lead: - if self.slow_lead_clear_since == 0.0: - self.slow_lead_clear_since = now - - if (now - self.slow_lead_clear_since) >= self.SLOW_LEAD_FORCE_CLEAR_TIME: - self.clear_slow_lead_state(tracking_lead) - else: - self.slow_lead_filter.update(False) - self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold) - else: - self.clear_slow_lead_state(tracking_lead) - - self.prev_tracking_lead = tracking_lead - - def clear_slow_lead_state(self, tracking_lead): - self.slow_lead_filter.update(False) - self.slow_lead_detected = False - self.slow_lead_clear_since = 0.0 - self.slow_lead_continuity_until = 0.0 - self.prev_tracking_lead = tracking_lead - - def reset_stop_light_state(self): - self.stop_light_filter.x = 0 - self.stop_light_detected = False - self.stop_light_model_detected = False - self.stop_light_detected_hold_until = 0.0 - self.lead_clear_filter.x = 0 - self.stop_approach_hold_until = 0.0 - - def in_committed_turn_scene(self, v_ego, sm): - car_state = sm["carState"] - if bool(getattr(car_state, "standstill", False)) or v_ego > self.TURN_STOP_LIGHT_VETO_MAX_SPEED: - return False - - if not (bool(getattr(car_state, "leftBlinker", False)) or bool(getattr(car_state, "rightBlinker", False))): - return False - - steering_angle = abs(float(getattr(car_state, "steeringAngleDeg", 0.0))) - return bool( - steering_angle >= self.TURN_STOP_LIGHT_VETO_STEERING_ANGLE or - getattr(self.starpilot_planner, "driving_in_curve", False) - ) - - def stop_sign_and_light(self, v_ego, sm, model_time): - now = time.monotonic() - - # While the dashboard has confirmed a stop sign on this approach, pin CEM in EXP. - # Approaches routinely exceed the mode_hold_until/mode_false_since hysteresis (0.5s/0.25s), - # so without this the model briefly losing the sign drops CEM to CHILL and stalls the - # force-stop activation path. Latch is owned by starpilot_vcruise. - if getattr(self.starpilot_planner.starpilot_vcruise, 'stop_sign_confirmed', False): - self.stop_light_filter.x = 1.0 - self.stop_light_detected = True - return - - if self.in_committed_turn_scene(v_ego, sm): - self.reset_stop_light_state() - return - - if not sm["starpilotCarState"].trafficModeEnabled: - speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph - - # Interp for smooth scaling in 35-45 mph - bp = [0, 35, 45] - low_filter_time = 0.0 # No filtering under 35 mph - tuned_filter_time_curves = self.FILTER_TIME_CURVES[1] # At 35-55 mph - tuned_filter_time_leads = self.FILTER_TIME_LEADS[1] - tuned_filter_time_lights = self.FILTER_TIME_LIGHTS[1] - low_boost = 1.0 - tuned_boost = self.LIGHT_BOOSTS[1] - low_cap_factor = 0.0 # No cap under 35 mph - tuned_cap_factor = 1.0 - - filter_time_curves = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_curves]) - filter_time_leads = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_leads]) - filter_time_lights = interp(speed_mph, bp, [self.LOW_SPEED_LIGHT_FILTER_TIME, self.LOW_SPEED_LIGHT_FILTER_TIME, tuned_filter_time_lights]) - lead_clear_filter_time = interp(speed_mph, bp, [self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_HIGH]) - light_boost = interp(speed_mph, bp, [low_boost, low_boost, tuned_boost]) - cap_factor = interp(speed_mph, bp, [low_cap_factor, low_cap_factor, tuned_cap_factor]) - - # Update filter times with interp - self.curvature_filter.update_alpha(filter_time_curves) - self.slow_lead_filter.update_alpha(filter_time_leads) - self.stop_light_filter.update_alpha(filter_time_lights) - self.lead_clear_filter.update_alpha(lead_clear_filter_time) - - # Disable stoplight detection at very high speeds to prevent false positives - if speed_mph > 75: # Disable above 75 mph - self.reset_stop_light_state() - return - - # Adjust model time with interp boost and gradual cap - adjusted_model_time = model_time * light_boost - if cap_factor > 0: - adjusted_model_time = min(adjusted_model_time, self.LIGHT_MAX_TIME * cap_factor + model_time * (1 - cap_factor)) # Gradual cap - - stop_threshold = max(v_ego * adjusted_model_time, 0.0) - if self.stop_light_model_detected: - model_stopping = self.starpilot_planner.model_length < stop_threshold + self.STOP_LIGHT_OFF_MARGIN - else: - model_stopping = self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_ON_MARGIN, 0.0) - self.stop_light_model_detected = model_stopping - - # `model_stopped` is a coarse horizon-length check (< 50 m with current constants) - # used elsewhere for force-stop/green-light behavior. Reusing it here causes - # ordinary low-speed cruising to look like a stop prediction and can latch the - # STOP_LIGHT CEM trigger. For the CEM detector, key strictly off the configured - # "predicted stop within N seconds" threshold. - # Key off relevant raw lead presence, not trackingLead. Vision-only GM can - # flap trackingLead around the model-length threshold while leadOne remains - # present; far/stale leads should not suppress true stop-light detection. - lead = getattr(self.starpilot_planner, "lead_one", None) - lead_distance = float(getattr(lead, "dRel", float("inf"))) - lead_speed = float(getattr(lead, "vLead", float("inf"))) - lead_radar = bool(getattr(lead, "radar", False)) - lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) - tracking_lead = bool(self.starpilot_planner.tracking_lead) - lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN - vision_stop_approach = ( - lead_relevant and - not lead_radar and - lead_prob >= self.STOP_APPROACH_MIN_MODEL_PROB and - lead_speed < self.STOP_APPROACH_MAX_LEAD_SPEED - ) - stop_approach_hold_active = now < self.stop_approach_hold_until - trackable_stop_approach = vision_stop_approach and not tracking_lead - if (self.stop_light_detected or self.stop_light_model_detected or stop_approach_hold_active) and trackable_stop_approach: - self.stop_approach_hold_until = now + self.STOP_APPROACH_LATCH_TIME - stop_approach_latched = now < self.stop_approach_hold_until and trackable_stop_approach - handoff_to_stopped_lead = ( - lead_relevant and - not tracking_lead and - ( - (self.stop_light_detected and lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED) or - stop_approach_latched - ) - ) - if handoff_to_stopped_lead: - lead_cleared = True - else: - self.lead_clear_filter.update(not lead_relevant) - lead_cleared = self.lead_clear_filter.x >= THRESHOLD - self.stop_light_filter.update(model_stopping and lead_cleared) - model_detector_active = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) - detector_active = bool(model_detector_active or handoff_to_stopped_lead or stop_approach_latched) - model_hold_qualifies = bool( - self.starpilot_planner.model_stopped or - self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN, 0.0) - ) - if model_detector_active and model_hold_qualifies: - self.stop_light_detected_hold_until = now + self.STOP_LIGHT_DETECTED_HOLD_TIME - - hold_context_ok = bool((not lead_relevant) or trackable_stop_approach) - self.stop_light_detected = bool( - detector_active or - (hold_context_ok and now < self.stop_light_detected_hold_until) - ) - else: - self.reset_stop_light_state() diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index 4d5c486e4..023d55519 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -212,9 +212,8 @@ class StarPilotAcceleration: stop_context = ( sm["carState"].standstill or getattr(sm["controlsState"], "forceDecel", False) or - getattr(self.starpilot_planner.starpilot_cem, "stop_light_detected", False) or - getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False) or - getattr(self.starpilot_planner.starpilot_following, "disable_throttle", False) + self.starpilot_planner.longitudinal_intent.stop_detected or + getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False) ) if (getattr(starpilot_toggles, "speed_limit_controller", False) and v_ego > SLC_COAST_MIN_SPEED and diff --git a/starpilot/controls/lib/starpilot_events.py b/starpilot/controls/lib/starpilot_events.py index c748ba5f5..05c775f7b 100644 --- a/starpilot/controls/lib/starpilot_events.py +++ b/starpilot/controls/lib/starpilot_events.py @@ -71,7 +71,7 @@ class StarPilotEvents: if not self.starpilot_planner.model_stopped and self.stopped_for_light and starpilot_toggles.green_light_alert: self.events.add(StarPilotEventName.greenLight) - self.stopped_for_light = self.starpilot_planner.starpilot_cem.stop_light_detected + self.stopped_for_light = self.starpilot_planner.longitudinal_intent.stop_detected else: self.stopped_for_light = False diff --git a/starpilot/controls/lib/starpilot_following.py b/starpilot/controls/lib/starpilot_following.py index df1eca5f3..1b5062449 100644 --- a/starpilot/controls/lib/starpilot_following.py +++ b/starpilot/controls/lib/starpilot_following.py @@ -2,8 +2,7 @@ import numpy as np from openpilot.common.constants import CV -from openpilot.selfdrive.controls.lib.lead_behavior import should_disable_far_lead_throttle -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, MAX_T_FOLLOW @@ -18,10 +17,6 @@ class StarPilotFollowing: def __init__(self, StarPilotPlanner): self.starpilot_planner = StarPilotPlanner - self.disable_throttle = False - self.following_lead = False - self.slower_lead = False - self.acceleration_jerk = 0 self.danger_jerk = 0 self.desired_follow_distance = 0 @@ -78,28 +73,10 @@ class StarPilotFollowing: self.danger_jerk = self.base_danger_jerk self.speed_jerk = self.base_speed_jerk - self.following_lead = self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego - self.slower_lead = False - if self.starpilot_planner.starpilot_weather.weather_id != 0: self.t_follow = min(self.t_follow + self.starpilot_planner.starpilot_weather.increase_following_distance, MAX_T_FOLLOW) - self.disable_throttle = False - if self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.status: - lead_distance = self.starpilot_planner.lead_one.dRel - v_lead = self.starpilot_planner.lead_one.vLead - closing_speed = max(0.0, v_ego - v_lead) - desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow)) - self.disable_throttle = should_disable_far_lead_throttle(v_ego, lead_distance, desired_gap, closing_speed, self.following_lead) - if long_control_active and self.starpilot_planner.tracking_lead: - self.update_follow_values(self.starpilot_planner.lead_one.dRel, v_ego, self.starpilot_planner.lead_one.vLead, starpilot_toggles) self.desired_follow_distance = int(desired_follow_distance(v_ego, self.starpilot_planner.lead_one.vLead, self.t_follow)) else: self.desired_follow_distance = 0 - - def update_follow_values(self, lead_distance, v_ego, v_lead, starpilot_toggles): - if starpilot_toggles.conditional_slower_lead and v_lead < v_ego: - distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) - braking_offset = float(np.clip(min(v_ego - v_lead, v_lead) - COMFORT_BRAKE, 1, distance_factor)) - self.slower_lead = braking_offset > 1 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index de73c8eb6..a0146680a 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -270,7 +270,7 @@ class StarPilotVCruise: long_control_active = sm["carControl"].longActive raw_stop_seen = bool( - self.starpilot_planner.starpilot_cem.stop_light_detected + self.starpilot_planner.longitudinal_intent.stop_detected or getattr(self.starpilot_planner, "raw_model_stopped", False) or sm["starpilotCarState"].dashboardStopSign > 0 ) @@ -303,11 +303,11 @@ class StarPilotVCruise: and not stop_then_turn ) - # CEM/model path: model predicted stop within ACTIVATION_M. + # Unified model path: model predicted stop within ACTIVATION_M. # Exclude when a lead is present (raw or filtered) — the handoff_to_stopped_lead path - # in CEM can set stop_light_detected even with a lead present, which would incorrectly + # can report stop intent even with a lead present, which would incorrectly # activate Force Stop and stop the car far behind the lead instead of letting ACC handle it. - cem_path = (self.starpilot_planner.starpilot_cem.stop_light_detected + cem_path = (self.starpilot_planner.longitudinal_intent.stop_detected and controls_enabled and starpilot_toggles.force_stops and self.starpilot_planner.model_length < ACTIVATION_M and self.override_force_stop_timer <= 0 @@ -388,7 +388,7 @@ class StarPilotVCruise: elif self.standstill_force_stop_hold: self.force_stop_timer = max(self.force_stop_timer, 0.5) elif (self.forcing_stop and sm["carState"].standstill and not dash_active and - not self.starpilot_planner.starpilot_cem.stop_light_detected and not raw_model_stopped): + not self.starpilot_planner.longitudinal_intent.stop_detected and not raw_model_stopped): self.force_stop_timer = 0.0 else: self.force_stop_timer = max(self.force_stop_timer - DT_MDL * 0.25, 0.0) diff --git a/starpilot/controls/lib/unified_longitudinal_intent.py b/starpilot/controls/lib/unified_longitudinal_intent.py new file mode 100644 index 000000000..6105b8685 --- /dev/null +++ b/starpilot/controls/lib/unified_longitudinal_intent.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +import numpy as np + +from openpilot.common.constants import CV +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.realtime import DT_MDL +from openpilot.starpilot.common.experimental_state import CEStatus + + +MODEL_STOP_TIME = 7.0 +MODEL_STOP_ENTER = 0.63 +MODEL_STOP_EXIT = 0.28 +MODEL_STOP_FILTER_TIME = 0.35 +SLOW_LEAD_FILTER_TIME = 0.35 +TURN_VETO_MAX_SPEED = 15.0 * CV.MPH_TO_MS +TURN_VETO_MIN_STEERING_ANGLE = 45.0 +MAX_STOP_DETECTION_SPEED = 75.0 * CV.MPH_TO_MS + + +class UnifiedLongitudinalIntent: + """Small scene detector for continuous longitudinal planning. + + This class never selects a planner mode. It reports model stop intent and a + UI reason while the longitudinal planner continuously considers cruise, + model, curves, and both leads. + """ + + def __init__(self, starpilot_planner): + self.starpilot_planner = starpilot_planner + self.params_memory = starpilot_planner.params_memory + self.stop_filter = FirstOrderFilter(0.0, MODEL_STOP_FILTER_TIME, DT_MDL) + self.lead_filter = FirstOrderFilter(0.0, SLOW_LEAD_FILTER_TIME, DT_MDL) + self.stop_detected = False + self.status_value = CEStatus["OFF"] + self._last_status = None + + @staticmethod + def _committed_turn(v_ego, car_state, driving_in_curve): + if car_state.standstill or v_ego > TURN_VETO_MAX_SPEED: + return False + if not (car_state.leftBlinker or car_state.rightBlinker): + return False + return abs(float(car_state.steeringAngleDeg)) >= TURN_VETO_MIN_STEERING_ANGLE or driving_in_curve + + def _model_stop_candidate(self, v_ego, sm): + model = sm["modelV2"] + if bool(getattr(model.action, "shouldStop", False)): + return True + if not len(model.position.x): + return False + + model_length = max(float(model.position.x[-1]), 0.0) + end_speed = float(model.velocity.x[-1]) if len(model.velocity.x) else v_ego + stop_distance = max(v_ego * MODEL_STOP_TIME - 2.5, 0.0) + return model_length < stop_distance and end_speed < max(2.0, 0.2 * v_ego) + + @staticmethod + def _slow_lead_candidate(v_ego, sm): + candidates = [] + for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo): + if not bool(getattr(lead, "status", False)): + continue + d_rel = float(getattr(lead, "dRel", np.inf)) + v_lead = float(getattr(lead, "vLead", v_ego)) + model_prob = float(getattr(lead, "modelProb", 1.0 if getattr(lead, "radar", False) else 0.0)) + credible = bool(getattr(lead, "radar", False)) or model_prob >= 0.85 + if credible and d_rel < max(40.0, 3.0 * v_ego) and v_lead < v_ego - 0.75: + candidates.append(lead) + return bool(candidates) + + def update(self, v_ego, sm, starpilot_toggles): + car_state = sm["carState"] + force_stop = bool(getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False)) + stop_sign = bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) + traffic_mode = bool(sm["starpilotCarState"].trafficModeEnabled) + turn_veto = self._committed_turn(v_ego, car_state, self.starpilot_planner.driving_in_curve) + + model_stop = self._model_stop_candidate(v_ego, sm) + model_stop &= not traffic_mode and not turn_veto and v_ego <= MAX_STOP_DETECTION_SPEED + self.stop_filter.update(model_stop) + + if force_stop or stop_sign: + self.stop_detected = True + self.stop_filter.x = 1.0 + elif self.stop_detected: + self.stop_detected = self.stop_filter.x > MODEL_STOP_EXIT + else: + self.stop_detected = self.stop_filter.x >= MODEL_STOP_ENTER + + slow_lead = self._slow_lead_candidate(v_ego, sm) + self.lead_filter.update(slow_lead) + slow_lead = self.lead_filter.x >= MODEL_STOP_ENTER + + signal = bool(car_state.leftBlinker or car_state.rightBlinker) and v_ego < 15.0 + curve = bool(self.starpilot_planner.road_curvature_detected or self.starpilot_planner.driving_in_curve) + slc_request = bool(self.starpilot_planner.starpilot_vcruise.slc.experimental_mode) + + if self.stop_detected: + status = CEStatus["STOP_LIGHT"] + elif slow_lead: + status = CEStatus["LEAD"] + elif signal: + status = CEStatus["SIGNAL"] + elif curve: + status = CEStatus["CURVATURE"] + elif slc_request: + status = CEStatus["SPEED_LIMIT"] + elif bool(getattr(starpilot_toggles, "longitudinal_model_preference", False)): + status = CEStatus["USER_OVERRIDDEN"] + else: + status = CEStatus["OFF"] + + self.status_value = status + if status != self._last_status: + self.params_memory.put_int("CEStatus", status) + self._last_status = status diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 5a2678582..25801102c 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -6,14 +6,7 @@ from openpilot.common.params import Params from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType from openpilot.selfdrive.selfdrived.events import ET -from openpilot.starpilot.common.experimental_state import ( - CCStatus, - CEStatus, - next_manual_cc_status, - next_manual_ce_status, - sync_manual_cc_state, - sync_manual_ce_state, -) +from openpilot.starpilot.common.experimental_state import toggle_longitudinal_model_preference from openpilot.starpilot.common.favorite_slots import toggle_favorite_slot from openpilot.starpilot.common.starpilot_utilities import is_FrogsGoMoo from openpilot.starpilot.common.starpilot_variables import ERROR_LOGS_PATH, GearShifter, NON_DRIVING_GEARS @@ -104,18 +97,7 @@ class StarPilotCard: if getattr(starpilot_toggles, "safe_mode", False): return - if starpilot_toggles.conditional_experimental_mode: - current_status = self.params_memory.get_int("CEStatus", default=CEStatus["OFF"]) - override_value = next_manual_ce_status(current_status, sm["selfdriveState"].experimentalMode) - self.params_memory.put_int("CEStatus", override_value) - sync_manual_ce_state(self.params, override_value) - elif getattr(starpilot_toggles, "conditional_chill_mode", False): - current_status = self.params_memory.get_int("CCStatus", default=CCStatus["OFF"]) - override_value = next_manual_cc_status(current_status, sm["selfdriveState"].experimentalMode) - self.params_memory.put_int("CCStatus", override_value) - sync_manual_cc_state(self.params, override_value) - else: - self.params.put_bool_nonblocking("ExperimentalMode", not sm["selfdriveState"].experimentalMode) + toggle_longitudinal_model_preference(self.params, self.params_memory) def update(self, carState, starpilotCarState, sm, starpilot_toggles): self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled") diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 719a5e3dc..9440c0bfc 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -4,33 +4,23 @@ import math import time import cereal.messaging as messaging -import numpy as np from openpilot.common.constants import CV -from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET -from openpilot.selfdrive.controls.lib.lead_behavior import ( - is_radarless_matched_follow_window, - should_hold_tracked_vision_lead, - should_track_lead, -) -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature -from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD -from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode -from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode +from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents from openpilot.starpilot.controls.lib.starpilot_following import StarPilotFollowing from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise +from openpilot.starpilot.controls.lib.unified_longitudinal_intent import UnifiedLongitudinalIntent from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker -RADARLESS_TRACK_HOLD_TIME = 0.45 - def _sanitize_json_value(value): if isinstance(value, float): @@ -52,11 +42,10 @@ class StarPilotPlanner: self.params_memory = Params(memory=True) self.starpilot_acceleration = StarPilotAcceleration(self) - self.starpilot_cem = ConditionalExperimentalMode(self) - self.starpilot_ccm = ConditionalChillMode(self, self.starpilot_cem) self.starpilot_events = StarPilotEvents(self, error_log, ThemeManager) self.starpilot_following = StarPilotFollowing(self) self.starpilot_vcruise = StarPilotVCruise(self) + self.longitudinal_intent = UnifiedLongitudinalIntent(self) self.starpilot_weather = WeatherChecker(self) self.driving_in_curve = False @@ -82,7 +71,6 @@ class StarPilotPlanner: self._lane_width_counter = 0 self.lateral_acceleration = 0 self.model_length = 0 - self.lead_path_y = 0 self.road_curvature = 0 self.time_to_curve = 0 self.v_cruise = 0 @@ -91,9 +79,6 @@ class StarPilotPlanner: self.gps_location_service = get_gps_location_service(self.params) - self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL) - self.radarless_follow_hold_until = 0.0 - def shutdown(self): self.starpilot_vcruise.slc.shutdown() self.starpilot_weather.executor.shutdown(wait=False, cancel_futures=True) @@ -189,13 +174,6 @@ class StarPilotPlanner: self.CS_prev_right_blinker = CS.rightBlinker self.model_length = sm["modelV2"].position.x[-1] - model_position = sm["modelV2"].position - model_path_y = getattr(model_position, "y", []) - if len(model_path_y) == len(model_position.x): - self.lead_path_y = float(np.interp(self.lead_one.dRel, model_position.x, model_path_y)) - else: - self.lead_path_y = 0.0 - self.raw_model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop @@ -203,24 +181,11 @@ class StarPilotPlanner: self.road_curvature_detected = (1 / abs(self.road_curvature))**0.5 < v_ego > CRUISING_SPEED and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) - if not sm["carState"].standstill: - self.tracking_lead = self.update_lead_status(v_ego) + self.tracking_lead = bool(self.lead_one.status) self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles) - conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled - if conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)): - # Keep CEM's filters warm in AOL so engagement can inherit the current scene. - self.starpilot_cem.update(v_ego, sm, starpilot_toggles) - self.starpilot_ccm.experimental_mode = True - elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_chill_mode", False)): - self.starpilot_ccm.update(v_ego, v_cruise, sm, starpilot_toggles) - self.starpilot_cem.experimental_mode = False - else: - self.starpilot_ccm.experimental_mode = True - self.starpilot_cem.experimental_mode = False - self.starpilot_cem.curve_detected = False - self.starpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2) + self.longitudinal_intent.update(v_ego, sm, starpilot_toggles) self.v_cruise = self.starpilot_vcruise.update(controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles) @@ -231,52 +196,6 @@ class StarPilotPlanner: else: self.starpilot_weather.weather_id = 0 - def update_lead_status(self, v_ego): - following_lead = should_track_lead( - self.lead_one.status, - self.lead_one.dRel, - self.model_length, - STOP_DISTANCE, - v_ego, - v_lead=self.lead_one.vLead, - radar=bool(getattr(self.lead_one, "radar", False)), - ) - continuity_candidate = self.tracking_lead or self.tracking_lead_filter.x >= THRESHOLD * 0.6 - if not following_lead and continuity_candidate: - following_lead = should_hold_tracked_vision_lead( - self.lead_one.status, - self.lead_one.dRel, - self.model_length, - STOP_DISTANCE, - v_ego, - model_prob=float(getattr(self.lead_one, "modelProb", 0.0)), - y_rel=float(getattr(self.lead_one, "yRel", 0.0)), - path_y=self.lead_path_y, - radar=bool(getattr(self.lead_one, "radar", False)), - ) - now_t = time.monotonic() - lead_radar = bool(getattr(self.lead_one, "radar", False)) - t_follow = max(float(getattr(self.starpilot_following, "t_follow", 0.0)), 1.45) - matched_follow_window = self.lead_one.status and is_radarless_matched_follow_window( - v_ego, - self.lead_one.dRel, - self.lead_one.vLead, - t_follow, - radar=lead_radar, - lead_brake=max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0))), - lead_prob=float(getattr(self.lead_one, "modelProb", 0.0)), - ) - if matched_follow_window and (following_lead or self.tracking_lead or self.tracking_lead_filter.x >= THRESHOLD * 0.6): - self.radarless_follow_hold_until = now_t + RADARLESS_TRACK_HOLD_TIME - elif lead_radar or not self.lead_one.status: - self.radarless_follow_hold_until = 0.0 - - if not following_lead and matched_follow_window and now_t < self.radarless_follow_hold_until: - following_lead = True - - self.tracking_lead_filter.update(following_lead) - return self.tracking_lead_filter.x >= THRESHOLD - def publish(self, theme_updated, sm, pm, starpilot_toggles, serialized_toggles=""): starpilot_plan_send = messaging.new_message("starpilotPlan") starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"]) @@ -293,16 +212,15 @@ class StarPilotPlanner: starpilotPlan.cscTraining = self.starpilot_vcruise.csc.enable_training starpilotPlan.desiredFollowDistance = int(self.starpilot_following.desired_follow_distance) - starpilotPlan.disableThrottle = self.starpilot_following.disable_throttle + starpilotPlan.disableThrottle = False starpilotPlan.trackingLead = self.tracking_lead - conditional_experimental_mode = False - if starpilot_toggles.conditional_experimental_mode: - conditional_experimental_mode = self.starpilot_cem.experimental_mode - elif starpilot_toggles.conditional_chill_mode: - conditional_experimental_mode = self.starpilot_ccm.experimental_mode - - starpilotPlan.experimentalMode = conditional_experimental_mode or self.starpilot_vcruise.slc.experimental_mode + starpilotPlan.experimentalMode = bool( + not getattr(starpilot_toggles, "safe_mode", False) and ( + starpilot_toggles.longitudinal_model_preference or + self.longitudinal_intent.status_value != 0 + ) + ) starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length @@ -327,7 +245,7 @@ class StarPilotPlanner: starpilotPlan.maxAcceleration = float(self.starpilot_acceleration.max_accel) starpilotPlan.minAcceleration = float(self.starpilot_acceleration.min_accel) - starpilotPlan.redLight = self.starpilot_cem.stop_light_detected + starpilotPlan.redLight = self.longitudinal_intent.stop_detected starpilotPlan.roadCurvature = self.road_curvature diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 8b5ce9b49..e13c9bb20 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -70,7 +70,6 @@ def make_toggles(**overrides): "bookmark_via_cancel_long": False, "bookmark_via_cancel_very_long": False, "bookmark_via_lkas": False, - "conditional_experimental_mode": False, "experimental_mode_via_lkas": False, "force_coast_via_lkas": False, "lkas_allowed_for_aol": False, @@ -592,21 +591,20 @@ def test_pacifica_hybrid_main_aol_waits_for_set_press(monkeypatch, tmp_path): assert ret.alwaysOnLateralEnabled is False -def test_conditional_chill_wheel_override_cycles_manual_state(monkeypatch, tmp_path): +def test_wheel_override_temporarily_flips_model_preference(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0)) sm = make_sm() - toggles = make_toggles(conditional_chill_mode=True) - - sm["selfdriveState"].experimentalMode = True - card.handle_experimental_mode(sm, toggles) - assert card.params_memory.get_int("CCStatus") == spc.CCStatus["USER_CHILL"] + toggles = make_toggles() card.handle_experimental_mode(sm, toggles) - assert card.params_memory.get_int("CCStatus") == spc.CCStatus["OFF"] + assert card.params_memory.get_int("LongitudinalModelPreferenceOverride") == 1 + + card.handle_experimental_mode(sm, toggles) + assert card.params_memory.get_int("LongitudinalModelPreferenceOverride") == 0 def test_cancel_button_short_press_can_run_independent_mapping(monkeypatch, tmp_path): diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index a5b7e69cc..3c7e2454e 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -589,123 +589,29 @@ "settings_tier": "advanced" }, { - "key": "ConditionalExperimental", - "label": "Conditional Experimental Mode", - "description": "Automatically switch to \"Experimental Mode\" when set conditions are met. Allows the model to handle challenging situations with smarter decision making.", - "data_type": "bool", - "ui_type": "toggle", - "is_parent_toggle": true, - "settings_tier": "simple" - }, - { - "key": "PersistExperimentalState", - "label": "Persist Experimental State", - "description": "Keep your manual Conditional Experimental override through reboots until you manually clear it.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalExperimental", - "settings_tier": "simple" - }, - { - "key": "CESpeed", - "label": "Below", - "description": "Switch to \"Experimental Mode\" when driving below this speed without a lead to help openpilot handle low-speed situations more smoothly.", + "key": "LongitudinalModelPreference", + "label": "Longitudinal Preference", + "description": "Set-Speed First prioritizes steady cruise on open roads. Model First gives the driving model more influence. Both use the same lead and stop safety.", "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 99.0, - "step": 1.0, - "parent_key": "ConditionalExperimental", - "settings_tier": "simple" - }, - { - "key": "CESpeedLead", - "label": "Below (With Lead)", - "description": "Switch to \"Experimental Mode\" when driving below this speed with a lead.", - "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 99.0, - "step": 1.0, - "parent_key": "ConditionalExperimental", - "settings_tier": "simple" - }, - { - "key": "CECurves", - "label": "Curve Detected Ahead", - "description": "Switch to \"Experimental Mode\" when a curve is detected to allow the model to set an appropriate speed for the curve.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalExperimental", - "settings_tier": "simple" - }, - { - "key": "CEStopLights", - "label": "\\\"Detected\\\" Stop Lights/Signs", - "description": "Switch to \"Experimental Mode\" whenever the driving model \"detects\" a red light or stop sign.\n\nDisclaimer: openpilot does not explicitly detect traffic lights or stop signs. In \"Experimental Mode\", openpilot makes end-to-end driving decisions from camera input, which means it may stop even when there's no clear reason!", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalExperimental", - "settings_tier": "simple" - }, - { - "key": "CELead", - "label": "Lead Detected Ahead", - "description": "Switch to \"Experimental Mode\" when a slower or stopped vehicle is detected. Can make braking smoother and more reliable on some vehicles.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalExperimental", - "is_parent_toggle": true, - "settings_tier": "simple" - }, - { - "key": "CESlowerLead", - "label": "Slower Lead", - "description": "Switch to \"Experimental Mode\" when a slower lead vehicle is detected ahead.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "CELead", - "settings_tier": "simple" - }, - { - "key": "CEStoppedLead", - "label": "Stopped Lead", - "description": "Switch to \"Experimental Mode\" when a stopped lead vehicle is detected ahead.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "CELead", - "settings_tier": "simple" - }, - { - "key": "CEModelStopTime", - "label": "Predicted Stop In", - "description": "Switch to \"Experimental Mode\" when openpilot predicts a stop within the set time. This is usually triggered when the model \"sees\" a red light or stop sign ahead.\n\nDisclaimer: openpilot does not explicitly detect traffic lights or stop signs. In \"Experimental Mode\", openpilot makes end-to-end driving decisions from camera input, which means it may stop even when there's no clear reason!", - "data_type": "float", - "ui_type": "numeric", - "min": 0.0, - "max": 9.0, - "parent_key": "ConditionalExperimental", - "settings_tier": "simple" - }, - { - "key": "CESignalSpeed", - "label": "Turn Signal Below", - "description": "Switch to \"Experimental Mode\" when using a turn signal below the set speed to allow the model to choose an appropriate speed for smoother left and right turns.", - "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 99.0, - "step": 1.0, - "parent_key": "ConditionalExperimental", + "ui_type": "dropdown", + "options": [ + { + "value": 0, + "label": "Set-Speed First" + }, + { + "value": 1, + "label": "Model First" + } + ], "settings_tier": "simple" }, { "key": "ShowCEMStatus", - "label": "Status Widget", - "description": "Show which condition triggered \"Experimental Mode\" on the driving screen.", + "label": "Longitudinal Status", + "description": "Show which constraint is currently influencing the unified longitudinal planner.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "ConditionalExperimental", "settings_tier": "simple" }, { @@ -1860,87 +1766,6 @@ "parent_key": "SpeedLimitController", "settings_tier": "advanced" }, - { - "key": "ConditionalChill", - "label": "Conditional Chill Mode", - "description": "Keep \"Experimental Mode\" on by default, but temporarily switch to \"Chill Mode\" in simple cruising scenes where speed holding is usually better.", - "data_type": "bool", - "ui_type": "toggle", - "is_parent_toggle": true, - "settings_tier": "advanced" - }, - { - "key": "PersistChillState", - "label": "Persist Chill State", - "description": "Keep your manual Conditional Chill override through reboots until you manually clear it.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalChill", - "settings_tier": "advanced" - }, - { - "key": "CCMSpeed", - "label": "Above", - "description": "Switch to \"Chill Mode\" on open roads above this speed when no lead is detected and the car is still below the set speed.", - "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 99.0, - "step": 1.0, - "parent_key": "ConditionalChill", - "settings_tier": "advanced" - }, - { - "key": "CCMSpeedLead", - "label": "Above (With Lead)", - "description": "Switch to \"Chill Mode\" when following a stable lead above this speed.", - "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 99.0, - "step": 1.0, - "parent_key": "ConditionalChill", - "settings_tier": "advanced" - }, - { - "key": "CCMLead", - "label": "Stable Lead Ahead", - "description": "Switch to \"Chill Mode\" when following a steady, well-tracked lead vehicle at cruising speeds.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalChill", - "settings_tier": "advanced" - }, - { - "key": "CCMLaunchAssist", - "label": "Launch Assist", - "description": "Temporarily switch to \"Chill Mode\" when starting from a stop if planner is already allowing throttle. Useful if your car launches too slowly from lights or stop signs.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalChill", - "settings_tier": "advanced" - }, - { - "key": "CCMSetSpeedMargin", - "label": "Set Speed Margin", - "description": "How far below the set speed the car must be before open-road Conditional Chill can engage.", - "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 15.0, - "step": 1.0, - "parent_key": "ConditionalChill", - "settings_tier": "advanced" - }, - { - "key": "ShowCCMStatus", - "label": "Status Widget", - "description": "Show which condition triggered \"Chill Mode\" on the driving screen.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "ConditionalChill", - "settings_tier": "advanced" - }, { "key": "SLCAbbreviatedSources", "label": "Show Abbreviated Icon Sources", diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index 3fe48bf4b..704e20199 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -43,7 +43,7 @@ def test_galaxy_layout_contains_basic_mode_controls(): assert {"AlwaysOnLateral", "LaneChanges", "QOLLateral"} <= sections["Lateral (Steering)"].keys() assert { - "ConditionalExperimental", + "LongitudinalModelPreference", "CurveSpeedController", "AccelerationProfile", "DecelerationProfile", @@ -86,7 +86,7 @@ def test_requested_simple_and_advanced_settings_tiers(): assert lateral[key]["settings_tier"] == "advanced" for key in ( - "ConditionalExperimental", + "LongitudinalModelPreference", "CurveSpeedController", "LongitudinalTune", "AccelerationProfile", @@ -102,7 +102,6 @@ def test_requested_simple_and_advanced_settings_tiers(): "TacoTune", "NavLongitudinalAllowed", "SpeedLimitController", - "ConditionalChill", ): assert longitudinal[key]["settings_tier"] == "advanced" diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index aeedda037..c58580a00 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -61,7 +61,6 @@ from openpilot.starpilot.common.maps_catalog import ( schedule_label, schedule_param_value, ) -from openpilot.starpilot.common.experimental_state import sync_persist_chill_state, sync_persist_experimental_state from openpilot.starpilot.common.favorite_slots import ( FAVORITE_ACTION_OPTIONS, FAVORITE_SLOTS_PARAM, @@ -936,15 +935,7 @@ _TROUBLESHOOT_PERSONALITY_KEYS = [ ] _TROUBLESHOOT_CEM_KEYS = [ - "ConditionalExperimental", - "CESpeed", - "CESpeedLead", - "CECurves", - "CELead", - "CESlowerLead", - "CEStoppedLead", - "CEModelStopTime", - "CESignalSpeed", + "LongitudinalModelPreference", "ShowCEMStatus", ] @@ -4510,22 +4501,6 @@ def setup(app): "updated": updated, }), 200 - if key in {"ConditionalExperimental", "ConditionalChill"}: - enabled = str_val.strip() in ("1", "true", "True") - params.put_bool(key, enabled) - - updated = {key: enabled} - if enabled: - other_key = "ConditionalChill" if key == "ConditionalExperimental" else "ConditionalExperimental" - params.put_bool(other_key, False) - updated[other_key] = False - - update_starpilot_toggles() - return jsonify({ - "message": f"Parameter '{key}' updated successfully.", - "updated": updated, - }), 200 - if key == "CustomAccelProfile": enabled = str_val.strip() in ("1", "true", "True") params.put_bool(key, enabled) @@ -4545,30 +4520,6 @@ def setup(app): "updated": updated, }), 200 - if key == "PersistExperimentalState": - enabled = str_val.strip() in ("1", "true", "True") - sync_persist_experimental_state(params, params_memory, enabled) - update_starpilot_toggles() - return jsonify({ - "message": f"Parameter '{key}' updated successfully.", - "updated": { - "PersistExperimentalState": enabled, - "PersistedCEStatus": params.get_int("PersistedCEStatus", default=0), - }, - }), 200 - - if key == "PersistChillState": - enabled = str_val.strip() in ("1", "true", "True") - sync_persist_chill_state(params, params_memory, enabled) - update_starpilot_toggles() - return jsonify({ - "message": f"Parameter '{key}' updated successfully.", - "updated": { - "PersistChillState": enabled, - "PersistedCCStatus": params.get_int("PersistedCCStatus", default=0), - }, - }), 200 - if key == "IsRHD": enabled = str_val.strip() in ("1", "true", "True") params.put_bool("IsRHD", enabled) diff --git a/tools/StarPilot/generate_galaxy_layout.py b/tools/StarPilot/generate_galaxy_layout.py index 1ad9153a7..76fbc9fa2 100755 --- a/tools/StarPilot/generate_galaxy_layout.py +++ b/tools/StarPilot/generate_galaxy_layout.py @@ -25,6 +25,26 @@ DROPDOWN_MAPPING = { # Custom controls implemented outside the tuple vectors in Qt settings panels. # Inject these so regenerated galaxy layouts retain equivalent functionality. INJECTED_SECTION_PARAMS = { + "Longitudinal (Speed & Following)": [ + { + "key": "LongitudinalModelPreference", + "label": "Longitudinal Preference", + "description": "Set-Speed First prioritizes steady cruise on open roads. Model First gives the driving model more influence. Both use the same lead and stop safety.", + "data_type": "int", + "ui_type": "dropdown", + "options": [ + {"value": 0, "label": "Set-Speed First"}, + {"value": 1, "label": "Model First"}, + ], + }, + { + "key": "ShowCEMStatus", + "label": "Longitudinal Status", + "description": "Show which constraint is currently influencing the unified longitudinal planner.", + "data_type": "bool", + "ui_type": "toggle", + }, + ], "Vehicle": [ { "key": "CarMake", @@ -61,13 +81,35 @@ INJECTED_SECTION_PARAMS = { # Keys explicitly hidden from The Galaxy's generic settings UI. HIDDEN_KEYS = { + "CCMLaunchAssist", + "CCMLead", + "CCMSetSpeedMargin", + "CCMSpeed", + "CCMSpeedLead", + "CECurves", + "CECurvesLead", + "CELead", + "CEModelStopTime", + "CESignalLaneDetection", + "CESignalSpeed", + "CESlowerLead", + "CESpeed", + "CESpeedLead", + "CEStopLights", + "CEStoppedLead", + "ConditionalChill", + "ConditionalExperimental", "FrogsGoMoosTweak", "HumanAcceleration", "DisableWideRoad", "LockDoorsTimer", "NewLongAPI", - "ToyotaDoors", + "PersistChillState", + "PersistExperimentalState", "ReverseCruise", + "ShowCCMStatus", + "ShowCEMStatus", + "ToyotaDoors", } HIDDEN_SECTION_NAMES = {"Model & Customization"} @@ -168,8 +210,6 @@ PARENT_KEYS_MAPPING = { "longitudinal_settings.cc": { "advancedLongitudinalTuneKeys": "AdvancedLongitudinalTune", "aggressivePersonalityKeys": "AggressivePersonalityProfile", - "conditionalChillKeys": "ConditionalChill", - "conditionalExperimentalKeys": "ConditionalExperimental", "curveSpeedKeys": "CurveSpeedController", "customDrivingPersonalityKeys": "CustomPersonalities", "longitudinalTuneKeys": "LongitudinalTune", @@ -546,58 +586,8 @@ def parse_cpp_file(filename): if key in child_to_parent: s["parent_key"] = child_to_parent[key] if key in ALL_PARENT_KEYS: s["is_parent_toggle"] = True - if key == "CELead": - s["is_parent_toggle"] = True - items.append(s) - # Mirror CELead's split sub-toggles from StarPilotButtonToggleControl. - if key == "CELead": - items.extend([ - { - "key": "CESlowerLead", - "label": "Slower Lead", - "description": "Switch to \"Experimental Mode\" when a slower lead vehicle is detected ahead.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "CELead", - }, - { - "key": "CEStoppedLead", - "label": "Stopped Lead", - "description": "Switch to \"Experimental Mode\" when a stopped lead vehicle is detected ahead.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "CELead", - }, - ]) - - # Mirror CESpeed/CCMSpeed's dual sliders (with-lead variants) from Qt. - if key == "CESpeed": - items.append({ - "key": "CESpeedLead", - "label": "Below (With Lead)", - "description": "Switch to \"Experimental Mode\" when driving below this speed with a lead.", - "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 99.0, - "step": 1.0, - "parent_key": "ConditionalExperimental", - }) - elif key == "CCMSpeed": - items.append({ - "key": "CCMSpeedLead", - "label": "Above (With Lead)", - "description": "Switch to \"Chill Mode\" when following a stable lead above this speed.", - "data_type": "int", - "ui_type": "numeric", - "min": 0.0, - "max": 99.0, - "step": 1.0, - "parent_key": "ConditionalChill", - }) - return items