#!/usr/bin/env python3 import math import numpy as np from openpilot.common.constants import CV from openpilot.common.params import Params from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.starpilot.common.accel_profile import ( ACCELERATION_PROFILES, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_TRAFFIC_ALL, DECELERATION_PROFILES, coerce_custom_accel_profile_values, get_accel_profile_curve_values, get_max_allowed_accel as get_profile_max_allowed_accel, interpolate_accel_profile, normalize_deceleration_profile, ) from openpilot.starpilot.common.longitudinal_personality_profiles import active_personality_id, interpolate_category_curve, resolve_personality_category from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target def cubic_interp(x, xp, fp): """Cubic interpolation using NumPy's native operations for speed.""" # Boundary conditions if x <= xp[0]: return fp[0] elif x >= xp[-1]: return fp[-1] # Find interval i = np.searchsorted(xp, x) - 1 i = max(0, min(i, len(xp)-2)) # clamp the index # Normalized position t = (x - xp[i]) / float(xp[i+1] - xp[i]) # Hermite cubic formula return fp[i]*(1 - 3*t**2 + 2*t**3) + fp[i+1]*(3*t**2 - 2*t**3) def akima_interp(x, xp, fp): """Akima-inspired interpolation with reduced overshoot characteristics.""" if x <= xp[0]: return fp[0] elif x >= xp[-1]: return fp[-1] i = np.searchsorted(xp, x) - 1 i = max(0, min(i, len(xp)-2)) # clamp the index t = (x - xp[i]) / float(xp[i+1] - xp[i]) # Quintic polynomial to reduce overshoot t2 = t*t t4 = t2*t2 t3 = t2*t return (fp[i]*(1 - 10*t3 + 15*t4 - 6*t3*t2) + fp[i+1]*(10*t3 - 15*t4 + 6*t3*t2)) A_CRUISE_MIN_ECO = A_CRUISE_MIN / 2 A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2 A_CRUISE_MIN_TRAFFIC = A_CRUISE_MIN * 0.35 # cruise-decel floor only; MPC lead braking keeps full ACCEL_MIN authority SLC_COAST_WINDOW_BP = [0.0, 10.0, 20.0, 35.0] SLC_COAST_WINDOW_BASE = [0.20, 0.40, 0.65, 1.10] SLC_EXCESS_SCALE_BP = [0.0, 10.0, 20.0, 35.0] SLC_EXCESS_SCALE_V = [0.8, 1.8, 3.5, 5.5] SLC_COAST_WINDOW_MULTIPLIER = { DECELERATION_PROFILES["ECO"]: 1.20, DECELERATION_PROFILES["STANDARD"]: 1.00, DECELERATION_PROFILES["SPORT"]: 0.75, } SLC_COAST_FLOOR = { DECELERATION_PROFILES["ECO"]: -0.02, DECELERATION_PROFILES["STANDARD"]: -0.03, DECELERATION_PROFILES["SPORT"]: -0.04, } SLC_COAST_MIN_SPEED = 4.0 SLC_TARGET_EPS = 0.15 RELEVANT_LEAD_MIN_CLOSING_SPEED = 0.5 RELEVANT_LEAD_MIN_BRAKE = -0.4 PULSE_GLIDE_MIN_TARGET_SPEED = 5.0 PULSE_GLIDE_MIN_LOWER_SPEED = 3.0 PULSE_GLIDE_HYSTERESIS = 0.25 PULSE_GLIDE_COAST_MIN_ACCEL = -0.03 PULSE_GLIDE_HILL_ENTER_PITCH = math.radians(3.0) PULSE_GLIDE_HILL_EXIT_PITCH = math.radians(2.5) GEAR_STATE_PROFILES = { "eco": (ACCELERATION_PROFILES["ECO"], DECELERATION_PROFILES["ECO"]), "sport": (ACCELERATION_PROFILES["SPORT_PLUS"], DECELERATION_PROFILES["SPORT"]), "normal": (ACCELERATION_PROFILES["STANDARD"], DECELERATION_PROFILES["STANDARD"]), } PERSONALITY_DECELERATION_PROFILES = { "eco": DECELERATION_PROFILES["ECO"], "standard": DECELERATION_PROFILES["STANDARD"], "sport": DECELERATION_PROFILES["SPORT"], "custom": DECELERATION_PROFILES["STANDARD"], } def get_max_accel_eco(v_ego, ev_tuning=True, truck_tuning=False): return interpolate_accel_profile(v_ego, get_accel_profile_curve_values(ACCELERATION_PROFILES["ECO"], ev_tuning, truck_tuning)) def get_max_accel_sport(v_ego, ev_tuning=True, truck_tuning=False): return interpolate_accel_profile(v_ego, get_accel_profile_curve_values(ACCELERATION_PROFILES["SPORT"], ev_tuning, truck_tuning)) def get_max_accel_standard(v_ego, ev_tuning=True, truck_tuning=False): return interpolate_accel_profile(v_ego, get_accel_profile_curve_values(ACCELERATION_PROFILES["STANDARD"], ev_tuning, truck_tuning)) def get_max_accel_traffic(v_ego): return interpolate_accel_profile(v_ego, A_CRUISE_MAX_VALS_TRAFFIC_ALL) def get_max_accel_custom(v_ego, custom_curve, acceleration_profile, ev_tuning=True, truck_tuning=False, custom_breakpoints=None): curve_breakpoints = A_CRUISE_MAX_BP_CUSTOM if custom_breakpoints is None else custom_breakpoints curve_values = coerce_custom_accel_profile_values( custom_curve, acceleration_profile, ev_tuning, truck_tuning, point_count=len(curve_breakpoints), ) return interpolate_accel_profile(v_ego, curve_values, curve_breakpoints) def get_max_allowed_accel(v_ego, ev_tuning=True, truck_tuning=False): return float(get_profile_max_allowed_accel(v_ego, ev_tuning, truck_tuning)) def get_profile_min_accel_floor(deceleration_profile): if deceleration_profile == DECELERATION_PROFILES["ECO"]: return A_CRUISE_MIN_ECO if deceleration_profile == DECELERATION_PROFILES["SPORT"]: return A_CRUISE_MIN_SPORT return A_CRUISE_MIN def lead_is_braking_relevant(lead, v_ego): if lead is None or not getattr(lead, "status", False): return False closing_speed = float(v_ego - getattr(lead, "vLead", 0.0)) if closing_speed > RELEVANT_LEAD_MIN_CLOSING_SPEED: return True if float(getattr(lead, "aLeadK", 0.0)) < RELEVANT_LEAD_MIN_BRAKE: return True return float(getattr(lead, "dRel", 1e6)) < max(18.0, 2.0 * float(v_ego)) def get_slc_shaped_min_accel(v_ego, v_target, deceleration_profile, full_brake_floor): profile = DECELERATION_PROFILES["STANDARD"] if deceleration_profile is None else deceleration_profile coast_floor = SLC_COAST_FLOOR.get(profile, SLC_COAST_FLOOR[DECELERATION_PROFILES["STANDARD"]]) coast_window = float(akima_interp(v_ego, SLC_COAST_WINDOW_BP, SLC_COAST_WINDOW_BASE)) coast_window *= SLC_COAST_WINDOW_MULTIPLIER.get(profile, 1.0) excess_scale = float(akima_interp(v_ego, SLC_EXCESS_SCALE_BP, SLC_EXCESS_SCALE_V)) excess_scale = max(excess_scale, coast_window + 0.1) excess = max(0.0, float(v_ego) - float(v_target)) if excess <= coast_window: return coast_floor t = float(np.clip((excess - coast_window) / (excess_scale - coast_window), 0.0, 1.0)) ** 2 return coast_floor + t * (full_brake_floor - coast_floor) class StarPilotAcceleration: def __init__(self, StarPilotPlanner): self.starpilot_planner = StarPilotPlanner self.params = Params() self.params_memory = Params(memory=True) self.max_accel = 0 self.min_accel = 0 self.last_gear_state = "init" self.pulse_glide_coasting = False self.pulse_glide_target = None self.pulse_glide_hill_paused = False def _update_pulse_glide_hill_pause(self, sm): try: orientation_ned = sm["carControl"].orientationNED if len(orientation_ned) < 2: return self.pulse_glide_hill_paused abs_pitch = abs(float(orientation_ned[1])) except (KeyError, IndexError, TypeError, ValueError, AttributeError): return self.pulse_glide_hill_paused if not math.isfinite(abs_pitch): return self.pulse_glide_hill_paused if self.pulse_glide_hill_paused: if abs_pitch <= PULSE_GLIDE_HILL_EXIT_PITCH: self.pulse_glide_hill_paused = False elif abs_pitch >= PULSE_GLIDE_HILL_ENTER_PITCH: self.pulse_glide_hill_paused = True return self.pulse_glide_hill_paused def _update_pulse_glide(self, v_ego, sm, starpilot_toggles): self.pulse_glide_target = None pulse_glide_enabled = bool(getattr(sm["starpilotCarState"], "pulseAndGlide", False)) if not pulse_glide_enabled: self.pulse_glide_coasting = False self.pulse_glide_hill_paused = False return False if self._update_pulse_glide_hill_pause(sm): self.pulse_glide_coasting = False return False raw_v_cruise_kph = 0.0 if sm["carState"].vCruise == V_CRUISE_UNSET else min(sm["carState"].vCruise, V_CRUISE_MAX) if 0 < raw_v_cruise_kph < V_CRUISE_UNSET and getattr(starpilot_toggles, "set_speed_offset", 0) > 0: raw_v_cruise_kph += starpilot_toggles.set_speed_offset raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS if raw_v_cruise <= 0.0: self.pulse_glide_coasting = False return False effective_slc_target = get_active_slc_control_target( getattr(starpilot_toggles, "speed_limit_controller", False), getattr(starpilot_toggles, "set_speed_limit", False), getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), max(float(getattr(sm["carState"], "vEgoCluster", v_ego) or v_ego), v_ego) - v_ego, allow_lower_override=getattr(starpilot_toggles, "redneck_cruise", False), ) v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise) if effective_slc_target > 0.0: v_target = min(v_target, effective_slc_target) delta = max(0.0, float(getattr(starpilot_toggles, "pulse_glide_speed_delta", 0.0))) lower_target = v_target - delta if (delta <= 0.0 or v_target <= PULSE_GLIDE_MIN_TARGET_SPEED or lower_target < PULSE_GLIDE_MIN_LOWER_SPEED): self.pulse_glide_coasting = False return False has_relevant_lead = any(lead_is_braking_relevant(lead, v_ego) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo)) 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) ) if has_relevant_lead or stop_context: self.pulse_glide_coasting = False return False if self.pulse_glide_coasting: if v_ego <= lower_target + PULSE_GLIDE_HYSTERESIS: self.pulse_glide_coasting = False elif v_ego >= v_target - PULSE_GLIDE_HYSTERESIS: self.pulse_glide_coasting = True if self.pulse_glide_coasting: self.pulse_glide_target = lower_target return self.pulse_glide_coasting def _shape_min_accel_for_slc(self, v_ego, sm, starpilot_toggles, deceleration_profile, full_brake_floor): raw_v_cruise_kph = 0.0 if sm["carState"].vCruise == V_CRUISE_UNSET else min(sm["carState"].vCruise, V_CRUISE_MAX) if 0 < raw_v_cruise_kph < V_CRUISE_UNSET and getattr(starpilot_toggles, "set_speed_offset", 0) > 0: raw_v_cruise_kph += starpilot_toggles.set_speed_offset raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS v_ego_cluster = getattr(sm["carState"], "vEgoCluster", v_ego) if v_ego_cluster is None: v_ego_cluster = v_ego v_ego_cluster = max(v_ego_cluster, v_ego) v_ego_diff = v_ego_cluster - v_ego effective_slc_target = get_active_slc_control_target( getattr(starpilot_toggles, "speed_limit_controller", False), getattr(starpilot_toggles, "set_speed_limit", False), getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), v_ego_diff, allow_lower_override=getattr(starpilot_toggles, "redneck_cruise", False), ) v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise) if effective_slc_target > 0.0: v_target = min(v_target, effective_slc_target) slc_limited = effective_slc_target > 0.0 and abs(v_target - effective_slc_target) <= SLC_TARGET_EPS and effective_slc_target < raw_v_cruise - SLC_TARGET_EPS has_relevant_lead = any(lead_is_braking_relevant(lead, v_ego) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo)) 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) ) if (getattr(starpilot_toggles, "speed_limit_controller", False) and v_ego > SLC_COAST_MIN_SPEED and v_ego > v_target + 0.05 and slc_limited and not has_relevant_lead and not stop_context): return get_slc_shaped_min_accel(v_ego, v_target, deceleration_profile, full_brake_floor) return full_brake_floor def _shape_personality_min_accel_for_cruise( self, v_ego, sm, starpilot_toggles, deceleration_profile, requested_floor, baseline_floor, ): raw_v_cruise_kph = 0.0 if sm["carState"].vCruise == V_CRUISE_UNSET else min(sm["carState"].vCruise, V_CRUISE_MAX) if 0 < raw_v_cruise_kph < V_CRUISE_UNSET and getattr(starpilot_toggles, "set_speed_offset", 0) > 0: raw_v_cruise_kph += starpilot_toggles.set_speed_offset raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS v_ego_cluster = getattr(sm["carState"], "vEgoCluster", v_ego) if v_ego_cluster is None: v_ego_cluster = v_ego effective_slc_target = get_active_slc_control_target( getattr(starpilot_toggles, "speed_limit_controller", False), getattr(starpilot_toggles, "set_speed_limit", False), getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), max(v_ego_cluster, v_ego) - v_ego, allow_lower_override=getattr(starpilot_toggles, "redneck_cruise", False), ) v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise) if effective_slc_target > 0.0: v_target = min(v_target, effective_slc_target) hazard_context = ( any(getattr(lead, "status", False) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo)) or 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) ) if hazard_context or v_target <= 0.0 or v_ego <= v_target + 0.05: return baseline_floor return self._shape_min_accel_for_slc( v_ego, sm, starpilot_toggles, deceleration_profile, requested_floor ) def update(self, v_ego, sm, starpilot_toggles): eco_gear = sm["starpilotCarState"].ecoGear sport_gear = sm["starpilotCarState"].sportGear ev_tuning = getattr(starpilot_toggles, "ev_tuning", True) personality_ev_tuning = getattr(starpilot_toggles, "personality_ev_tuning", ev_tuning) truck_tuning = getattr(starpilot_toggles, "truck_tuning", False) personality_truck_tuning = ( getattr(starpilot_toggles, "personality_truck_tuning", truck_tuning) and not personality_ev_tuning ) custom_accel_profile = getattr(starpilot_toggles, "custom_accel_profile", False) custom_accel_profile_values = getattr(starpilot_toggles, "custom_accel_profile_values", []) custom_accel_profile_breakpoints = getattr(starpilot_toggles, "custom_accel_profile_breakpoints", A_CRUISE_MAX_BP_CUSTOM) deceleration_profile = normalize_deceleration_profile( getattr(starpilot_toggles, "deceleration_profile", DECELERATION_PROFILES["STANDARD"]) ) traffic_mode = sm["starpilotCarState"].trafficModeEnabled personality_document = getattr(starpilot_toggles, "longitudinal_personality_profiles", {}) personality_acceleration = None personality_braking = None personality_id = active_personality_id(traffic_mode, sm["selfdriveState"].personality) profile_enabled = personality_id is not None and getattr( starpilot_toggles, f"{personality_id}_personality_profile", True ) if getattr(starpilot_toggles, "custom_personalities", False) and profile_enabled: personality_acceleration = resolve_personality_category( personality_document, traffic_mode, sm["selfdriveState"].personality, "acceleration" ) personality_braking = resolve_personality_category( personality_document, traffic_mode, sm["selfdriveState"].personality, "braking" ) if personality_acceleration is not None and (traffic_mode or not starpilot_toggles.map_acceleration): self.max_accel = interpolate_category_curve( "acceleration", v_ego, personality_acceleration, personality_ev_tuning, personality_truck_tuning ) elif traffic_mode: self.max_accel = get_max_accel_traffic(v_ego) elif custom_accel_profile: self.max_accel = get_max_accel_custom( v_ego, custom_accel_profile_values, starpilot_toggles.acceleration_profile, ev_tuning, truck_tuning, custom_accel_profile_breakpoints, ) elif starpilot_toggles.map_acceleration: if eco_gear: self.max_accel = get_max_accel_eco(v_ego, ev_tuning, truck_tuning) elif sport_gear: self.max_accel = get_max_allowed_accel(v_ego, ev_tuning, truck_tuning) else: self.max_accel = get_max_accel_standard(v_ego, ev_tuning, truck_tuning) else: if starpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["ECO"]: self.max_accel = get_max_accel_eco(v_ego, ev_tuning, truck_tuning) elif starpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT"]: self.max_accel = get_max_accel_sport(v_ego, ev_tuning, truck_tuning) elif starpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT_PLUS"]: self.max_accel = get_max_allowed_accel(v_ego, ev_tuning, truck_tuning) else: self.max_accel = get_max_accel_standard(v_ego, ev_tuning, truck_tuning) if self.starpilot_planner.starpilot_weather.weather_id != 0: self.max_accel -= self.max_accel * self.starpilot_planner.starpilot_weather.reduce_acceleration pulse_glide_coasting = self._update_pulse_glide(v_ego, sm, starpilot_toggles) if sm["starpilotCarState"].forceCoast: self.min_accel = A_CRUISE_MIN_ECO elif pulse_glide_coasting: self.min_accel = PULSE_GLIDE_COAST_MIN_ACCEL elif personality_braking is not None and (traffic_mode or not starpilot_toggles.map_deceleration): requested_floor = -interpolate_category_curve( "braking", v_ego, personality_braking, personality_ev_tuning, personality_truck_tuning ) baseline_floor = A_CRUISE_MIN_TRAFFIC if traffic_mode else A_CRUISE_MIN profile_deceleration = PERSONALITY_DECELERATION_PROFILES.get( personality_braking.get("preset"), DECELERATION_PROFILES["STANDARD"] ) if personality_braking["preset"] != "custom" and not traffic_mode: self.min_accel = self._shape_min_accel_for_slc( v_ego, sm, starpilot_toggles, profile_deceleration, get_profile_min_accel_floor(profile_deceleration), ) else: self.min_accel = self._shape_personality_min_accel_for_cruise( v_ego, sm, starpilot_toggles, profile_deceleration, requested_floor, baseline_floor ) elif traffic_mode: self.min_accel = A_CRUISE_MIN_TRAFFIC elif starpilot_toggles.map_deceleration and (eco_gear or sport_gear): if eco_gear: self.min_accel = A_CRUISE_MIN_ECO else: self.min_accel = A_CRUISE_MIN_SPORT else: if starpilot_toggles.map_deceleration: deceleration_profile = DECELERATION_PROFILES["STANDARD"] self.min_accel = get_profile_min_accel_floor(deceleration_profile) self.min_accel = self._shape_min_accel_for_slc(v_ego, sm, starpilot_toggles, deceleration_profile, self.min_accel) gear_state = "eco" if eco_gear else ("sport" if sport_gear else "normal") mapping_enabled = starpilot_toggles.map_acceleration or starpilot_toggles.map_deceleration if gear_state != self.last_gear_state and mapping_enabled: self.last_gear_state = gear_state mapped_acceleration_profile, mapped_deceleration_profile = GEAR_STATE_PROFILES[gear_state] if starpilot_toggles.map_acceleration: self.params.put_nonblocking("AccelerationProfile", mapped_acceleration_profile) if starpilot_toggles.map_deceleration: self.params.put_nonblocking("DecelerationProfile", mapped_deceleration_profile) self.params_memory.put_bool("StarPilotTogglesUpdated", True)