From 47c2cc9990b13febf708991ac70bcec116098043 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 4 Jun 2026 13:40:17 -0500 Subject: [PATCH] stuff and things --- opendbc_repo/opendbc/car/gm/interface.py | 6 ++ opendbc_repo/opendbc/car/gm/tests/test_gm.py | 15 ++++ selfdrive/car/redneck_cruise.py | 2 +- selfdrive/car/tests/test_redneck_cruise.py | 11 +++ selfdrive/controls/lib/latcontrol_torque.py | 22 +++++- .../controls/lib/longitudinal_planner.py | 78 +++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 4 + .../tests/test_longitudinal_planner.py | 53 +++++++++++++ 8 files changed, 186 insertions(+), 5 deletions(-) diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 10b67084f..f7e8894d7 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -609,6 +609,12 @@ class CarInterface(CarInterfaceBase): ret.startAccel = 1.15 ret.vEgoStarting = max(ret.vEgoStarting, 0.35) + if ret.openpilotLongitudinalControl and candidate in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) and not ret.enableGasInterceptorDEPRECATED: + ret.longitudinalTuning.kpBP = [0.0, 5.0, 15.0, 35.0] + ret.longitudinalTuning.kpV = [0.02, 0.03, 0.028, 0.022] + ret.longitudinalTuning.kiBP = [0.0, 5.0, 15.0, 35.0] + ret.longitudinalTuning.kiV = [0.28, 0.26, 0.20, 0.16] + elif candidate in CC_ONLY_CAR and not ret.enableGasInterceptorDEPRECATED: ret.flags |= GMFlags.CC_LONG.value ret.alphaLongitudinalAvailable = False diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index b1d115db9..66eca413f 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -117,6 +117,21 @@ class TestGMInterface: assert car_params.flags & GMFlags.NO_CAMERA.value assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_NO_CAMERA.value + def test_silverado_alpha_long_uses_trimmed_longitudinal_tune(self): + CarInterface = interfaces[CAR.CHEVROLET_SILVERADO] + fingerprint = _empty_fingerprint() + fingerprint[0] = FINGERPRINTS[CAR.CHEVROLET_SILVERADO][0].copy() + + car_params = CarInterface.get_params(CAR.CHEVROLET_SILVERADO, fingerprint, [], alpha_long=True, is_release=False, + docs=False, starpilot_toggles=_test_starpilot_toggles()) + + assert car_params.openpilotLongitudinalControl + assert not car_params.enableGasInterceptorDEPRECATED + assert list(car_params.longitudinalTuning.kpBP) == pytest.approx([0.0, 5.0, 15.0, 35.0]) + assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.02, 0.03, 0.028, 0.022]) + assert list(car_params.longitudinalTuning.kiBP) == pytest.approx([0.0, 5.0, 15.0, 35.0]) + assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.28, 0.26, 0.20, 0.16]) + def test_volt_gateway_without_accel_pos_uses_brake_pedal_message(self): CarInterface = interfaces[CAR.CHEVROLET_VOLT] fingerprint = _empty_fingerprint() diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py index f9c33796b..d5f51f543 100644 --- a/selfdrive/car/redneck_cruise.py +++ b/selfdrive/car/redneck_cruise.py @@ -34,7 +34,7 @@ def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float, if allow_plan_decrease and len(plan_speeds_ms) > 0: decrease_target_speed_ms = min(plan_speeds_ms[:lookahead_points]) - if decrease_target_speed_ms < min(target_speed_ms, float(speed_cluster_ms)): + if decrease_target_speed_ms < target_speed_ms: return decrease_target_speed_ms return target_speed_ms diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index 524d8b6d2..453c36312 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -141,6 +141,17 @@ class TestRedneckCruise(unittest.TestCase): ) self.assertAlmostEqual(71.0 * 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, + 32.9 * CV.MPH_TO_MS, + 47.8 * CV.MPH_TO_MS, + [37.3 * CV.MPH_TO_MS, 37.2 * CV.MPH_TO_MS, 37.1 * CV.MPH_TO_MS], + 10, + allow_plan_decrease=True, + ) + self.assertAlmostEqual(37.1 * CV.MPH_TO_MS, target_speed) + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 55019c2d4..a9f8546ff 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -484,11 +484,17 @@ IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04 IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60 IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.96 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.0 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.4 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06 +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.12 +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.16 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 4.5 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8 +IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.10 +IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10 @@ -1573,7 +1579,15 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo turn_in_weight * low_speed_factor) unwind_taper = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_TAPER_LEFT, IONIQ_6_UNWIND_TAPER_RIGHT) * unwind_weight * (0.30 + 0.70 * low_speed_factor)) - return (1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) + crawl_turn_in_scale = 0.0 + if desired_lateral_accel * desired_lateral_jerk > 0.0: + crawl_speed_weight = _ioniq_6_sigmoid((IONIQ_6_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) / + IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH) + crawl_lat_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CRAWL_TURN_IN_FF_LAT) / + IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH) + crawl_turn_in_scale = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT, + IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight + return (1.0 + crawl_turn_in_scale + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 889f2d322..413f00ca0 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -194,6 +194,17 @@ LOW_SPEED_FOLLOW_TRANSITION_PREV_ACCEL_MIN = 0.18 LOW_SPEED_FOLLOW_TRANSITION_TARGET_BRAKE_MIN = -0.18 LOW_SPEED_FOLLOW_TRANSITION_MAX_BRAKE = 0.14 LOW_SPEED_FOLLOW_TRANSITION_MIN_BRAKE = 0.08 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED = 10.0 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED = 20.0 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.85 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE = 0.25 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED = 1.0 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN = 12.0 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.9 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.15 +CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED = 1.5 +CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA = 0.25 +CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL = 0.18 # Uncertainty-based filter disable thresholds UNCERT_SLOPE_TRIG = 0.12 # per second @@ -1154,6 +1165,61 @@ class LongitudinalPlanner: )) return -cap_decel + def get_cruise_tracking_lead_accel_cap(self, lead, v_ego, t_follow, current_source, tracking_lead_active): + if lead is None or not lead.status or current_source != "cruise": + return None + if not (CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED): + return None + + lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0)) + if not bool(getattr(lead, "radar", False)) and lead_prob < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB: + return None + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE: + return None + + if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET: + return None + + lead_delta = float(lead.vLead) - float(v_ego) + if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED: + return None + + closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) + raw_close_lead = self.raw_close_lead_needs_control(lead, v_ego) + unresolved_slow_lead = ( + closing_speed >= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED and + lead_delta <= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA + ) + if not tracking_lead_active and not raw_close_lead and not unresolved_slow_lead: + return None + + desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) + gap_error = float(lead.dRel) - desired_gap + gap_buffer = max(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN, + CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego)) + if gap_error > gap_buffer: + return None + + base_cap = float(np.interp( + lead_delta, + [-1.5, -0.5, 0.0, 0.5, CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED], + [0.0, 0.04, 0.08, 0.12, 0.16], + )) + + if raw_close_lead: + base_cap = min(base_cap, float(np.interp(closing_speed, [0.5, 1.5, 3.5], [0.10, 0.05, 0.0]))) + else: + base_cap = min(base_cap, float(np.interp(closing_speed, [0.0, 1.0, 2.0], [0.18, 0.12, 0.06]))) + + if gap_error <= 0.0: + return max(0.0, base_cap) + + gap_factor = float(np.clip(gap_error / max(gap_buffer, 0.1), 0.0, 1.0)) + cap = min(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL, base_cap + 0.06 * gap_factor) + return max(0.0, cap) + def lead_is_matched_follow_window(self, lead, v_ego, base_t_follow): if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: return False @@ -2177,6 +2243,18 @@ class LongitudinalPlanner: self.a_desired = max(self.a_desired, near_duplicate_transition_target) output_a_target = near_duplicate_transition_target + if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap( + follow_control_lead, + scene_v_ego, + effective_t_follow, + self.mpc.source, + tracking_lead, + ) + if cruise_tracking_lead_accel_cap is not None: + self.a_desired = min(self.a_desired, cruise_tracking_lead_accel_cap) + output_a_target = min(output_a_target, cruise_tracking_lead_accel_cap) + output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 450038a24..968ecc947 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -457,6 +457,9 @@ class TestLatControl: assert get_ioniq_6_ff_scale(-0.4, -0.7, 8.0) >= get_ioniq_6_ff_scale(-0.4, 0.0, 8.0) >= get_ioniq_6_ff_scale(-0.4, 0.7, 8.0) assert get_ioniq_6_ff_scale(-1.2, 0.0, 20.0) < get_ioniq_6_ff_scale(1.2, 0.0, 20.0) < 1.0 assert get_ioniq_6_ff_scale(-1.2, 0.7, 20.0) <= get_ioniq_6_ff_scale(-1.2, 0.0, 20.0) + assert get_ioniq_6_ff_scale(0.30, 0.60, 3.0) > get_ioniq_6_ff_scale(0.30, 0.60, 6.0) + assert get_ioniq_6_ff_scale(0.30, 0.60, 6.0) > get_ioniq_6_ff_scale(0.30, 0.60, 12.0) + assert get_ioniq_6_ff_scale(0.30, -0.60, 3.0) < get_ioniq_6_ff_scale(0.30, 0.60, 3.0) def test_ioniq_6_directional_taper_curve(self): assert get_ioniq_6_directional_taper_scale(0.0, 0.0) == 1.0 @@ -478,6 +481,7 @@ class TestLatControl: assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 20.0) assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 6.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 20.0) + assert get_ioniq_6_directional_taper_scale(0.30, 0.60, 5.0) > get_ioniq_6_directional_taper_scale(0.30, 0.60, 12.0) def test_ioniq_6_output_taper_curve(self): assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 8c833f799..2ecd0ff08 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1996,6 +1996,59 @@ def test_matched_follow_transition_target_skips_low_speed_real_braking(): assert smoothed is None +def test_cruise_tracking_lead_accel_cap_limits_mid_speed_follow_nibble(): + v_ego = 16.2 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) + + cap = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=True, + ) + + assert cap is not None + assert 0.05 <= cap <= 0.10 + + +def test_cruise_tracking_lead_accel_cap_blocks_unresolved_raw_close_lead_burst(): + v_ego = 17.6 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=41.9, v_lead=14.2, a_lead=0.0, radar=True, model_prob=0.99, y_rel=-0.97) + + cap = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=False, + ) + + assert cap is not None + assert 0.0 <= cap <= 0.05 + + +def test_cruise_tracking_lead_accel_cap_skips_when_lead_clearly_pulls_away(): + v_ego = 14.5 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=35.0, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.1) + + cap = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=True, + ) + + assert cap is None + + def test_near_duplicate_lead_source_hysteresis_prefers_previous_source(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)