diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 22e892749..1ae17a696 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -74,6 +74,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "HYUNDAI_TUCSON_HEV_2025" = [2.5, 2.5, 0.1] "HYUNDAI_TUCSON_PHEV_2025" = [2.5, 2.5, 0.1] "KIA_SPORTAGE_5TH_GEN" = [2.6, 2.6, 0.1] +"KIA_XCEED_PHEV" = [2.9638737459977467, 2.1259108157250735, 0.07813665616927593] "KIA_SPORTAGE_2026" = [nan, 2.5, nan] "KIA_SPORTAGE_HEV_2026" = [nan, 2.5, nan] "GENESIS_GV70_1ST_GEN" = [2.42, 2.42, 0.1] diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index fd0057190..f538aa6e2 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -24,7 +24,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "KIA_OPTIMA_H_G4_FL" = "HYUNDAI_SONATA" "KIA_FORTE" = "HYUNDAI_SONATA" "KIA_CEED" = "HYUNDAI_SONATA" -"KIA_XCEED_PHEV" = "HYUNDAI_SONATA" "KIA_SELTOS" = "HYUNDAI_SONATA" "KIA_NIRO_PHEV" = "KIA_NIRO_EV" "KIA_NIRO_PHEV_2022" = "KIA_NIRO_EV" diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 148866bb6..5d6573b16 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -141,6 +141,9 @@ ELANTRA_NON_SCC_CARS = ( KIA_EV6_CARS = ( HYUNDAI_CAR.KIA_EV6, ) +KIA_XCEED_CARS = ( + HYUNDAI_CAR.KIA_XCEED_PHEV, +) KIA_FORTE_CARS = ( HYUNDAI_CAR.KIA_FORTE, HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, @@ -317,6 +320,24 @@ ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT = 0.12 ELANTRA_NON_SCC_UNWIND_TAPER_LEFT = 0.22 ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT = 0.12 +KIA_XCEED_FF_REDUCTION_LEFT = 0.08 +KIA_XCEED_FF_REDUCTION_RIGHT = 0.10 +KIA_XCEED_FF_ONSET = 0.16 +KIA_XCEED_FF_ONSET_WIDTH = 0.06 +KIA_XCEED_FF_CUTOFF = 1.30 +KIA_XCEED_FF_CUTOFF_WIDTH = 0.36 +KIA_XCEED_TRANSITION_SPEED = 8.5 +KIA_XCEED_PHASE_SCALE = 0.10 +KIA_XCEED_TURN_IN_BOOST_LEFT = 0.08 +KIA_XCEED_TURN_IN_BOOST_RIGHT = 0.06 +KIA_XCEED_UNWIND_TAPER_LEFT = 0.16 +KIA_XCEED_UNWIND_TAPER_RIGHT = 0.14 +KIA_XCEED_CENTER_TAPER_MAX = 0.05 +KIA_XCEED_CENTER_TAPER_LAT = 0.14 +KIA_XCEED_CENTER_TAPER_LAT_WIDTH = 0.03 +KIA_XCEED_CENTER_TAPER_SPEED = 17.5 +KIA_XCEED_CENTER_TAPER_SPEED_WIDTH = 2.5 + KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 KIA_FORTE_FF_REDUCTION_LEFT = 0.05 KIA_FORTE_FF_REDUCTION_RIGHT = 0.10 @@ -1274,6 +1295,54 @@ def get_elantra_non_scc_ff_scale(desired_lateral_accel: float, desired_lateral_j return base_scale * turn_in_boost * max(unwind_taper, 0.0) +def _kia_xceed_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _kia_xceed_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_XCEED_TRANSITION_SPEED) ** 2) + + +def _kia_xceed_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_XCEED_PHASE_SCALE) + + +def _kia_xceed_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def get_kia_xceed_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _kia_xceed_sigmoid((abs_lateral_accel - KIA_XCEED_FF_ONSET) / KIA_XCEED_FF_ONSET_WIDTH) + cutoff = _kia_xceed_sigmoid((KIA_XCEED_FF_CUTOFF - abs_lateral_accel) / KIA_XCEED_FF_CUTOFF_WIDTH) + base_reduction = _kia_xceed_side_value(desired_lateral_accel, + KIA_XCEED_FF_REDUCTION_LEFT, + KIA_XCEED_FF_REDUCTION_RIGHT) * onset * cutoff + phase = _kia_xceed_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _kia_xceed_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_kia_xceed_side_value(desired_lateral_accel, + KIA_XCEED_TURN_IN_BOOST_LEFT, + KIA_XCEED_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_kia_xceed_side_value(desired_lateral_accel, + KIA_XCEED_UNWIND_TAPER_LEFT, + KIA_XCEED_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) + + +def get_kia_xceed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _kia_xceed_sigmoid((v_ego - KIA_XCEED_CENTER_TAPER_SPEED) / KIA_XCEED_CENTER_TAPER_SPEED_WIDTH) + center_weight = _kia_xceed_sigmoid((KIA_XCEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_XCEED_CENTER_TAPER_LAT_WIDTH) + reduction = KIA_XCEED_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + def _kia_forte_sigmoid(x: float) -> float: return _sigmoid(x) @@ -1917,6 +1986,7 @@ class LatControlTorque(LatControl): self.is_sonata = CP.carFingerprint in SONATA_CARS self.is_sonata_hybrid = CP.carFingerprint in SONATA_HYBRID_CARS self.is_elantra_non_scc = CP.carFingerprint in ELANTRA_NON_SCC_CARS + self.is_kia_xceed = CP.carFingerprint in KIA_XCEED_CARS self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) @@ -2051,6 +2121,7 @@ class LatControlTorque(LatControl): sonata_active = self.is_sonata sonata_hybrid_active = self.is_sonata_hybrid elantra_non_scc_active = self.is_elantra_non_scc + kia_xceed_active = self.is_kia_xceed kia_forte_active = self.is_kia_forte kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active() @@ -2060,6 +2131,7 @@ class LatControlTorque(LatControl): ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0 sonata_center_taper = get_sonata_center_taper_scale(setpoint, CS.vEgo) if sonata_active else 1.0 sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0 + kia_xceed_center_taper = get_kia_xceed_center_taper_scale(setpoint, CS.vEgo) if kia_xceed_active else 1.0 kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0 kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( @@ -2110,6 +2182,8 @@ class LatControlTorque(LatControl): ff *= get_sonata_hybrid_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_hybrid_center_taper elif elantra_non_scc_active: ff *= get_elantra_non_scc_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + elif kia_xceed_active: + ff *= get_kia_xceed_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_xceed_center_taper elif kia_forte_active: ff *= get_kia_forte_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_forte_center_taper friction_threshold = get_kia_forte_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 3acd935d9..87c356521 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -33,6 +33,11 @@ MIN_ALLOW_THROTTLE_SPEED = 5.0 RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5 RAW_LEAD_SAFETY_TTC = 7.0 RAW_LEAD_SAFETY_DISTANCE = 40.0 +RAW_LEAD_LOW_SPEED_HOLD_MAX_EGO_SPEED = 4.5 +RAW_LEAD_LOW_SPEED_HOLD_MAX_LEAD_SPEED = 3.5 +RAW_LEAD_LOW_SPEED_HOLD_MAX_DISTANCE = 10.0 +RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET = 1.75 +RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED = 0.15 STANDSTILL_LEAD_NUDGE_ACCEL = 0.05 STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0 STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.35 @@ -75,6 +80,23 @@ LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_ACCEL = 0.12 LEAD_DEPART_ACCEL_HOLD_MAX_LEAD_BRAKE = 0.2 LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL = 0.25 LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.45 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED = 4.5 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE = 4.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE = 18.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED = 4.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.75 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.9 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA = -0.5 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA = 0.75 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL = -0.4 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL = 0.25 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL = 0.08 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL = 0.22 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED = 1.25 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP = 4.0 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED = 1.2 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA = 0.8 +LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL = 0.5 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0 VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5 @@ -1118,6 +1140,59 @@ class LongitudinalPlanner: 0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0) return min(accel_cap, max(float(model_desired_accel), LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL)) + def get_low_speed_weak_lead_accel_cap(self, lead, v_ego): + if lead is None or not lead.status: + return None + + 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 < LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_MODEL_PROB: + return None + + d_rel = float(getattr(lead, "dRel", 0.0)) + lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) + lead_delta = lead_speed - float(v_ego) + lead_accel = float(getattr(lead, "aLeadK", 0.0)) + lead_lateral = abs(float(getattr(lead, "yRel", 0.0))) + if ( + float(v_ego) > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_EGO_SPEED or + d_rel > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE or + lead_speed > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_SPEED or + lead_lateral > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET or + lead_delta > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA or + lead_accel > LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL + ): + return None + + if ( + float(v_ego) <= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MAX_EGO_SPEED and + d_rel >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_GAP and + lead_speed >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_SPEED and + lead_delta >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_DELTA and + lead_accel >= LOW_SPEED_WEAK_LEAD_ACCEL_CAP_STRONG_DEPART_MIN_LEAD_ACCEL + ): + return None + + distance_factor = float(np.clip( + (d_rel - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE) / + max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_DISTANCE - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_DISTANCE, 0.1), + 0.0, 1.0, + )) + delta_factor = float(np.clip( + (lead_delta - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA) / + max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_DELTA - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_DELTA, 0.1), + 0.0, 1.0, + )) + accel_factor = float(np.clip( + (lead_accel - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL) / + max(LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_LEAD_ACCEL - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_LEAD_ACCEL, 0.1), + 0.0, 1.0, + )) + cap_strength = float(np.clip(0.5 * distance_factor + 0.3 * delta_factor + 0.2 * accel_factor, 0.0, 1.0)) + return LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL + ( + LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MAX_ACCEL - LOW_SPEED_WEAK_LEAD_ACCEL_CAP_MIN_ACCEL + ) * cap_strength + def get_standstill_stopped_lead_guard_cap(self, lead, v_ego, accel_min, stop_distance, release_ready, confident_depart_ready): if lead is None or not lead.status or release_ready or confident_depart_ready: @@ -1777,12 +1852,23 @@ class LongitudinalPlanner: if lead is None or not lead.status: return False + d_rel = max(float(lead.dRel), 0.0) + lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) closing_speed = float(v_ego - lead.vLead) lead_braking = float(lead.aLeadK) < -0.5 + centered_lead = abs(float(getattr(lead, "yRel", 0.0))) <= RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET + if ( + centered_lead and + float(v_ego) <= RAW_LEAD_LOW_SPEED_HOLD_MAX_EGO_SPEED and + lead_speed <= RAW_LEAD_LOW_SPEED_HOLD_MAX_LEAD_SPEED and + d_rel <= RAW_LEAD_LOW_SPEED_HOLD_MAX_DISTANCE and + closing_speed >= RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED + ): + return True + if closing_speed <= RAW_LEAD_SAFETY_MIN_CLOSING_SPEED and not lead_braking: return False - d_rel = max(float(lead.dRel), 0.0) dynamic_distance = max(RAW_LEAD_SAFETY_DISTANCE, 3.0 * float(v_ego)) ttc = d_rel / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf") return d_rel < dynamic_distance and (ttc < RAW_LEAD_SAFETY_TTC or lead_braking) @@ -2317,6 +2403,17 @@ class LongitudinalPlanner: float(sm['carState'].vEgo) <= LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED ) + low_speed_weak_lead_accel_cap = None + if not output_should_stop: + low_speed_weak_lead_accel_caps = [ + cap for cap in ( + self.get_low_speed_weak_lead_accel_cap(self.lead_one, scene_v_ego), + self.get_low_speed_weak_lead_accel_cap(self.lead_two, scene_v_ego), + ) if cap is not None + ] + if low_speed_weak_lead_accel_caps: + low_speed_weak_lead_accel_cap = min(low_speed_weak_lead_accel_caps) + close_stop_active = bool(output_should_stop or vision_low_speed_stop_active) close_stop_hold_cap = None @@ -2540,6 +2637,10 @@ class LongitudinalPlanner: if lead_depart_accel_hold_active: output_a_target = max(output_a_target, lead_depart_accel_floor) + if low_speed_weak_lead_accel_cap is not None: + self.a_desired = min(self.a_desired, low_speed_weak_lead_accel_cap) + output_a_target = min(output_a_target, low_speed_weak_lead_accel_cap) + force_stop_handoff = bool( getattr(sm['starpilotPlan'], 'forcingStop', False) and not lead_control_active and diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 6cdfa0b13..3a46811dd 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -567,6 +567,60 @@ def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_ver assert planner_with_lead.output_a_target < -0.2 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_acc_mode_keeps_close_slow_radar_lead_active_when_tracking_flaps(model_version): + v_ego = 0.96 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) + planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) + sm_no_lead = make_sm( + v_ego, + desired_accel=0.45, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + ) + sm_with_lead = make_sm( + v_ego, + desired_accel=0.45, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=8.15, v_lead=0.09, a_lead=-0.36, radar=True, model_prob=1.0, y_rel=0.1), + ) + sm_no_lead["starpilotPlan"].vCruise = v_ego + 8.0 + sm_with_lead["starpilotPlan"].vCruise = v_ego + 8.0 + + planner_no_lead.update(sm_no_lead, make_toggles(model_version)) + planner_with_lead.update(sm_with_lead, make_toggles(model_version)) + + assert planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) + assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.15 + assert planner_with_lead.output_a_target <= 0.22 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_low_speed_weak_departure_accel_cap_softens_voacc_follow_pulse(model_version): + v_ego = 2.8 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.45, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=16.6, v_lead=2.85, a_lead=0.05, radar=False, model_prob=0.99, y_rel=0.0), + ) + sm["starpilotPlan"].vCruise = v_ego + 10.0 + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_a_target <= 0.22 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_tracking(model_version): v_ego = 21.48 diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index f9799d336..240896a24 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -79,6 +79,20 @@ def should_loud_blindspot_alert_without_lateral(CS, sm, starpilot_toggles) -> bo ) +def get_starpilot_alert_filters(current_alert_types: list[str], clear_event_types: set[str], starpilot_events: Events) -> tuple[list[str], set[str]]: + starpilot_alert_types = list(current_alert_types) + starpilot_clear_event_types = set(clear_event_types) + + # This alert is explicitly allowed while lateral is paused/off. The state + # machine only exposes WARNING while active/AOL, so let this warning through. + if StarPilotEventName.laneChangeBlockedLoud in starpilot_events.names: + if ET.WARNING not in starpilot_alert_types: + starpilot_alert_types.append(ET.WARNING) + starpilot_clear_event_types.discard(ET.WARNING) + + return starpilot_alert_types, starpilot_clear_event_types + + class SelfdriveD: def __init__(self, CP=None): self.params = Params() @@ -691,11 +705,14 @@ class SelfdriveD: self.AM.add_many(self.sm.frame, alerts) self.AM.process_alerts(self.sm.frame, clear_event_types) - starpilot_alerts = self.starpilot_events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric, + starpilot_alert_types, starpilot_clear_event_types = get_starpilot_alert_filters( + self.state_machine.current_alert_types, clear_event_types, self.starpilot_events + ) + starpilot_alerts = self.starpilot_events.create_alerts(starpilot_alert_types, [self.CP, CS, self.sm, self.is_metric, self.state_machine.soft_disable_timer, pers, self.starpilot_toggles]) self.starpilot_AM.add_many(self.sm.frame, starpilot_alerts) - self.starpilot_AM.process_alerts(self.sm.frame, clear_event_types) + self.starpilot_AM.process_alerts(self.sm.frame, starpilot_clear_event_types) def publish_selfdriveState(self, CS): # selfdriveState diff --git a/selfdrive/selfdrived/tests/test_blindspot_alerts.py b/selfdrive/selfdrived/tests/test_blindspot_alerts.py index e2876fcde..520df905d 100644 --- a/selfdrive/selfdrived/tests/test_blindspot_alerts.py +++ b/selfdrive/selfdrived/tests/test_blindspot_alerts.py @@ -1,11 +1,14 @@ from types import SimpleNamespace -from cereal import log -from openpilot.selfdrive.selfdrived.selfdrived import should_loud_blindspot_alert_without_lateral +from cereal import custom, log +from openpilot.selfdrive.selfdrived.alertmanager import AlertManager +from openpilot.selfdrive.selfdrived.events import ET, Events +from openpilot.selfdrive.selfdrived.selfdrived import get_starpilot_alert_filters, should_loud_blindspot_alert_without_lateral LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection +StarPilotEventName = custom.StarPilotOnroadEvent.EventName def _car_state(left_blinker=False, right_blinker=False, left_blindspot=False, right_blindspot=False): @@ -66,3 +69,28 @@ def test_loud_blindspot_alert_without_lateral_handles_paused_pre_lane_change_wit sm = _sm(lane_change_state=LaneChangeState.preLaneChange, lat_active=True, lateral_check=True, pause_lateral=True) assert should_loud_blindspot_alert_without_lateral(CS, sm, _toggles()) + + +def test_loud_blindspot_alert_survives_disabled_warning_filter(): + events = Events(starpilot=True) + events.add(StarPilotEventName.laneChangeBlockedLoud) + + alert_types, clear_event_types = get_starpilot_alert_filters([ET.PERMANENT], {ET.WARNING}, events) + + alerts = events.create_alerts(alert_types) + alert_manager = AlertManager() + alert_manager.add_many(0, alerts) + alert_manager.process_alerts(0, clear_event_types) + + assert alert_manager.current_alert.alert_type == "laneChangeBlockedLoud/warning" + + +def test_disabled_starpilot_warnings_stay_filtered_without_blindspot_event(): + events = Events(starpilot=True) + events.add(StarPilotEventName.noLaneAvailable) + + alert_types, clear_event_types = get_starpilot_alert_filters([ET.PERMANENT], {ET.WARNING}, events) + + assert ET.WARNING not in alert_types + assert ET.WARNING in clear_event_types + assert events.create_alerts(alert_types) == []