From 2aaad84ab6bcdc4aae889c1eace52e9990d22d5f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 4 Jun 2026 23:32:16 -0500 Subject: [PATCH] livin off energy drinks and Marlboro Reds --- selfdrive/car/card.py | 17 +++++--- selfdrive/car/redneck_cruise.py | 42 ++++++++++++++------ selfdrive/car/tests/test_redneck_cruise.py | 46 +++++++++++++++++++++- 3 files changed, 86 insertions(+), 19 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index a62a2e6d0..d3530e193 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -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: diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py index d5f51f543..09eed0153 100644 --- a/selfdrive/car/redneck_cruise.py +++ b/selfdrive/car/redneck_cruise.py @@ -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 diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index 453c36312..d9371d1c4 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -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)