diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 6cde385c3..5ed72f8b4 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index fb8edd62f..db47fefaf 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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)