diff --git a/frogpilot/controls/lib/conditional_experimental_mode.py b/frogpilot/controls/lib/conditional_experimental_mode.py index 3be9c54f..2ce9dfab 100644 --- a/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/frogpilot/controls/lib/conditional_experimental_mode.py @@ -1,13 +1,12 @@ #!/usr/bin/env python3 import time - 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.common.constants import CV -from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, THRESHOLD +from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, THRESHOLD, params_memory CEStatus = { "OFF": 0, # Off @@ -21,51 +20,78 @@ CEStatus = { "STOP_LIGHT": 8 # Stop light or sign condition } +def interp(x, xp, fp): + return float(np.interp(x, xp, fp)) + + +def scale_threshold(v_ego): + # Keep StarPilot 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_CURVES = [0.9, 0.8, 0.6, 0.5] - FILTER_TIME_LEADS = [0.9, 0.8, 0.7, 0.5] - FILTER_TIME_LIGHTS = [0.9, 0.8, 0.75, 0.55] - LIGHT_BOOSTS = [1.0, 1.2, 1.1, 1.0] - LIGHT_MAX_TIME = 9.0 + + # 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 + + # ===== 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, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner - self.curvature_filter = FirstOrderFilter(0.0, self.FILTER_TIME_CURVES[1], DT_MDL) - self.slow_lead_filter = FirstOrderFilter(0.0, self.FILTER_TIME_LEADS[1], DT_MDL) - self.stop_light_filter = FirstOrderFilter(0.0, self.FILTER_TIME_LIGHTS[1], DT_MDL) + # 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.curve_detected = False self.slow_lead_detected = False self.experimental_mode = False self.stop_light_detected = False + self.prev_experimental_mode = False # For hysteresis self.mode_hold_until = 0.0 self.mode_false_since = 0.0 - def _update_filter_time_constants(self, v_ego): - speed_mph = v_ego * CV.MS_TO_MPH - curve_time = float(np.interp(speed_mph, [0, 35, 55, 70], self.FILTER_TIME_CURVES)) - lead_time = float(np.interp(speed_mph, [0, 35, 55, 70], self.FILTER_TIME_LEADS)) - light_time = float(np.interp(speed_mph, [0, 35, 55, 70], self.FILTER_TIME_LIGHTS)) - - self.curvature_filter = FirstOrderFilter(self.curvature_filter.x, curve_time, DT_MDL) - self.slow_lead_filter = FirstOrderFilter(self.slow_lead_filter.x, lead_time, DT_MDL) - self.stop_light_filter = FirstOrderFilter(self.stop_light_filter.x, light_time, DT_MDL) - def update(self, v_ego, sm, frogpilot_toggles): now = time.monotonic() + if frogpilot_toggles.experimental_mode_via_press: - self.status_value = self.frogpilot_planner.params_memory.get_int("CEStatus") + self.status_value = params_memory.get_int("CEStatus") else: self.status_value = CEStatus["OFF"] if self.status_value not in (CEStatus["USER_DISABLED"], CEStatus["USER_OVERRIDDEN"]) and not sm["carState"].standstill: self.update_conditions(v_ego, sm, frogpilot_toggles) - triggered = self.check_conditions(v_ego, sm, frogpilot_toggles) + triggered = self.check_conditions(v_ego, sm, frogpilot_toggles) if triggered: self.mode_hold_until = now + self.CEM_TRANSITION_GUARD_TIME self.mode_false_since = 0.0 @@ -76,90 +102,113 @@ class ConditionalExperimentalMode: transition_buffer_active = self.mode_false_since != 0.0 and (now - self.mode_false_since) < self.CEM_TRANSITION_BUFFER_TIME self.experimental_mode = triggered or hold_active or transition_buffer_active - self.frogpilot_planner.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else CEStatus["OFF"]) + self.prev_experimental_mode = self.experimental_mode + params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else CEStatus["OFF"]) else: self.mode_hold_until = 0.0 self.mode_false_since = 0.0 - self.experimental_mode &= sm["carState"].standstill and self.frogpilot_planner.model_stopped - self.experimental_mode &= self.status_value != CEStatus["USER_DISABLED"] - self.experimental_mode |= self.status_value == CEStatus["USER_OVERRIDDEN"] - + self.experimental_mode = (self.status_value == CEStatus["USER_OVERRIDDEN"] or + (sm["carState"].standstill and self.experimental_mode and self.frogpilot_planner.model_stopped)) self.stop_light_detected &= self.status_value not in (CEStatus["USER_DISABLED"], CEStatus["USER_OVERRIDDEN"]) self.stop_light_filter.x = 0 def check_conditions(self, v_ego, sm, frogpilot_toggles): - if self.curve_detected and frogpilot_toggles.conditional_curves and (not self.frogpilot_planner.frogpilot_following.following_lead or frogpilot_toggles.conditional_curves_lead): + below_speed = frogpilot_toggles.conditional_limit > v_ego >= 1 and not self.frogpilot_planner.frogpilot_following.following_lead + below_speed_with_lead = frogpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.frogpilot_planner.frogpilot_following.following_lead + if below_speed or below_speed_with_lead: + self.status_value = CEStatus["SPEED"] + return True + + desired_lane = self.frogpilot_planner.lane_width_left if sm["carState"].leftBlinker else self.frogpilot_planner.lane_width_right + lane_available = desired_lane >= frogpilot_toggles.lane_detection_width or not frogpilot_toggles.conditional_signal_lane_detection + if v_ego < frogpilot_toggles.conditional_signal and (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and not lane_available: + self.status_value = CEStatus["SIGNAL"] + return True + + if frogpilot_toggles.conditional_curves and self.curve_detected and (frogpilot_toggles.conditional_curves_lead or not self.frogpilot_planner.frogpilot_following.following_lead): self.status_value = CEStatus["CURVATURE"] return True - if self.slow_lead_detected and frogpilot_toggles.conditional_lead: + if frogpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31: self.status_value = CEStatus["LEAD"] return True - if (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and v_ego < frogpilot_toggles.conditional_signal: - desired_lane = self.frogpilot_planner.lane_width_left if sm["carState"].leftBlinker else self.frogpilot_planner.lane_width_right - if desired_lane < frogpilot_toggles.lane_detection_width or not frogpilot_toggles.conditional_signal_lane_detection: - self.status_value = CEStatus["SIGNAL"] - return True - - below_speed = 1 <= v_ego < frogpilot_toggles.conditional_limit and not self.frogpilot_planner.frogpilot_following.following_lead - below_speed_with_lead = 1 <= v_ego < frogpilot_toggles.conditional_limit_lead and self.frogpilot_planner.frogpilot_following.following_lead - if below_speed or below_speed_with_lead: - self.status_value = CEStatus["SPEED"] + if frogpilot_toggles.conditional_model_stop_time != 0 and self.stop_light_detected: + self.status_value = CEStatus["STOP_LIGHT"] return True if self.frogpilot_planner.frogpilot_vcruise.slc.experimental_mode: self.status_value = CEStatus["SPEED_LIMIT"] return True - if self.stop_light_detected and frogpilot_toggles.conditional_model_stop_time != 0: - self.status_value = CEStatus["STOP_LIGHT"] - return True - return False def update_conditions(self, v_ego, sm, frogpilot_toggles): - self._update_filter_time_constants(v_ego) self.curve_detection(v_ego, frogpilot_toggles) - self.slow_lead(v_ego, frogpilot_toggles) + self.slow_lead(frogpilot_toggles, v_ego) self.stop_sign_and_light(v_ego, sm, frogpilot_toggles.conditional_model_stop_time) def curve_detection(self, v_ego, frogpilot_toggles): - self.curvature_filter.update(self.frogpilot_planner.driving_in_curve or self.frogpilot_planner.road_curvature_detected) - self.curve_detected = self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED + self.curvature_filter.update(self.frogpilot_planner.road_curvature_detected or self.frogpilot_planner.driving_in_curve) + self.curve_detected = bool(self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED) - def slow_lead(self, v_ego, frogpilot_toggles): + def slow_lead(self, frogpilot_toggles, v_ego): if self.frogpilot_planner.tracking_lead: - slower_lead = (v_ego - self.frogpilot_planner.lead_one.vLead) > CRUISING_SPEED and frogpilot_toggles.conditional_slower_lead - slower_lead |= getattr(self.frogpilot_planner.frogpilot_following, "slower_lead", False) and frogpilot_toggles.conditional_slower_lead - stopped_lead = self.frogpilot_planner.lead_one.vLead < 1 and frogpilot_toggles.conditional_stopped_lead + slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead + stopped_lead = frogpilot_toggles.conditional_stopped_lead and self.frogpilot_planner.lead_one.vLead < 1 + lead_threshold = scale_threshold(v_ego) + + # Adjust threshold based on lead probability for vision-only accuracy + lead_prob = getattr(self.frogpilot_planner.lead_one, 'modelProb', 1.0) + adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence self.slow_lead_filter.update(slower_lead or stopped_lead) - lead_prob = getattr(self.frogpilot_planner.lead_one, 'modelProb', 1.0) - adjusted_threshold = THRESHOLD * (1.0 + 0.2 * (1.0 - lead_prob)) - self.slow_lead_detected = self.slow_lead_filter.x >= adjusted_threshold + self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold) else: self.slow_lead_filter.x = 0 self.slow_lead_detected = False def stop_sign_and_light(self, v_ego, sm, model_time): - if sm["frogpilotCarState"].trafficModeEnabled: + if not sm["frogpilotCarState"].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, [low_filter_time, low_filter_time, tuned_filter_time_lights]) + 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 = FirstOrderFilter(self.curvature_filter.x, filter_time_curves, DT_MDL) + self.slow_lead_filter = FirstOrderFilter(self.slow_lead_filter.x, filter_time_leads, DT_MDL) + self.stop_light_filter = FirstOrderFilter(self.stop_light_filter.x, filter_time_lights, DT_MDL) + + # Disable stoplight detection at very high speeds to prevent false positives + if speed_mph > 75: # Disable above 75 mph + self.stop_light_filter.x = 0 + self.stop_light_detected = False + 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 + + model_stopping = self.frogpilot_planner.model_length < v_ego * adjusted_model_time + + self.stop_light_filter.update(self.frogpilot_planner.model_stopped or model_stopping) + self.stop_light_detected = bool(self.stop_light_filter.x >= THRESHOLD**2 and not self.frogpilot_planner.tracking_lead) + else: self.stop_light_filter.x = 0 self.stop_light_detected = False - return - - speed_mph = v_ego * CV.MS_TO_MPH - if speed_mph > 75: - self.stop_light_filter.x = 0 - self.stop_light_detected = False - return - - light_boost = float(np.interp(speed_mph, [0, 35, 55, 70], self.LIGHT_BOOSTS)) - cap_factor = float(np.interp(speed_mph, [0, 35, 45], [0.0, 0.0, 1.0])) - 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.0 - cap_factor)) - - model_stopping = self.frogpilot_planner.model_length < v_ego * adjusted_model_time - self.stop_light_filter.update(self.frogpilot_planner.model_stopped or model_stopping) - self.stop_light_detected = self.stop_light_filter.x >= (THRESHOLD ** 2) and not self.frogpilot_planner.tracking_lead diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 301f3bef..cd7e0d2d 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -1,49 +1,58 @@ #!/usr/bin/env python3 import numpy as np -from openpilot.selfdrive.controls.lib.longitudinal_planner import ACCEL_MIN, get_max_accel +from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT def cubic_interp(x, xp, fp): - if x <= xp[0]: - return fp[0] - elif x >= xp[-1]: - return fp[-1] + """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] - i = np.searchsorted(xp, x) - 1 - i = max(0, min(i, len(xp) - 2)) - t = (x - xp[i]) / float(xp[i + 1] - xp[i]) + # Find interval + i = np.searchsorted(xp, x) - 1 + i = max(0, min(i, len(xp)-2)) # clamp the index - return fp[i] * (1 - 3 * t ** 2 + 2 * t ** 3) + fp[i + 1] * (3 * t ** 2 - 2 * t ** 3) + # 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): - if x <= xp[0]: - return fp[0] - elif x >= xp[-1]: - return fp[-1] + """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)) - t = (x - xp[i]) / float(xp[i + 1] - xp[i]) + i = np.searchsorted(xp, x) - 1 + i = max(0, min(i, len(xp)-2)) # clamp the index - t2 = t * t - t3 = t2 * t - t4 = t2 * t2 - return (fp[i] * (1 - 10 * t3 + 15 * t4 - 6 * t3 * t2) - + fp[i + 1] * (10 * t3 - 15 * t4 + 6 * t3 * t2)) + t = (x - xp[i]) / float(xp[i+1] - xp[i]) -A_CRUISE_MIN_ECO = ACCEL_MIN / 2 -A_CRUISE_MIN_SPORT = ACCEL_MIN * 2 + # 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 # MPH = [0.0, 11, 22, 34, 45, 56, 89] -A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] +A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] A_CRUISE_MAX_VALS_ECO_EV = [1.15, 1.15, 1.15, 1.15, 1.30, 1.30, 1.72] A_CRUISE_MAX_VALS_STANDARD_EV = [1.25, 1.25, 1.25, 1.25, 1.45, 1.50, 2.00] A_CRUISE_MAX_VALS_SPORT_EV = [1.35, 1.35, 1.35, 1.35, 1.60, 1.60, 2.10] A_CRUISE_MAX_VALS_SPORT_PLUS_EV = [1.55, 1.55, 1.55, 1.55, 1.84, 1.84, 2.42] -A_CRUISE_MAX_VALS_ECO_GAS = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] -A_CRUISE_MAX_VALS_SPORT_GAS = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] +A_CRUISE_MAX_VALS_ECO_GAS = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] +A_CRUISE_MAX_VALS_SPORT_GAS = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] A_CRUISE_MAX_VALS_ECO_TRUCK = [3.00, 1.05, 0.60, 0.50, 0.50, 0.45, 0.35] A_CRUISE_MAX_VALS_STANDARD_TRUCK = [6.00, 1.10, 0.70, 0.60, 0.55, 0.45, 0.35] A_CRUISE_MAX_VALS_SPORT_TRUCK = [6.00, 1.15, 0.75, 0.70, 0.60, 0.50, 0.40] @@ -153,4 +162,4 @@ class FrogPilotAcceleration: elif frogpilot_toggles.deceleration_profile == DECELERATION_PROFILES["SPORT"]: self.min_accel = A_CRUISE_MIN_SPORT else: - self.min_accel = ACCEL_MIN + self.min_accel = A_CRUISE_MIN diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 8000fb32..c15d9f66 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,20 +1,31 @@ -import numpy as np from cereal import car +import numpy as np from openpilot.common.realtime import DT_CTRL -from openpilot.common.filter_simple import FirstOrderFilter 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 opendbc.car.gm.values import CarControllerParams, GMFlags CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] +clip = np.clip +interp = np.interp LongCtrlState = car.CarControl.Actuators.LongControlState +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < -deadzone: + error += deadzone + else: + error = 0.0 + return error + def long_control_state_trans(CP, active, long_control_state, v_ego, should_stop, brake_pressed, cruise_standstill, frogpilot_toggles): - # Ignore cruise standstill if car has a gas interceptor. + # Ignore cruise standstill if car has a gas interceptor cruise_standstill = cruise_standstill and not CP.enableGasInterceptorDEPRECATED stopping_condition = should_stop starting_condition = (not should_stop and @@ -48,6 +59,45 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, long_control_state = LongCtrlState.pid return long_control_state +def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_target, + v_target_1sec, brake_pressed, cruise_standstill, frogpilot_toggles): + accelerating = v_target_1sec > v_target + planned_stop = (v_target < frogpilot_toggles.vEgoStopping and + v_target_1sec < frogpilot_toggles.vEgoStopping and + not accelerating) + stay_stopped = (v_ego < frogpilot_toggles.vEgoStopping and + (brake_pressed or cruise_standstill)) + stopping_condition = planned_stop or stay_stopped + + starting_condition = (v_target_1sec > frogpilot_toggles.vEgoStarting and + accelerating and + not cruise_standstill and + not brake_pressed) + started_condition = v_ego > frogpilot_toggles.vEgoStarting + + if not active: + long_control_state = LongCtrlState.off + + else: + if long_control_state in (LongCtrlState.off, LongCtrlState.pid): + long_control_state = LongCtrlState.pid + if stopping_condition: + long_control_state = LongCtrlState.stopping + + elif long_control_state == LongCtrlState.stopping: + if starting_condition and CP.startingState: + long_control_state = LongCtrlState.starting + elif starting_condition: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.starting: + if stopping_condition: + long_control_state = LongCtrlState.stopping + elif started_condition: + long_control_state = LongCtrlState.pid + + return long_control_state + class LongControl: def __init__(self, CP): self.CP = CP @@ -56,9 +106,10 @@ class LongControl: self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=1 / DT_CTRL) - # Preserve legacy behavior when no feedforward gain is provided (default of 0.0). + # Preserve legacy behaviour when no feedforward gain is provided (default of 0.0) 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.last_a_target = 0.0 @@ -67,8 +118,9 @@ class LongControl: CP.brand == "gm" and CP.enableGasInterceptorDEPRECATED and (CP.flags & GMFlags.PEDAL_LONG.value) ) - def update_mpc_mode(self, experimental_mode: bool): + 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 @@ -77,6 +129,7 @@ class LongControl: 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: @@ -103,9 +156,9 @@ class LongControl: self.integrator_hold_frames = 0 return False - handoff_threshold = np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.35, 0.45, 0.55, 0.70]) + handoff_threshold = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.35, 0.45, 0.55, 0.70]) if abs(a_target - self.last_a_target) > handoff_threshold: - hold_frames = int(round(np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [25.0, 20.0, 14.0, 10.0]))) + hold_frames = int(round(interp(v_ego, [0.0, 4.0, 12.0, 25.0], [25.0, 20.0, 14.0, 10.0]))) self.integrator_hold_frames = max(self.integrator_hold_frames, hold_frames) self.last_a_target = a_target @@ -146,18 +199,19 @@ class LongControl: else: # LongCtrlState.pid error = a_target - CS.aEgo self.update_mpc_mode(self.experimental_mode) + feedforward = a_target * self.feedforward_gain freeze_integrator = self._get_pedal_long_freeze(a_target, error, CS.vEgo, accel_limits) - raw_output_accel = self.pid.update(error, speed=CS.vEgo, - feedforward=a_target * self.feedforward_gain, + raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, freeze_integrator=freeze_integrator) + 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. - urgency_denom = max(1e-3, abs(CarControllerParams.ACCEL_MIN)) - urgency = abs(raw_output_accel / urgency_denom) - urgency_smooth = min(1.0, urgency ** 0.4) + # Soften transition at low urgency, but keep sharp for high decel + # 20% smoother for chill decel (lower exponent) + urgency = abs(raw_output_accel / CarControllerParams.ACCEL_MIN) + 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: @@ -165,5 +219,72 @@ class LongControl: else: output_accel = raw_output_accel - self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1]) + self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) + return self.last_output_accel + + def reset_old_long(self, v_pid): + """Reset PID controller and change setpoint""" + self.pid.reset() + self.v_pid = v_pid + self.last_a_target = 0.0 + self.integrator_hold_frames = 0 + + def update_old_long(self, active, CS, long_plan, accel_limits, t_since_plan, frogpilot_toggles): + """Update longitudinal control. This updates the state machine and runs a PID loop""" + # Interp control trajectory + speeds = long_plan.speeds + if len(speeds) == CONTROL_N: + v_target_now = interp(t_since_plan, CONTROL_N_T_IDX, speeds) + a_target_now = interp(t_since_plan, CONTROL_N_T_IDX, long_plan.accels) + + v_target = interp(frogpilot_toggles.longitudinalActuatorDelay + t_since_plan, CONTROL_N_T_IDX, speeds) + a_target = 2 * (v_target - v_target_now) / frogpilot_toggles.longitudinalActuatorDelay - a_target_now + + v_target_1sec = interp(frogpilot_toggles.longitudinalActuatorDelay + t_since_plan + 1.0, CONTROL_N_T_IDX, speeds) + else: + v_target = 0.0 + v_target_now = 0.0 + v_target_1sec = 0.0 + a_target = 0.0 + + self.pid.neg_limit = accel_limits[0] + self.pid.pos_limit = accel_limits[1] + + output_accel = self.last_output_accel + self.long_control_state = long_control_state_trans_old_long(self.CP, active, self.long_control_state, CS.vEgo, + v_target, v_target_1sec, CS.brakePressed, + CS.cruiseState.standstill, frogpilot_toggles) + + if self.long_control_state == LongCtrlState.off: + self.reset_old_long(CS.vEgo) + output_accel = 0. + + elif self.long_control_state == LongCtrlState.stopping: + if output_accel > frogpilot_toggles.stopAccel: + output_accel = min(output_accel, 0.0) + output_accel -= frogpilot_toggles.stoppingDecelRate * DT_CTRL + self.reset_old_long(CS.vEgo) + + elif self.long_control_state == LongCtrlState.starting: + output_accel = frogpilot_toggles.startAccel + self.reset_old_long(CS.vEgo) + + elif self.long_control_state == LongCtrlState.pid: + self.v_pid = v_target_now + + # Toyota starts braking more when it thinks you want to stop + # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration + # TODO too complex, needs to be simplified and tested on toyotas + prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid + deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) + error = self.v_pid - CS.vEgo + error_deadzone = apply_deadzone(error, deadzone) + freeze_integrator = prevent_overshoot or self._get_pedal_long_freeze(a_target, error_deadzone, CS.vEgo, accel_limits) + feedforward = a_target * self.feedforward_gain + output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, + feedforward=feedforward, + freeze_integrator=freeze_integrator) + + 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 index ec00204e..ec01cf73 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -16,9 +16,6 @@ from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function -# Keep in sync with selfdrive/controls/radard.py -_LEAD_ACCEL_TAU = 1.5 - if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: @@ -41,29 +38,60 @@ 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 = [0, 35, 55, 70] +# 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 + +# DISTANCE ADAPTATION STRENGTH (How much penalties increase when close to lead) # [City, Urban Hwy, Rural Hwy, High Speed] -X_EGO_OBSTACLE_COSTS = [3.0, 3.0, 2.5, 2.0] -J_EGO_COSTS = [5.0, 4.75, 4.5, 4.0] -A_CHANGE_COSTS = [200.0, 195.0, 180.0, 170.0] -LEAD_FILTER_TIME_LOW = 0.8 -LEAD_FILTER_TIME_HIGH = 1.2 -DIST_ADAPTS = [0.04, 0.06, 0.06, 0.05] +DIST_ADAPTS = [0.04, 0.06, 0.06, 0.05] # Balanced across speeds -# Current active values +# ===== END TUNING PARAMETERS ===== + +# 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. -J_EGO_COST = 5.5 -A_CHANGE_COST = 250.0 DANGER_ZONE_COST = 100. CRASH_DISTANCE = .25 LEAD_DANGER_FACTOR = 0.75 LIMIT_COST = 1e6 ACADOS_SOLVER_TYPE = 'SQP_RTI' +# Default lead acceleration decay set to 50% at 1s +LEAD_ACCEL_TAU = 1.5 # Fewer timestamps don't hurt performance and lead to @@ -80,11 +108,6 @@ STOP_DISTANCE = 6.0 CRUISE_MIN_ACCEL = -1.2 CRUISE_MAX_ACCEL = 1.6 - -def get_speed_based_param(speed_mph, param_array): - return float(np.interp(speed_mph, SPEED_BREAKPOINTS, param_array)) - - 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, @@ -132,11 +155,9 @@ def get_T_FOLLOW(aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1. def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) - def get_safe_obstacle_distance(v_ego, t_follow, stop_distance=STOP_DISTANCE): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance - def desired_follow_distance(v_ego, v_lead, t_follow=None, stop_distance=STOP_DISTANCE): if t_follow is None: t_follow = get_T_FOLLOW() @@ -216,11 +237,12 @@ def gen_long_ocp(): # 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, - a_ego - prev_a, + accel_change, j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) @@ -279,24 +301,24 @@ class LongitudinalMpc: 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.prev_filter_time = self.current_filter_time 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) + # 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.stop_distance = STOP_DISTANCE - self.reset() def reset(self): @@ -349,29 +371,40 @@ class LongitudinalMpc: uncertainty=0.0, accel_reengage=False, panic_bypass=False, stop_distance=STOP_DISTANCE): self.stop_distance = max(float(stop_distance), 4.0) - speed_mph = v_ego * CV.MS_TO_MPH + # Update parameters based on current speed with interpolation for smooth scaling + speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph + + # 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) + # 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 + self.current_filter_time = 0.0 else: - self.current_filter_time = float(np.interp(speed_mph, [47, 65], [0.0, LEAD_FILTER_TIME_HIGH])) - - if abs(self.current_filter_time - self.prev_filter_time) > 0.1: - self.lead_a_filter = FirstOrderFilter(self.lead_a_filter.x, self.current_filter_time, self.dt) - self.lead_v_filter = FirstOrderFilter(self.lead_v_filter.x, self.current_filter_time, self.dt) + 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 + prev_filter_time_factor = getattr(self, 'prev_filter_time_factor', 1.0) + # Target factor from uncertainty if uncertainty <= 0.45: tgt_factor = 1.0 elif uncertainty >= 0.70: @@ -381,15 +414,21 @@ class LongitudinalMpc: 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 + # Slew-limit changes to avoid step-wise filter jumps 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) + filter_time_factor = float(self.filter_time_factor) + # When uncertainty is moderately elevated, allow accel but cap jerk by increasing jerk cost if 0.45 <= uncertainty < 0.60: - speed_jerk *= float(np.interp(uncertainty, [0.45, 0.60], [1.2, 1.5])) + 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 @@ -403,11 +442,14 @@ class LongitudinalMpc: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) - if abs(self.filter_time_factor - self.prev_filter_time_factor) > 0.05: - new_filter_time = self.current_filter_time * self.filter_time_factor - self.lead_a_filter = FirstOrderFilter(self.lead_a_filter.x, new_filter_time, self.dt) - self.lead_v_filter = FirstOrderFilter(self.lead_v_filter.x, new_filter_time, self.dt) - self.prev_filter_time_factor = self.filter_time_factor + # Adjust filter time constants for complex scenes + if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.05: + 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 + new_filter_time = self.current_filter_time * filter_time_factor + 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] @@ -420,9 +462,11 @@ class LongitudinalMpc: @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 - exp_weight = float(np.interp(speed_mph, [0, 20, 35], [1.0, 1.0, 0.0])) + 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 - if exp_weight > 0.0: + 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) @@ -430,11 +474,13 @@ class LongitudinalMpc: x_lead_traj_exp = np.zeros_like(T_IDXS) v_lead_traj_exp = np.zeros_like(T_IDXS) + # 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 - v_lead_traj = exp_weight * v_lead_traj_exp + (1.0 - exp_weight) * v_lead_traj_const - x_lead_traj = exp_weight * x_lead_traj_exp + (1.0 - exp_weight) * x_lead_traj_const + # 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 @@ -447,10 +493,11 @@ class LongitudinalMpc: a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau else: + # Fake a fast lead car, so mpc can keep running in the same mode x_lead = 50.0 v_lead = v_ego + 10.0 a_lead = 0.0 - a_lead_tau = _LEAD_ACCEL_TAU + a_lead_tau = LEAD_ACCEL_TAU # MPC will not converge if immediate crash is expected # Clip lead distance to what is still possible to brake for @@ -458,13 +505,17 @@ class LongitudinalMpc: 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.) - + # Apply smoothing filters with interp scaling self.lead_a_filter.update(a_lead) self.lead_v_filter.update(v_lead) - lead_xv = self.extrapolate_lead(x_lead, self.lead_v_filter.x, self.lead_a_filter.x, a_lead_tau, v_ego) + a_lead = self.lead_a_filter.x + v_lead = self.lead_v_filter.x + lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego) return lead_xv 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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9d44a0f3..301005ee 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import math -import time import numpy as np +import time import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX @@ -12,13 +12,14 @@ from openpilot.selfdrive.modeld.constants import ModelConstants 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 T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan +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.frogpilot.common.frogpilot_variables import MINIMUM_LATERAL_ACCELERATION 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] @@ -59,6 +60,44 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] +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) + + 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) + 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 + + class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP @@ -67,10 +106,12 @@ class LongitudinalPlanner: self.dt = dt self.allow_throttle = True self.mode = 'acc' + self.generation = None self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) + self.v_model_error = 0.0 self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] self.output_a_target = 0.0 self.output_should_stop = False @@ -80,12 +121,18 @@ class LongitudinalPlanner: self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - self.uncert_slow = FirstOrderFilter(0.0, 1.6, self.dt) - self.uncert_fast = FirstOrderFilter(0.0, 0.9, self.dt) + # ---- 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 @@ -93,6 +140,11 @@ class LongitudinalPlanner: def mlsim(self): return self.generation in ("v8", "v10", "v11", "v12") + 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): if len(model_msg.temporalPose.trans): @@ -115,18 +167,17 @@ class LongitudinalPlanner: v = np.zeros(len(T_IDXS_MPC)) a = np.zeros(len(T_IDXS_MPC)) j = np.zeros(len(T_IDXS_MPC)) - if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1: - throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] - else: - throttle_prob = 1.0 - # FrogPilot variables if frogpilot_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 def update(self, sm, frogpilot_toggles): @@ -157,57 +208,77 @@ class LongitudinalPlanner: prev_accel_constraint = not (reset_state or sm['carState'].standstill) if self.mpc.mode == 'acc': - accel_clip = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] + accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - if not sm['frogpilotPlan'].cscControllingSpeed: - accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) + accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) else: - accel_clip = [ACCEL_MIN, ACCEL_MAX] + accel_limits = [ACCEL_MIN, ACCEL_MAX] + accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] 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_clip[0], accel_clip[1]) + self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - model_error = self.get_model_speed_error(sm['modelV2'], v_ego) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], model_error, v_ego, frogpilot_toggles) + # 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, frogpilot_toggles) # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED self.allow_throttle &= not sm['frogpilotPlan'].disableThrottle if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_clip[0]) - clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) - accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) + clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) + clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) + accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) if force_slow_decel: 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) - accel_clip[0] = min(accel_clip[0], self.a_desired + 0.05) - accel_clip[1] = max(accel_clip[1], self.a_desired - 0.05) + self.lead_one = sm['radarState'].leadOne + self.lead_two = sm['radarState'].leadTwo - lead_one = sm['radarState'].leadOne - lead_dist = lead_one.dRel if lead_one.status else 50.0 + lead_dist = self.lead_one.dRel if self.lead_one.status 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() - v_rel = (v_ego - lead_one.vLead) if lead_one.status else 0.0 + # relative speed (ego - lead) positive when closing + v_rel = (v_ego - self.lead_one.vLead) if self.lead_one.status 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 = ( + self.lead_one.status and + abs(v_rel) < 0.5 and + abs(d_rel_dot) < 0.5 and + not recently_braked + ) + + # Calculate scene uncertainty from model desire prediction entropy and disengage predictions uncertainty = 0.0 - raw_brake_max = 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 @@ -218,37 +289,40 @@ class LongitudinalPlanner: 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)) + 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 + 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) - if raw_brake_max > 0.02: - self.last_big_brake_t = now_t - - recently_braked = (now_t - self.last_big_brake_t) < 0.7 - self.stable_lead = ( - lead_one.status and - abs(v_rel) < 0.5 and - abs(d_rel_dot) < 0.5 and - not recently_braked - ) - + # --- Slope-based panic bypass --- if self._uncert_last_t is None: uncert_slope = 0.0 else: @@ -257,10 +331,15 @@ class LongitudinalPlanner: self._uncert_last = uncertainty self._uncert_last_t = now_t - closing_fast = lead_one.status and (v_ego - lead_one.vLead) > 0.5 + closing_fast = (self.lead_one.status and (v_ego - self.lead_one.vLead) > 0.5) + # Trigger if either slope is high or magnitude is high; require a valid lead and closing panic_bypass = closing_fast and (uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG) + if panic_bypass: - cloudlog.error(f"LON_SLOPE slope={uncert_slope:.3f} uncertainty={uncertainty:.3f} v_ego={v_ego:.2f}") + try: + cloudlog.error(f"LON_SLOPE; slope={uncert_slope:.3f}/s; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f}; v_rel={(v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0:.2f}; lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}; trigger=True") + except Exception: + pass self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, @@ -272,10 +351,12 @@ class LongitudinalPlanner: uncertainty=uncertainty, panic_bypass=panic_bypass, stop_distance=getattr(frogpilot_toggles, "stop_distance", 6.0)) - self.mpc.set_accel_limits(accel_clip[0], accel_clip[1]) + 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) - # Keep StarPilot behavior as the primary gate: desired follow distance implies - # lead tracking in ACC mode. trackingLead is treated as an additive hint. + # 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 tracking_lead = sm['frogpilotPlan'].desiredFollowDistance > 0 if hasattr(sm['frogpilotPlan'], 'trackingLead'): tracking_lead = tracking_lead or bool(sm['frogpilotPlan'].trackingLead) @@ -283,6 +364,7 @@ class LongitudinalPlanner: sm['frogpilotPlan'].dangerFactor, sm['frogpilotPlan'].tFollow, personality=sm['selfdriveState'].personality, tracking_lead=tracking_lead) + 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) @@ -292,31 +374,42 @@ class LongitudinalPlanner: 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 - if lead_one.status: - rel_v = max(0.0, v_ego - lead_one.vLead) + # Anticipatory pre-brake to avoid "coming in hot" when closing on a lead + if self.lead_one.status: + rel_v = max(0.0, v_ego - self.lead_one.vLead) + # dynamic time headway adds a small buffer when uncertainty is elevated base_th = 1.6 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: + 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) - self.a_desired = float(self.a_desired - min(pre_brake, 0.06)) + 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 action_t = frogpilot_toggles.longitudinalActuatorDelay + DT_MDL - output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping) + 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=frogpilot_toggles.vEgoStopping) output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - # Keep StarPilot behavior: for tinygrad v10/v11/v12 in experimental mode, blend with model action output. if self.mode == 'acc' or self.generation == 'v9': output_a_target = output_a_target_mpc self.output_should_stop = output_should_stop_mpc @@ -325,9 +418,9 @@ class LongitudinalPlanner: self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc for idx in range(2): - accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) - self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) - self.prev_accel_clip = accel_clip + accel_limits_turns[idx] = np.clip(accel_limits_turns[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) + self.output_a_target = np.clip(output_a_target, accel_limits_turns[0], accel_limits_turns[1]) + self.prev_accel_clip = accel_limits_turns def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan')