stuff and things

2026-08-17 06:13:52 +08:00 · 2026-06-04 13:40:17 -05:00
parent 85e3e11c10
commit 47c2cc9990
8 changed files with 186 additions and 5 deletions
@@ -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
@@ -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()
@@ -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
@@ -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()
@@ -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:
@@ -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))

@@ -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
@@ -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)