fcw

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -102,6 +102,9 @@ LIMIT_COST = 1e6
 ACADOS_SOLVER_TYPE = 'SQP_RTI'
 # Default lead acceleration decay set to 50% at 1s
 LEAD_ACCEL_TAU = 1.5
+FCW_MIN_MODEL_PROB = 0.9
+FCW_MIN_CLOSING_SPEED = 0.5
+FCW_MAX_TTC = 4.0
 
 
 # Fewer timestamps don't hurt performance and lead to
@@ -116,6 +119,18 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
+
+def should_trigger_planner_fcw(lead, v_ego: float) -> bool:
+  if lead is None or not lead.status or float(getattr(lead, "modelProb", 0.0)) <= FCW_MIN_MODEL_PROB:
+    return False
+
+  closing_speed = max(0.0, float(v_ego) - float(getattr(lead, "vLead", 0.0)))
+  if closing_speed < FCW_MIN_CLOSING_SPEED:
+    return False
+
+  ttc = max(0.0, float(getattr(lead, "dRel", 0.0))) / max(closing_speed, 1e-3)
+  return ttc < FCW_MAX_TTC
+
 def get_jerk_factor(aggressive_jerk_acceleration=0.5, aggressive_jerk_danger=0.5, aggressive_jerk_speed=0.5,
                     standard_jerk_acceleration=1.0, standard_jerk_danger=1.0, standard_jerk_speed=1.0,
                     relaxed_jerk_acceleration=1.0, relaxed_jerk_danger=1.0, relaxed_jerk_speed=1.0,
@@ -652,7 +667,7 @@ class LongitudinalMpc:
 
     self.run()
     if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
-            lead_one.modelProb > 0.9):
+            should_trigger_planner_fcw(lead_one, v_ego)):
       self.crash_cnt += 1
     else:
       self.crash_cnt = 0

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -10,7 +10,7 @@ from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel, should_trigger_planner_fcw
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 
 
@@ -202,6 +202,24 @@ def test_soften_far_radar_lead_accel_keeps_close_closing_brake():
   assert softened == pytest.approx(baseline)
 
 
+def test_planner_fcw_suppresses_low_speed_opening_or_low_ttc_false_positives():
+  assert not should_trigger_planner_fcw(
+    make_lead(status=True, d_rel=7.156, v_lead=0.798, a_lead=0.021, radar=False, model_prob=0.99),
+    0.402,
+  )
+  assert not should_trigger_planner_fcw(
+    make_lead(status=True, d_rel=9.311, v_lead=0.911, a_lead=-0.263, radar=False, model_prob=0.99),
+    1.252,
+  )
+
+
+def test_planner_fcw_keeps_real_low_speed_closing_alerts():
+  assert should_trigger_planner_fcw(
+    make_lead(status=True, d_rel=1.8, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.99),
+    1.6,
+  )
+
+
 def test_vision_lead_approach_cap_brakes_before_hard_cap():
   v_ego = 21.535
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)