diff --git a/frogpilot/assets/other_images/frogpilot_boot_logo.jpg b/frogpilot/assets/other_images/frogpilot_boot_logo.jpg new file mode 100644 index 000000000..e88547c36 Binary files /dev/null and b/frogpilot/assets/other_images/frogpilot_boot_logo.jpg differ diff --git a/frogpilot/assets/other_images/frogpilot_boot_logo.png b/frogpilot/assets/other_images/frogpilot_boot_logo.png index 2505b8cb5..f502d99f4 100644 Binary files a/frogpilot/assets/other_images/frogpilot_boot_logo.png and b/frogpilot/assets/other_images/frogpilot_boot_logo.png differ diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index c66269283..2f00a81d6 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -373,6 +373,10 @@ misc_tuning_levels: list[tuple[str, str | bytes, int]] = [ ("WheelControls", "", 2) ] + +def scale_threshold(v_ego): + return 0.0 if v_ego > 31.3 else np.interp(v_ego, [0, 17.9, 26.8, 35.8, 44.7], [0.63, 0.63, 0.65, 0.95, 0.95]) + class FrogPilotVariables: def __init__(self): self.frogpilot_toggles = get_frogpilot_toggles(block=False) diff --git a/frogpilot/controls/lib/conditional_experimental_mode.py b/frogpilot/controls/lib/conditional_experimental_mode.py index 626b263a0..f221510e6 100644 --- a/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/frogpilot/controls/lib/conditional_experimental_mode.py @@ -2,7 +2,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL -from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory +from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory, scale_threshold class ConditionalExperimentalMode: def __init__(self, FrogPilotPlanner): @@ -53,7 +53,7 @@ class ConditionalExperimentalMode: self.status_value = 8 return True - if frogpilot_toggles.conditional_lead and self.slow_lead_detected: + if frogpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 29.1: self.status_value = 9 if self.frogpilot_planner.lead_one.vLead < 1 else 10 return True @@ -69,7 +69,7 @@ class ConditionalExperimentalMode: def update_conditions(self, frogpilotCarState, v_ego, frogpilot_toggles): self.curve_detection(v_ego, frogpilot_toggles) - self.slow_lead(frogpilot_toggles) + self.slow_lead(frogpilot_toggles, v_ego) self.stop_sign_and_light(frogpilotCarState, v_ego, frogpilot_toggles) def curve_detection(self, v_ego, frogpilot_toggles): @@ -78,13 +78,14 @@ class ConditionalExperimentalMode: self.curvature_filter.update(self.frogpilot_planner.road_curvature_detected or curve_active) self.curve_detected = self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED - def slow_lead(self, frogpilot_toggles): + def slow_lead(self, frogpilot_toggles, v_ego): + v_lead = self.frogpilot_planner.lead_one.vLead if self.frogpilot_planner.tracking_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 - + stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1 + lead_threshold = scale_threshold(v_ego) self.slow_lead_filter.update(slower_lead or stopped_lead) - self.slow_lead_detected = self.slow_lead_filter.x >= THRESHOLD + self.slow_lead_detected = self.slow_lead_filter.x >= lead_threshold else: self.slow_lead_filter.x = 0 self.slow_lead_detected = False diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 0726242d3..2170605e7 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -1,6 +1,43 @@ #!/usr/bin/env python3 import numpy as np +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)) + from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT @@ -11,26 +48,26 @@ 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_VALS_ECO = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] -A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] -A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] +A_CRUISE_MAX_VALS_SPORT = [1.5, 1.5, 1.25, 1.5, 1.5, 1.5, 2.0] +A_CRUISE_MAX_VALS_SPORT_PLUS = [2.5, 2.5, 3.0, 2.5, 2.5, 2.5, 2.5] def get_max_accel_eco(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)) def get_max_accel_sport(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)) def get_max_accel_sport_plus(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)) def get_max_accel_low_speeds(max_accel, v_cruise): - return float(np.interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])) + return float(akima_interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])) def get_max_accel_ramp_off(max_accel, v_cruise, v_ego): - return float(np.interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel])) + return float(akima_interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel])) def get_max_allowed_accel(v_ego): - return float(np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])) # ISO 15622:2018 + return float(akima_interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])) # ISO 15622:2018 class FrogPilotAcceleration: def __init__(self, FrogPilotPlanner): diff --git a/frogpilot/system/fleetmanager/static/frog.png b/frogpilot/system/fleetmanager/static/frog.png index 5285f0be6..1b3bc866e 100644 Binary files a/frogpilot/system/fleetmanager/static/frog.png and b/frogpilot/system/fleetmanager/static/frog.png differ diff --git a/frogpilot/system/fleetmanager/templates/layout.html b/frogpilot/system/fleetmanager/templates/layout.html index 21d1e487e..a4ddee271 100644 --- a/frogpilot/system/fleetmanager/templates/layout.html +++ b/frogpilot/system/fleetmanager/templates/layout.html @@ -40,7 +40,7 @@ text-align: center; } - FrogPilot: {% block title %}{% endblock %} + StarPilot: {% block title %}{% endblock %}