mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-07-26 12:22:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

livin off energy drinks and Marlboro Reds

Browse Source
This commit is contained in:
firestar5683
2026-06-04 23:32:16 -05:00
parent a1d338f35c
commit 2aaad84ab6
3 changed files with 86 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/card.py
+12 -5
View File
@@ -392,13 +392,16 @@ class Car:
return
filtered_CS = self._get_button_event_filtered_state(CS)
send_button, v_target = self.redneck_cruise.run(filtered_CS, CC, self._get_redneck_target_speed(CS), self.is_metric)
v_target_ms, lead_present = self._get_redneck_target_speed(CS)
send_button, v_target = self.redneck_cruise.run(filtered_CS, CC, v_target_ms, self.is_metric, lead_present=lead_present)
self.CI.CS.redneck_send_button = send_button
self.CI.CS.redneck_v_target = v_target
def _get_redneck_target_speed(self, CS: car.CarState) -> float:
def _get_redneck_target_speed(self, CS: car.CarState) -> tuple[float, bool]:
starpilot_target_speed = 0.0
allow_plan_decrease = False
lead_present = False
lookahead_points = REDNECK_DECREASE_LOOKAHEAD_POINTS
if self.sm.seen['starpilotPlan'] and self.sm.valid['starpilotPlan']:
starpilot_target_speed = float(self.sm['starpilotPlan'].vCruise)
@@ -406,17 +409,21 @@ class Car:
if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']:
longitudinal_plan = self.sm['longitudinalPlan']
plan_speeds = [float(speed) for speed in longitudinal_plan.speeds if math.isfinite(float(speed))]
allow_plan_decrease = bool(longitudinal_plan.hasLead or longitudinal_plan.shouldStop or
lead_present = bool(longitudinal_plan.hasLead)
allow_plan_decrease = bool(lead_present or longitudinal_plan.shouldStop or
str(longitudinal_plan.longitudinalPlanSource) != "cruise")
if lead_present and len(plan_speeds) > 0:
lookahead_points = len(plan_speeds)
return select_redneck_target_speed(
float(getattr(CS, "vCruise", 0.0)),
float(CS.cruiseState.speedCluster),
starpilot_target_speed,
plan_speeds,
REDNECK_DECREASE_LOOKAHEAD_POINTS,
lookahead_points,
allow_plan_decrease=allow_plan_decrease,
)
lead_present=lead_present,
), lead_present
def _advance_redneck_button_feedback_filter(self) -> None:
if self.redneck_cruise is None:
selfdrive/car/redneck_cruise.py
+30 -12
View File
@@ -12,6 +12,9 @@ SEND_BUTTON_DECREASE = 2
HYST_GAP = 0.0
INCREASE_INACTIVE_TIMER = 0.4
DECREASE_INACTIVE_TIMER = 0.1
LEAD_INCREASE_INACTIVE_TIMER = 0.1
LEAD_RECOVERY_LOOKAHEAD_POINTS = 4
LEAD_COAST_BUFFER_MS = 1.0 * CV.MPH_TO_MS
CRUISE_BUTTON_TIMERS = {
int(ButtonType.decelCruise): 0,
@@ -25,7 +28,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) -> float:
lookahead_points: int, allow_plan_decrease: bool = True,
lead_present: bool = False) -> float:
target_speed_ms = float(speed_cluster_ms)
if v_cruise_kph > 0:
target_speed_ms = float(v_cruise_kph) * CV.KPH_TO_MS
@@ -33,7 +37,15 @@ def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
target_speed_ms = float(starpilot_target_speed_ms)
if allow_plan_decrease and len(plan_speeds_ms) > 0:
if lead_present 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]))
return min(target_speed_ms, recovery_target_speed_ms)
decrease_target_speed_ms = min(plan_speeds_ms[:lookahead_points])
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)
if decrease_target_speed_ms < target_speed_ms:
return decrease_target_speed_ms
@@ -106,20 +118,25 @@ class RedneckCruise:
return "holding"
@staticmethod
def _get_pre_active_frames(state: str) -> int:
timer = DECREASE_INACTIVE_TIMER if state == "decreasing" else INCREASE_INACTIVE_TIMER
def _get_pre_active_frames(state: str, lead_present: bool) -> int:
if state == "decreasing":
timer = DECREASE_INACTIVE_TIMER
elif lead_present:
timer = LEAD_INCREASE_INACTIVE_TIMER
else:
timer = INCREASE_INACTIVE_TIMER
return int(timer / DT_CTRL)
def _arm_pre_active(self, desired_state: str) -> None:
def _arm_pre_active(self, desired_state: str, lead_present: bool) -> None:
if desired_state == "holding":
self.state = "holding"
self.pre_active_timer = 0
return
self.state = "preActive"
self.pre_active_timer = self._get_pre_active_frames(desired_state)
self.pre_active_timer = self._get_pre_active_frames(desired_state, lead_present)
def _update_state_machine(self) -> int:
def _update_state_machine(self, lead_present: bool) -> int:
desired_state = self._desired_state()
if not self.is_ready:
@@ -127,35 +144,36 @@ class RedneckCruise:
self.pre_active_timer = 0
elif self.state == "inactive":
if not self.is_ready_prev:
self._arm_pre_active(desired_state)
self._arm_pre_active(desired_state, lead_present)
elif self.state == "preActive":
if desired_state == "holding":
self.state = "holding"
self.pre_active_timer = 0
else:
desired_frames = self._get_pre_active_frames(desired_state)
desired_frames = self._get_pre_active_frames(desired_state, lead_present)
self.pre_active_timer = max(0, min(self.pre_active_timer, desired_frames) - 1)
if self.pre_active_timer <= 0:
self.state = desired_state
elif self.state == "holding":
if desired_state != "holding":
self._arm_pre_active(desired_state)
self._arm_pre_active(desired_state, lead_present)
elif self.state != desired_state:
if desired_state == "holding":
self.state = "holding"
else:
self._arm_pre_active(desired_state)
self._arm_pre_active(desired_state, lead_present)
return self._send_button_for_state(self.state)
def run(self, CS: car.CarState, CC: car.CarControl, v_target_ms: float, is_metric: bool) -> tuple[int, int]:
def run(self, CS: car.CarState, CC: car.CarControl, v_target_ms: float, is_metric: bool,
lead_present: bool = False) -> tuple[int, int]:
if self.FPCP.pcmCruiseSpeed or not self.FPCP.redneckCruiseAvailable:
self._reset()
return SEND_BUTTON_NONE, 0
self._update_calculations(CS, v_target_ms, is_metric)
self._update_readiness(CS, CC)
send_button = self._update_state_machine()
send_button = self._update_state_machine(lead_present)
self.is_ready_prev = self.is_ready
return send_button, self.v_target
selfdrive/car/tests/test_redneck_cruise.py
+44 -2
View File
@@ -7,6 +7,7 @@ from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car.redneck_cruise import (
DECREASE_INACTIVE_TIMER,
INCREASE_INACTIVE_TIMER,
LEAD_INCREASE_INACTIVE_TIMER,
RedneckCruise,
SEND_BUTTON_DECREASE,
SEND_BUTTON_INCREASE,
@@ -41,7 +42,8 @@ class TestRedneckCruise(unittest.TestCase):
def _button_event(button_type, pressed):
return SimpleNamespace(type=button_type, pressed=pressed)
def _run_until_active(self, target_mph, speed_cluster_mph=20.0, button_events=None, override=False, cancel=False, resume=False):
def _run_until_active(self, target_mph, speed_cluster_mph=20.0, button_events=None,
override=False, cancel=False, resume=False, lead_present=False):
frames = int(max(INCREASE_INACTIVE_TIMER, DECREASE_INACTIVE_TIMER) / DT_CTRL) + 2
send_button = SEND_BUTTON_NONE
v_target = 0
@@ -51,11 +53,12 @@ class TestRedneckCruise(unittest.TestCase):
self._new_control(override=override, cancel=cancel, resume=resume),
target_mph * CV.MPH_TO_MS,
is_metric=False,
lead_present=lead_present,
)
button_events = None
return send_button, v_target
def _frames_until_button(self, target_mph, speed_cluster_mph):
def _frames_until_button(self, target_mph, speed_cluster_mph, lead_present=False):
frames = int(INCREASE_INACTIVE_TIMER / DT_CTRL) + 4
for frame in range(frames):
send_button, _ = self.redneck.run(
@@ -63,6 +66,7 @@ class TestRedneckCruise(unittest.TestCase):
self._new_control(),
target_mph * CV.MPH_TO_MS,
is_metric=False,
lead_present=lead_present,
)
if send_button != SEND_BUTTON_NONE:
return frame
@@ -87,6 +91,16 @@ class TestRedneckCruise(unittest.TestCase):
self.assertIsNotNone(increase_frame)
self.assertLess(decrease_frame, increase_frame)
def test_lead_increase_activates_faster_than_free_cruise_increase(self):
free_cruise_frame = self._frames_until_button(target_mph=25.0, speed_cluster_mph=20.0, lead_present=False)
self.redneck = RedneckCruise(self.CP, self.FPCP)
lead_frame = self._frames_until_button(target_mph=25.0, speed_cluster_mph=20.0, lead_present=True)
self.assertIsNotNone(free_cruise_frame)
self.assertIsNotNone(lead_frame)
self.assertLess(lead_frame, free_cruise_frame)
self.assertLessEqual(lead_frame, int(LEAD_INCREASE_INACTIVE_TIMER / DT_CTRL))
def test_suppresses_output_during_manual_cruise_button_use(self):
button_event = self._button_event(ButtonType.accelCruise, True)
send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, button_events=[button_event])
@@ -141,6 +155,33 @@ class TestRedneckCruise(unittest.TestCase):
)
self.assertAlmostEqual(71.0 * CV.MPH_TO_MS, target_speed)
def test_target_speed_uses_longer_horizon_and_buffer_for_lead_slowdown(self):
target_speed = select_redneck_target_speed(
120.0,
75.0 * CV.MPH_TO_MS,
0.0,
[74.9 * CV.MPH_TO_MS, 74.6 * CV.MPH_TO_MS, 74.2 * CV.MPH_TO_MS, 73.8 * CV.MPH_TO_MS,
73.4 * CV.MPH_TO_MS, 73.0 * CV.MPH_TO_MS, 72.6 * CV.MPH_TO_MS, 72.2 * CV.MPH_TO_MS,
71.8 * CV.MPH_TO_MS, 71.4 * CV.MPH_TO_MS, 71.0 * CV.MPH_TO_MS],
11,
allow_plan_decrease=True,
lead_present=True,
)
self.assertLess(target_speed, 71.4 * CV.MPH_TO_MS)
def test_target_speed_uses_near_term_recovery_for_lead_speedup(self):
target_speed = select_redneck_target_speed(
120.0,
55.0 * CV.MPH_TO_MS,
0.0,
[57.15 * CV.MPH_TO_MS, 56.9 * CV.MPH_TO_MS, 56.4 * CV.MPH_TO_MS, 55.8 * CV.MPH_TO_MS,
54.88 * CV.MPH_TO_MS, 52.0 * CV.MPH_TO_MS, 50.15 * CV.MPH_TO_MS],
10,
allow_plan_decrease=True,
lead_present=True,
)
self.assertAlmostEqual(55.8 * CV.MPH_TO_MS, target_speed)
def test_target_speed_stays_on_lead_target_when_cluster_drops_below_it(self):
target_speed = select_redneck_target_speed(
76.9,
@@ -149,6 +190,7 @@ class TestRedneckCruise(unittest.TestCase):
[37.3 * CV.MPH_TO_MS, 37.2 * CV.MPH_TO_MS, 37.1 * CV.MPH_TO_MS],
10,
allow_plan_decrease=True,
lead_present=True,
)
self.assertAlmostEqual(37.1 * CV.MPH_TO_MS, target_speed)