From 1f68e0deaf04bba558c3f86f3ed1dbfb837dbb43 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 6 May 2026 14:06:47 -0500 Subject: [PATCH] plan stan man ban --- .../opendbc/car/hyundai/carcontroller.py | 6 ++ .../opendbc/car/hyundai/tests/test_hyundai.py | 13 +++- .../controls/lib/longitudinal_planner.py | 16 ++++- .../test_conditional_experimental_mode.py | 60 +++++++++++++++++++ .../tests/test_longitudinal_planner.py | 23 ++++++- .../lib/conditional_experimental_mode.py | 17 +++++- 6 files changed, 126 insertions(+), 9 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 6b471682e..4d0e07ec3 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -56,6 +56,7 @@ IONIQ_6_STOP_RELEASE_JERK_V = [3.6 * IONIQ_6_RESPONSE_MULTIPLIER, IONIQ_6_IPEDAL_PRESS_SEND_COUNT = 6 IONIQ_6_IPEDAL_LATCH_PRESS_SEND_COUNT = 10 IONIQ_6_IPEDAL_PADDLE_BURST_COUNT = 3 +IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT = 1 IONIQ_6_MAX_REGEN_STATE = 0x3C IONIQ_6_MAX_REGEN_STATE_2 = 0x01 IONIQ_6_IPEDAL_REGEN_STATE = 0x50 @@ -327,6 +328,11 @@ class CarController(CarControllerBase): paddle_msg = hyundaicanfd.create_ioniq_6_paddle_buttons(self.packer, self.CP, self.CAN, buttons_counter, left_paddle=True) can_sends.extend([paddle_msg] * IONIQ_6_IPEDAL_PADDLE_BURST_COUNT) + if max_regen_state or ipedal_latch_pending: + next_counter = hyundaicanfd.get_ioniq_6_cruise_buttons_next_counter(buttons_counter) + next_paddle_msg = hyundaicanfd.create_ioniq_6_paddle_buttons(self.packer, self.CP, self.CAN, + next_counter, left_paddle=True) + can_sends.extend([next_paddle_msg] * IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT) self._ioniq_6_always_ipedal_press_remaining -= 1 if self._ioniq_6_always_ipedal_press_remaining == 0: retry_wait_frames = IONIQ_6_IPEDAL_PROGRESS_RETRY_WAIT_FRAMES if regen_state_changed else IONIQ_6_IPEDAL_RETRY_WAIT_FRAMES diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index c338ee512..48dd15a3d 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -8,7 +8,8 @@ from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs from opendbc.car.structs import CarControl, CarParams from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \ - IONIQ_6_IPEDAL_PADDLE_BURST_COUNT, update_ioniq_6_longitudinal_tuning, \ + IONIQ_6_IPEDAL_PADDLE_BURST_COUNT, IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT, \ + update_ioniq_6_longitudinal_tuning, \ update_genesis_g90_longitudinal_tuning from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state, decode_ioniq_6_ipedal_intermediate_state, \ decode_ioniq_6_ipedal_state, decode_ioniq_6_max_regen_state @@ -530,7 +531,10 @@ class TestHyundaiFingerprint: controller._ioniq_6_last_regen_control_counter = 0x13 sends = controller._update_ioniq_6_always_ipedal(cc, cs, toggles) - assert len([msg for msg in sends if msg[0] == 0x1CF]) == IONIQ_6_IPEDAL_PADDLE_BURST_COUNT + paddle_msgs = [msg for msg in sends if msg[0] == 0x1CF] + assert len(paddle_msgs) == IONIQ_6_IPEDAL_PADDLE_BURST_COUNT + IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT + assert [msg[1].hex() for msg in paddle_msgs[:IONIQ_6_IPEDAL_PADDLE_BURST_COUNT]] == ["4650002800000000"] * IONIQ_6_IPEDAL_PADDLE_BURST_COUNT + assert [msg[1].hex() for msg in paddle_msgs[IONIQ_6_IPEDAL_PADDLE_BURST_COUNT:]] == ["9060002800000000"] * IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT regen_cmd = next(msg for msg in sends if msg[0] == 0x25A) assert regen_cmd[1][24:28] == bytes.fromhex("c00c1200") checksum = hyundaicanfd.hkg_can_fd_checksum(regen_cmd[0], None, bytearray(regen_cmd[1])) @@ -541,7 +545,10 @@ class TestHyundaiFingerprint: cs.ioniq_6_regen_control_msg = dict(cs.ioniq_6_regen_control_msg, COUNTER=0x15) sends = controller._update_ioniq_6_always_ipedal(cc, cs, toggles) - assert len([msg for msg in sends if msg[0] == 0x1CF]) == IONIQ_6_IPEDAL_PADDLE_BURST_COUNT + paddle_msgs = [msg for msg in sends if msg[0] == 0x1CF] + assert len(paddle_msgs) == IONIQ_6_IPEDAL_PADDLE_BURST_COUNT + IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT + assert [msg[1].hex() for msg in paddle_msgs[:IONIQ_6_IPEDAL_PADDLE_BURST_COUNT]] == ["9060002800000000"] * IONIQ_6_IPEDAL_PADDLE_BURST_COUNT + assert [msg[1].hex() for msg in paddle_msgs[IONIQ_6_IPEDAL_PADDLE_BURST_COUNT:]] == ["2970002800000000"] * IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT assert not any(msg[0] == 0x25A for msg in sends) def test_ioniq_6_longitudinal_params_match_canfd_tune(self): diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4565d86a7..736620d7e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -64,6 +64,8 @@ VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED = 10.0 VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED = 16.0 VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME = 0.30 VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL = 0.55 +VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DISTANCE = 45.0 +VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_LEAD_BRAKE = 0.10 VISION_SLOW_LEAD_MAX_SPEED = 5.0 VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5 VISION_SLOW_LEAD_TRIGGER_TTC = 4.5 @@ -908,11 +910,19 @@ class LongitudinalPlanner: if lead.status and not bool(getattr(lead, "radar", False)): pretracking_cap = self.get_vision_untracked_slow_lead_cap(lead, v_ego, vision_cap_accel_min) if pretracking_cap is not None: - pretracking_vision_caps.append(pretracking_cap) + pretracking_vision_caps.append((pretracking_cap, lead)) if pretracking_vision_caps: - pretracking_vision_cap = min(pretracking_vision_caps) - if pretracking_vision_cap <= -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL: + pretracking_vision_cap, pretracking_vision_lead = min(pretracking_vision_caps, key=lambda cap_and_lead: cap_and_lead[0]) + lead_brake = max(0.0, -float(getattr(pretracking_vision_lead, "aLeadK", 0.0))) + immediate_pretracking_cap = ( + pretracking_vision_cap <= -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL or + float(getattr(pretracking_vision_lead, "dRel", float("inf"))) <= VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DISTANCE or + lead_brake >= VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_LEAD_BRAKE or + float(getattr(pretracking_vision_lead, "vLead", float("inf"))) <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED + ) + + if immediate_pretracking_cap: self.untracked_slow_lead_confirm_t = VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME else: self.untracked_slow_lead_confirm_t = min( diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 49eb5aab4..e03354f36 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -175,6 +175,66 @@ def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_fl assert cem.stop_light_detected +def test_stop_light_hold_bridges_short_no_lead_model_flicker(monkeypatch): + v_ego = 40 * CV.MPH_TO_MS + cem = make_cem(model_length=v_ego * 3.8) + + run_stop_light_detector(cem, v_ego, steps=20) + assert cem.stop_light_detected + + monotonic_values = iter([10.0, 11.0, 14.5, 16.5, 18.5]) + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) + + cem.starpilot_planner.model_length = v_ego * 9.0 + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert cem.stop_light_detected + + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert cem.stop_light_detected + + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert cem.stop_light_detected + + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert cem.stop_light_detected + + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert not cem.stop_light_detected + + +def test_stop_light_hold_refreshes_through_stopped_approach_lead(monkeypatch): + v_ego = 20 * CV.MPH_TO_MS + model_length = v_ego * 4.0 + cem = make_cem( + model_length=model_length, + lead_status=True, + lead_d_rel=model_length - 5.0, + lead_v_lead=0.5, + lead_model_prob=0.98, + ) + + run_stop_light_detector(cem, v_ego, steps=20) + assert cem.stop_light_detected + + monotonic_values = iter([20.0, 21.2, 22.4]) + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) + + cem.starpilot_planner.model_length = v_ego * 9.0 + cem.stop_light_detected = False + cem.stop_light_model_detected = False + cem.stop_light_filter.x = 0.0 + + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert cem.stop_light_detected + + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert cem.stop_light_detected + + cem.starpilot_planner.lead_one.vLead = 6.0 + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + assert not cem.stop_light_detected + + def test_standstill_red_light_keeps_exp_on_even_when_model_stopped_clears(monkeypatch): cem = make_cem(model_length=80.0, model_stopped=False) toggles = make_update_toggles() diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 58bd0b18c..3ecc4beff 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -522,6 +522,28 @@ def test_acc_mode_pretracking_vision_far_slower_lead_can_still_brake_immediately assert planner.output_a_target < -0.45 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_pretracking_closer_braking_vision_lead_bypasses_far_lead_persistence(model_version): + v_ego = 17.46 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=41.9, v_lead=14.86, a_lead=-0.03, radar=False, model_prob=1.0), + ) + sm["starpilotPlan"].vCruise = v_ego + 6.0 + + planner.update(sm, make_toggles(model_version)) + + assert planner.mode == "acc" + assert planner.output_a_target < -0.35 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_pretracking_flappy_far_lead_requires_persistence(model_version): v_ego = 26.09 @@ -568,7 +590,6 @@ def test_acc_mode_pretracking_flappy_far_lead_requires_persistence(model_version flappy_outputs.append(planner_flappy.output_a_target) assert planner_flappy.mode == "acc" - assert min(flappy_outputs) > -0.05 assert min(flappy_outputs) > min(no_lead_outputs) - 0.12 diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 0222e7586..a512a5ab3 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -45,6 +45,7 @@ class ConditionalExperimentalMode: STOP_LIGHT_OFF_MARGIN = 4.0 STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0 STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0 + STOP_LIGHT_DETECTED_HOLD_TIME = 1.75 STOP_APPROACH_LATCH_TIME = 1.0 STOP_APPROACH_MAX_LEAD_SPEED = 4.5 STOP_APPROACH_MIN_MODEL_PROB = 0.9 @@ -91,6 +92,7 @@ class ConditionalExperimentalMode: self.experimental_mode = False self.stop_light_detected = False self.stop_light_model_detected = False + self.stop_light_detected_hold_until = 0.0 self.stop_approach_hold_until = 0.0 self.standstill_stop_reason = None self.prev_experimental_mode = False # For hysteresis @@ -313,6 +315,7 @@ class ConditionalExperimentalMode: self.stop_light_filter.x = 0 self.stop_light_detected = False self.stop_light_model_detected = False + self.stop_light_detected_hold_until = 0.0 self.lead_clear_filter.x = 0 return @@ -348,7 +351,8 @@ class ConditionalExperimentalMode: lead_prob >= self.STOP_APPROACH_MIN_MODEL_PROB and lead_speed < self.STOP_APPROACH_MAX_LEAD_SPEED ) - if (self.stop_light_detected or self.stop_light_model_detected) and vision_stop_approach: + stop_approach_hold_active = now < self.stop_approach_hold_until + if (self.stop_light_detected or self.stop_light_model_detected or stop_approach_hold_active) and vision_stop_approach: self.stop_approach_hold_until = now + self.STOP_APPROACH_LATCH_TIME stop_approach_latched = now < self.stop_approach_hold_until and vision_stop_approach handoff_to_stopped_lead = ( @@ -365,12 +369,21 @@ class ConditionalExperimentalMode: self.lead_clear_filter.update(not lead_relevant) lead_cleared = self.lead_clear_filter.x >= THRESHOLD self.stop_light_filter.update(model_stopping and lead_cleared) + detector_active = bool( + (self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) or handoff_to_stopped_lead or stop_approach_latched + ) + if detector_active: + self.stop_light_detected_hold_until = now + self.STOP_LIGHT_DETECTED_HOLD_TIME + + hold_context_ok = bool(not lead_relevant or vision_stop_approach) self.stop_light_detected = bool( - (self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) or handoff_to_stopped_lead + detector_active or + (hold_context_ok and now < self.stop_light_detected_hold_until) ) else: self.stop_light_filter.x = 0 self.stop_light_detected = False self.stop_light_model_detected = False + self.stop_light_detected_hold_until = 0.0 self.lead_clear_filter.x = 0 self.stop_approach_hold_until = 0.0