mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Long changes

Browse Source
This commit is contained in:
firestar5683
2026-03-25 12:09:35 -05:00
parent 187eb34733
commit 00b8eb2b20
5 changed files with 535 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/controls/lib/conditional_experimental_mode.py
+122 -73
View File
@@ -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
frogpilot/controls/lib/frogpilot_acceleration.py
+36 -27
View File
@@ -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