diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 2a41b0569..a061a6417 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -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): diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index d24fe7f34..53b5c47ed 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -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) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 09c73a534..7c258688e 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -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): diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py index 3588ecec1..31be4ecb4 100644 --- a/selfdrive/car/redneck_cruise.py +++ b/selfdrive/car/redneck_cruise.py @@ -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 diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index 6d3ffdc18..6df2f11aa 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -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,