Applebees

selfdrive/controls/lib/latcontrol_torque.py

@@ -493,18 +493,18 @@ IONIQ_6_FF_CUTOFF_WIDTH = 0.12
 IONIQ_6_TRANSITION_SPEED = 10.0
 IONIQ_6_PHASE_SCALE = 0.10
 IONIQ_6_TURN_IN_BOOST_LEFT = 1.64
-IONIQ_6_TURN_IN_BOOST_RIGHT = 1.88
+IONIQ_6_TURN_IN_BOOST_RIGHT = 2.10
 IONIQ_6_UNWIND_TAPER_LEFT = 3.18
 IONIQ_6_UNWIND_TAPER_RIGHT = 8.20
 IONIQ_6_FRICTION_MULT = 0.928
 IONIQ_6_FRICTION_LAT_RISE = 0.20
 IONIQ_6_FRICTION_JERK_RISE = 0.24
 IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.78
-IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.24
+IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.42
 IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.90
 IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 10.20
 IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.44
-IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.78
+IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.94
 IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55
 IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 9.10
 IONIQ_6_CENTER_TAPER_MAX = 0.082

selfdrive/controls/lib/longitudinal_planner.py

@@ -1813,17 +1813,25 @@ class LongitudinalPlanner:
                                                 current_source, tracking_lead_active):
     if lead is None or not lead.status:
       return None
-    if current_source not in ("lead0", "lead1") and not tracking_lead_active:
+    if current_source not in ("cruise", "lead0", "lead1") and not tracking_lead_active:
       return None
     if not (self.lead_one.status and self.lead_two.status):
       return None
     if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego):
       return None
-    if float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED:
+    low_speed_extension_active = bool(
+      tracking_lead_active and
+      current_source in ("cruise", "lead0", "lead1") and
+      LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED
+    )
+    if float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED:
+      return None
+    if float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED and not low_speed_extension_active:
       return None
 
-    lead_prob = float(getattr(lead, "modelProb", 0.0))
-    if bool(getattr(lead, "radar", False)) or lead_prob < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB:
+    lead_radar = bool(getattr(lead, "radar", False))
+    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
+    if not lead_radar and lead_prob < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB:
       return None
 
     lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
@@ -1849,6 +1857,10 @@ class LongitudinalPlanner:
     if abs(target_delta) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A:
       return None
 
+    if low_speed_extension_active and (float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET or
+                                       float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET):
+      return None
+
     positive_step = NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP
     negative_step = NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP
     if float(prev_output_a_target) * float(output_a_target) < 0.0:

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -2730,6 +2730,31 @@ def test_near_duplicate_lead_transition_target_damps_tracking_cruise_sign_flip()
   assert smoothed == pytest.approx(-0.92, abs=1e-6)
 
 
+def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_handoff():
+  v_ego = 14.31
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0)
+  lead_two = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0)
+  lead_one.vRel = lead_one.vLead - v_ego
+  lead_two.vRel = lead_two.vLead - v_ego
+  planner.lead_one = lead_one
+  planner.lead_two = lead_two
+
+  smoothed = planner.get_near_duplicate_lead_transition_target(
+    lead_one,
+    v_ego,
+    1.45,
+    prev_output_a_target=0.68,
+    output_a_target=0.03,
+    current_source="lead0",
+    tracking_lead_active=True,
+  )
+
+  assert smoothed is not None
+  assert smoothed == pytest.approx(0.36, abs=1e-6)
+
+
 def test_duplicate_slow_lead_brake_hold_prevents_zero_cross_from_duplicate_voacc_leads():
   v_ego = 24.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)