diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 0cde32b45..b3816c165 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -94,6 +94,11 @@ NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF = 0.35 NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MIN = 1.25 NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MAX = 2.25 NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_KEEP_MARGIN = 0.35 +IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MIN_SPEED = 10.0 +IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_HEADWAY_ABOVE_TARGET = 0.40 +IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_LEAD_BRAKE = 0.25 +IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_PULLAWAY_SPEED = 1.5 +IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 10.0 # Function to get parameter value based on current speed def get_speed_based_param(speed_mph, param_array): @@ -682,6 +687,41 @@ class LongitudinalMpc: return None return prev_source + def get_identical_radar_duplicate_cruise_hold(self, prev_source, lead_one, lead_two, + lead_0_obstacle, lead_1_obstacle, cruise_obstacle, + v_ego, t_follow): + if prev_source not in ("lead0", "lead1"): + return None + if float(v_ego) < IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MIN_SPEED: + return None + if not self.leads_share_identical_radar_track(lead_one, lead_two): + return None + if abs(float(lead_0_obstacle) - float(lead_1_obstacle)) > NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_KEEP_MARGIN: + return None + + prev_lead = lead_one if prev_source == "lead0" else lead_two + if prev_lead is None or not prev_lead.status: + return None + + actual_headway = float(prev_lead.dRel) / max(float(v_ego), 1e-3) + if actual_headway > float(t_follow) + IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_HEADWAY_ABOVE_TARGET: + return None + + lead_brake = max(0.0, -float(getattr(prev_lead, "aLeadK", 0.0))) + if lead_brake > IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_LEAD_BRAKE: + return None + + lead_delta = float(prev_lead.vLead) - float(v_ego) + if lead_delta > IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_PULLAWAY_SPEED: + return None + + prev_lead_obstacle = float(lead_0_obstacle if prev_source == "lead0" else lead_1_obstacle) + cruise_advantage = prev_lead_obstacle - float(cruise_obstacle) + if cruise_advantage > IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE: + return None + + return prev_source + def set_accel_limits(self, min_a, max_a): # TODO this sets a max accel limit, but the minimum limit is only for cruise decel # needs refactor @@ -737,14 +777,26 @@ class LongitudinalMpc: x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) candidate_source = SOURCES[np.argmin(x_obstacles[0])] sticky_source = None - if optional_far_lead_comfort and candidate_source in ("lead0", "lead1"): - sticky_source = self.get_identical_radar_duplicate_source_hold( - prev_source, - lead_one, - lead_two, - lead_0_obstacle[0], - lead_1_obstacle[0], - ) + if optional_far_lead_comfort: + if candidate_source in ("lead0", "lead1"): + sticky_source = self.get_identical_radar_duplicate_source_hold( + prev_source, + lead_one, + lead_two, + lead_0_obstacle[0], + lead_1_obstacle[0], + ) + elif candidate_source == "cruise": + sticky_source = self.get_identical_radar_duplicate_cruise_hold( + prev_source, + lead_one, + lead_two, + lead_0_obstacle[0], + lead_1_obstacle[0], + cruise_obstacle[0], + v_ego, + t_follow, + ) self.source = sticky_source or candidate_source # These are not used in ACC mode diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 09902e30c..66e7bc074 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2919,6 +2919,54 @@ def test_identical_radar_duplicate_source_hold_keeps_previous_label(): assert sticky == "lead1" +def test_identical_radar_duplicate_cruise_hold_keeps_previous_lead_through_route_like_crossover(): + v_ego = 22.57 + t_follow = 1.20 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=32.3, v_lead=23.74, a_lead=0.67, radar=True, model_prob=1.0) + lead_two = make_lead(status=True, d_rel=32.3, v_lead=23.74, a_lead=0.67, radar=True, model_prob=1.0) + lead_one.radarTrackId = 2493 + lead_two.radarTrackId = 2493 + + sticky = planner.mpc.get_identical_radar_duplicate_cruise_hold( + "lead0", + lead_one, + lead_two, + 144.92, + 144.91, + 135.10, + v_ego, + t_follow, + ) + + assert sticky == "lead0" + + +def test_identical_radar_duplicate_cruise_hold_skips_clear_pullaway(): + v_ego = 22.57 + t_follow = 1.20 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=48.0, v_lead=25.5, a_lead=0.45, radar=True, model_prob=1.0) + lead_two = make_lead(status=True, d_rel=48.0, v_lead=25.5, a_lead=0.45, radar=True, model_prob=1.0) + lead_one.radarTrackId = 2493 + lead_two.radarTrackId = 2493 + + sticky = planner.mpc.get_identical_radar_duplicate_cruise_hold( + "lead0", + lead_one, + lead_two, + 180.0, + 180.0, + 150.0, + v_ego, + t_follow, + ) + + assert sticky is None + + def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)