mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-27 17:42:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

fartlek

Browse Source
This commit is contained in:
firestar5683
2026-06-23 11:28:17 -05:00
parent 066a0f2520
commit bb36fe4287
5 changed files with 170 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/gm/carcontroller.py
+7 -1
View File
@@ -78,7 +78,13 @@ def use_interceptor_sng_launch(CP, CS, maneuver_mode=False):
launch_speed = max(CP.vEgoStarting, 0.3)
if maneuver_mode:
launch_speed = max(launch_speed, 2.0)
return CS.out.cruiseState.standstill and (CS.out.standstill or CS.out.vEgo < launch_speed)
near_stop = CS.out.standstill or CS.out.vEgo < launch_speed
if (
getattr(CP, "carFingerprint", None) == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and
getattr(CP, "enableGasInterceptorDEPRECATED", False)
):
return near_stop
return CS.out.cruiseState.standstill and near_stop
def should_spoof_dash_speed(CP, starpilot_toggles):
opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py
+12
View File
@@ -744,6 +744,18 @@ def test_use_interceptor_sng_launch_requires_actual_near_stop():
assert not use_interceptor_sng_launch(CP, _sng_cs(0.0, True, False))
def test_bolt_acc_pedal_sng_launch_uses_physical_standstill_without_stock_acc_bit():
CP = SimpleNamespace(
vEgoStarting=0.25,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
enableGasInterceptorDEPRECATED=True,
)
assert use_interceptor_sng_launch(CP, _sng_cs(0.0, True, False))
assert use_interceptor_sng_launch(CP, _sng_cs(0.2, False, False))
assert not use_interceptor_sng_launch(CP, _sng_cs(1.2, False, False))
def test_use_interceptor_sng_launch_extends_for_maneuver_mode():
CP = SimpleNamespace(vEgoStarting=0.25)
selfdrive/car/card.py
+9
View File
@@ -383,6 +383,8 @@ class Car:
starpilot_target_speed = 0.0
allow_plan_decrease = False
lead_present = False
lead_distance_m = 0.0
lead_rel_speed_ms = 0.0
lookahead_points = REDNECK_DECREASE_LOOKAHEAD_POINTS
if self.sm.seen['starpilotPlan'] and self.sm.valid['starpilotPlan']:
starpilot_target_speed = float(self.sm['starpilotPlan'].vCruise)
@@ -396,6 +398,11 @@ class Car:
str(longitudinal_plan.longitudinalPlanSource) != "cruise")
if lead_present and len(plan_speeds) > 0:
lookahead_points = len(plan_speeds)
if self.sm.seen['radarState'] and self.sm.valid['radarState']:
lead = self.sm['radarState'].leadOne
if lead.status:
lead_distance_m = max(float(lead.dRel), 0.0)
lead_rel_speed_ms = float(lead.vRel)
return select_redneck_target_speed(
float(getattr(CS, "vCruise", 0.0)),
@@ -405,6 +412,8 @@ class Car:
lookahead_points,
allow_plan_decrease=allow_plan_decrease,
lead_present=lead_present,
lead_distance_m=lead_distance_m,
lead_rel_speed_ms=lead_rel_speed_ms,
), lead_present
def step(self):
selfdrive/car/redneck_cruise.py
+67 -2
View File
@@ -17,6 +17,17 @@ MANUAL_BUTTON_INACTIVE_TIMER = 0.5
LEAD_RECOVERY_LOOKAHEAD_POINTS = 4
LEAD_RECOVERY_HOLD_BUFFER_MS = 0.5 * CV.MPH_TO_MS
LEAD_COAST_BUFFER_MS = 1.0 * CV.MPH_TO_MS
LEAD_EXTRA_COAST_BUFFER_FACTOR = 0.6
LEAD_EXTRA_COAST_BUFFER_MAX_MS = 3.0 * CV.MPH_TO_MS
LEAD_EXTRA_COAST_HEADWAY_MIN_S = 1.5
LEAD_EXTRA_COAST_HEADWAY_MAX_S = 3.0
LEAD_DEPARTURE_REL_SPEED_MIN_MS = 1.0 * CV.MPH_TO_MS
LEAD_DEPARTURE_HEADWAY_MIN_S = 1.8
LEAD_DEPARTURE_HEADWAY_MAX_S = 4.5
LEAD_DEPARTURE_BOOST_MIN_MS = 0.75 * CV.MPH_TO_MS
LEAD_DEPARTURE_BOOST_MAX_MS = 1.5 * CV.MPH_TO_MS
LEAD_DEPARTURE_BOOST_FACTOR = 0.35
LEAD_DEPARTURE_PLAN_POINTS = 3
CRUISE_BUTTON_TIMERS = {
int(ButtonType.decelCruise): 0,
@@ -31,7 +42,8 @@ CRUISE_BUTTON_TIMERS = {
def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
starpilot_target_speed_ms: float, plan_speeds_ms: list[float],
lookahead_points: int, allow_plan_decrease: bool = True,
lead_present: bool = False) -> float:
lead_present: bool = False, lead_distance_m: float = 0.0,
lead_rel_speed_ms: float = 0.0) -> float:
target_speed_ms = float(speed_cluster_ms)
if v_cruise_kph > 0:
target_speed_ms = float(v_cruise_kph) * CV.KPH_TO_MS
@@ -42,6 +54,14 @@ def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
if lead_present and target_speed_ms > speed_cluster_ms and plan_speeds_ms[0] > speed_cluster_ms:
recovery_lookahead_points = min(len(plan_speeds_ms), LEAD_RECOVERY_LOOKAHEAD_POINTS)
recovery_target_speed_ms = max(speed_cluster_ms, min(plan_speeds_ms[:recovery_lookahead_points]))
departure_boost_ms = get_lead_departure_boost_ms(
speed_cluster_ms,
lead_distance_m,
lead_rel_speed_ms,
plan_speeds_ms,
)
if departure_boost_ms > 0.0:
recovery_target_speed_ms = max(recovery_target_speed_ms, speed_cluster_ms + departure_boost_ms)
return min(target_speed_ms, recovery_target_speed_ms)
decrease_target_speed_ms = min(plan_speeds_ms[:lookahead_points])
@@ -50,7 +70,11 @@ def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
return speed_cluster_ms
if lead_present and decrease_target_speed_ms < speed_cluster_ms:
decrease_target_speed_ms = max(0.0, decrease_target_speed_ms - LEAD_COAST_BUFFER_MS)
decrease_target_speed_ms = max(0.0, decrease_target_speed_ms - get_lead_coast_buffer_ms(
speed_cluster_ms,
lead_distance_m,
lead_rel_speed_ms,
))
if decrease_target_speed_ms < target_speed_ms:
return decrease_target_speed_ms
@@ -58,6 +82,47 @@ def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
return target_speed_ms
def get_lead_coast_buffer_ms(speed_cluster_ms: float, lead_distance_m: float, lead_rel_speed_ms: float) -> float:
lead_closing_speed_ms = max(-float(lead_rel_speed_ms), 0.0)
if lead_closing_speed_ms <= 0.0:
return LEAD_COAST_BUFFER_MS
headway_factor = 0.0
if lead_distance_m > 0.0 and speed_cluster_ms > 0.1:
headway_s = lead_distance_m / speed_cluster_ms
headway_factor = min(max(
(LEAD_EXTRA_COAST_HEADWAY_MAX_S - headway_s) /
(LEAD_EXTRA_COAST_HEADWAY_MAX_S - LEAD_EXTRA_COAST_HEADWAY_MIN_S),
0.0,
), 1.0)
extra_buffer_ms = min(LEAD_EXTRA_COAST_BUFFER_MAX_MS, lead_closing_speed_ms * LEAD_EXTRA_COAST_BUFFER_FACTOR)
return LEAD_COAST_BUFFER_MS + extra_buffer_ms * (0.5 + (0.5 * headway_factor))
def get_lead_departure_boost_ms(speed_cluster_ms: float, lead_distance_m: float, lead_rel_speed_ms: float,
plan_speeds_ms: list[float]) -> float:
if lead_rel_speed_ms < LEAD_DEPARTURE_REL_SPEED_MIN_MS or speed_cluster_ms <= 0.1 or lead_distance_m <= 0.0:
return 0.0
plan_points = min(len(plan_speeds_ms), LEAD_DEPARTURE_PLAN_POINTS)
if plan_points <= 0 or min(plan_speeds_ms[:plan_points]) <= speed_cluster_ms:
return 0.0
headway_s = lead_distance_m / speed_cluster_ms
if headway_s < LEAD_DEPARTURE_HEADWAY_MIN_S:
return 0.0
headway_factor = min(max(
(headway_s - LEAD_DEPARTURE_HEADWAY_MIN_S) /
(LEAD_DEPARTURE_HEADWAY_MAX_S - LEAD_DEPARTURE_HEADWAY_MIN_S),
0.0,
), 1.0)
extra_boost_ms = max(lead_rel_speed_ms - LEAD_DEPARTURE_REL_SPEED_MIN_MS, 0.0) * LEAD_DEPARTURE_BOOST_FACTOR
boost_ms = LEAD_DEPARTURE_BOOST_MIN_MS + extra_boost_ms * (0.5 + (0.5 * headway_factor))
return min(boost_ms, LEAD_DEPARTURE_BOOST_MAX_MS)
def get_minimum_set_speed(is_metric: bool) -> int:
return 30 if is_metric else 20
selfdrive/car/tests/test_redneck_cruise.py
+75
View File
@@ -13,6 +13,8 @@ from openpilot.selfdrive.car.redneck_cruise import (
SEND_BUTTON_DECREASE,
SEND_BUTTON_INCREASE,
SEND_BUTTON_NONE,
get_lead_coast_buffer_ms,
get_lead_departure_boost_ms,
select_redneck_target_speed,
)
@@ -191,6 +193,41 @@ class TestRedneckCruise(unittest.TestCase):
)
self.assertLess(target_speed, 71.4 * CV.MPH_TO_MS)
def test_lead_coast_buffer_grows_with_closing_speed_and_tighter_headway(self):
base_buffer = get_lead_coast_buffer_ms(
75.0 * CV.MPH_TO_MS,
0.0,
0.0,
)
fast_closing_far_buffer = get_lead_coast_buffer_ms(
75.0 * CV.MPH_TO_MS,
70.0,
-5.0 * CV.MPH_TO_MS,
)
fast_closing_near_buffer = get_lead_coast_buffer_ms(
75.0 * CV.MPH_TO_MS,
35.0,
-5.0 * CV.MPH_TO_MS,
)
self.assertGreater(fast_closing_far_buffer, base_buffer)
self.assertGreater(fast_closing_near_buffer, fast_closing_far_buffer)
def test_target_speed_uses_extra_lead_buffer_when_closing_on_slower_car(self):
target_speed = select_redneck_target_speed(
120.0,
56.0 * CV.MPH_TO_MS,
0.0,
[55.9 * CV.MPH_TO_MS, 55.7 * CV.MPH_TO_MS, 55.3 * CV.MPH_TO_MS, 55.0 * CV.MPH_TO_MS,
54.8 * CV.MPH_TO_MS],
5,
allow_plan_decrease=True,
lead_present=True,
lead_distance_m=60.0,
lead_rel_speed_ms=-4.5 * CV.MPH_TO_MS,
)
self.assertLess(target_speed, 53.0 * CV.MPH_TO_MS)
def test_target_speed_uses_near_term_recovery_for_lead_speedup(self):
target_speed = select_redneck_target_speed(
120.0,
@@ -204,6 +241,44 @@ class TestRedneckCruise(unittest.TestCase):
)
self.assertAlmostEqual(55.8 * CV.MPH_TO_MS, target_speed)
def test_lead_departure_boost_requires_positive_rel_speed_and_stable_plan(self):
boost = get_lead_departure_boost_ms(
77.0 * CV.MPH_TO_MS,
63.0,
2.0 * CV.MPH_TO_MS,
[77.4 * CV.MPH_TO_MS, 77.5 * CV.MPH_TO_MS, 77.6 * CV.MPH_TO_MS],
)
blocked_by_plan = get_lead_departure_boost_ms(
77.0 * CV.MPH_TO_MS,
63.0,
2.0 * CV.MPH_TO_MS,
[77.4 * CV.MPH_TO_MS, 76.9 * CV.MPH_TO_MS, 77.6 * CV.MPH_TO_MS],
)
blocked_by_headway = get_lead_departure_boost_ms(
77.0 * CV.MPH_TO_MS,
35.0,
2.0 * CV.MPH_TO_MS,
[77.4 * CV.MPH_TO_MS, 77.5 * CV.MPH_TO_MS, 77.6 * CV.MPH_TO_MS],
)
self.assertGreater(boost, 0.0)
self.assertEqual(blocked_by_plan, 0.0)
self.assertEqual(blocked_by_headway, 0.0)
def test_target_speed_gets_small_departure_boost_for_opening_lead(self):
target_speed = select_redneck_target_speed(
128.0,
77.0 * CV.MPH_TO_MS,
0.0,
[77.4 * CV.MPH_TO_MS, 77.5 * CV.MPH_TO_MS, 77.6 * CV.MPH_TO_MS, 77.8 * CV.MPH_TO_MS],
10,
allow_plan_decrease=True,
lead_present=True,
lead_distance_m=63.0,
lead_rel_speed_ms=2.0 * CV.MPH_TO_MS,
)
self.assertGreater(target_speed, 77.5 * CV.MPH_TO_MS)
def test_target_speed_holds_current_step_during_lead_recovery(self):
target_speed = select_redneck_target_speed(
128.0,