From 0913a22dfe356f2a12eeb87d1c1121b0c5cd3a36 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 5 May 2026 20:32:20 -0500 Subject: [PATCH] poodle --- .../opendbc/car/hyundai/hyundaicanfd.py | 4 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 2 + .../opendbc/safety/modes/hyundai_canfd.h | 10 ++++ .../safety/tests/test_hyundai_canfd.py | 10 ++++ .../test_conditional_experimental_mode.py | 50 +++++++++++++++++++ .../lib/conditional_experimental_mode.py | 30 +++++++++-- 6 files changed, 99 insertions(+), 7 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index e63624496..282e2df03 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -165,10 +165,10 @@ IONIQ_6_CRUISE_BUTTONS_BASE_CHECKSUMS = ( IONIQ_6_CRUISE_BUTTONS_LEFT_PADDLE_CHECKSUM_XOR = 0x77 IONIQ_6_CRUISE_BUTTONS_RIGHT_PADDLE_CHECKSUM_XOR = 0xD4 IONIQ_6_REGEN_CONTROL_REQUEST_TAILS = { - # Stock manual left-paddle latch request seen on route 0000025e. (0xA8, 0x0C, 0x12, 0x0E): (0xC0, 0x0C, 0x12, 0x00), - # Stock manual left-paddle latch request seen on route 00000260. (0xA8, 0x0E, 0x07, 0x0E): (0x85, 0x0E, 0x07, 0x0C), + (0x18, 0x10, 0x07, 0x02): (0xB5, 0x10, 0x07, 0x00), + (0xA8, 0x10, 0x07, 0x0E): (0xB5, 0x10, 0x07, 0x00), } diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 86452f949..77b62f610 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -346,6 +346,8 @@ class TestHyundaiFingerprint: @pytest.mark.parametrize(("stock_hex", "expected_tail"), [ ("45421440801f000000000000a865170000a000000080c071a80c120e00000000", bytes.fromhex("c00c1200")), ("47745840801f0000000005006e5e0e0000a000000080c071a80e070e00000000", bytes.fromhex("850e070c")), + ("7aea8140801f000000000000b965140000a000000080c0711810070200000000", bytes.fromhex("b5100700")), + ("32438340801f000000000000b965140000a000000080c071a810070e00000000", bytes.fromhex("b5100700")), ]) def test_ioniq_6_regen_control_message_preserves_stock_frame_and_flips_only_ipedal_request_bytes(self, stock_hex, expected_tail): CP = CarParams.new_message() diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index 0b817b05c..342dc9498 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -98,6 +98,16 @@ static bool hyundai_canfd_ioniq_6_regen_tail_allowed(const CANPacket_t *msg) { (msg->data[26] == 0x07U) && (msg->data[27] == 0x0CU); } + if ((stock24 == 0x18U) && (stock25 == 0x10U) && (stock26 == 0x07U) && (stock27 == 0x02U)) { + return (msg->data[24] == 0xB5U) && (msg->data[25] == 0x10U) && + (msg->data[26] == 0x07U) && (msg->data[27] == 0x00U); + } + + if ((stock24 == 0xA8U) && (stock25 == 0x10U) && (stock26 == 0x07U) && (stock27 == 0x0EU)) { + return (msg->data[24] == 0xB5U) && (msg->data[25] == 0x10U) && + (msg->data[26] == 0x07U) && (msg->data[27] == 0x00U); + } + return false; } diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index bcbc484fb..d900b1aec 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -532,6 +532,16 @@ class TestHyundaiCanfdLKASteeringEVAlwaysIPedal(TestHyundaiCanfdLKASteeringEV): self.assertTrue(self._tx(self._regen_control_msg(byte24=0x85, byte25=0x0E, byte26=0x07, byte27=0x0C, counter=0x58))) self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte25=0x0E, byte26=0x07, byte27=0x00, counter=0x58))) + stock_msg = self._regen_control_msg(byte24=0x18, byte25=0x10, byte26=0x07, byte27=0x02, counter=0x81) + self._rx(stock_msg) + self.assertTrue(self._tx(self._regen_control_msg(byte24=0xB5, byte25=0x10, byte26=0x07, byte27=0x00, counter=0x81))) + self.assertFalse(self._tx(self._regen_control_msg(byte24=0xA8, byte25=0x10, byte26=0x07, byte27=0x00, counter=0x81))) + + stock_msg = self._regen_control_msg(byte24=0xA8, byte25=0x10, byte26=0x07, byte27=0x0E, counter=0x82) + self._rx(stock_msg) + self.assertTrue(self._tx(self._regen_control_msg(byte24=0xB5, byte25=0x10, byte26=0x07, byte27=0x00, counter=0x82))) + self.assertFalse(self._tx(self._regen_control_msg(byte24=0xA8, byte25=0x02, byte26=0x07, byte27=0x0E, counter=0x82))) + self.safety.set_controls_allowed(True) self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x00, counter=0x14))) diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 1519dd05f..49eb5aab4 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -332,6 +332,56 @@ def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead() assert cem.slow_lead_detected +def test_untracked_raw_slow_lead_does_not_self_trigger_without_recent_tracking(monkeypatch): + v_ego = 35 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=False, + lead_status=True, + lead_d_rel=v_ego * 3.0, + lead_v_lead=8.0 * CV.MPH_TO_MS, + lead_model_prob=0.95, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + + monotonic_values = iter([20.0 + 0.1 * i for i in range(24)]) + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) + + for _ in range(12): + cem.slow_lead(toggles, v_ego) + + assert not cem.slow_lead_detected + + +def test_untracked_raw_slow_lead_continuity_expires_after_tracking_flap(monkeypatch): + v_ego = 35 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=v_ego * 3.0, + lead_v_lead=8.0 * CV.MPH_TO_MS, + lead_model_prob=0.95, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + + cem.slow_lead_filter.x = 1.0 + cem.slow_lead_detected = True + monotonic_values = iter([30.0, 30.1, 31.6]) + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) + + cem.slow_lead(toggles, v_ego) + assert cem.slow_lead_detected + + cem.starpilot_planner.tracking_lead = False + cem.starpilot_planner.starpilot_following.slower_lead = False + cem.slow_lead(toggles, v_ego) + assert cem.slow_lead_detected + + cem.slow_lead(toggles, v_ego) + assert not cem.slow_lead_detected + + def test_tracked_highway_mild_closing_lead_does_not_trigger_raw_slow_lead(): v_ego = 65 * CV.MPH_TO_MS cem = make_cem( diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index a9621cb3e..0222e7586 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -51,6 +51,7 @@ class ConditionalExperimentalMode: SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB = 0.85 SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME = 4.0 SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5 + SLOW_LEAD_CONTINUITY_HOLD_TIME = 1.25 SLOW_LEAD_MIN_CLOSING_SPEED = 0.75 SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5 @@ -85,6 +86,8 @@ class ConditionalExperimentalMode: self.curve_detected = False self.slow_lead_detected = False + self.prev_tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False)) + self.slow_lead_continuity_until = 0.0 self.experimental_mode = False self.stop_light_detected = False self.stop_light_model_detected = False @@ -208,7 +211,9 @@ class ConditionalExperimentalMode: self.curve_detected = bool(self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED) def slow_lead(self, starpilot_toggles, v_ego): + now = time.monotonic() lead = self.starpilot_planner.lead_one + tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False)) lead_status = bool(getattr(lead, "status", False)) lead_distance = float(getattr(lead, "dRel", float("inf"))) lead_speed = float(getattr(lead, "vLead", float("inf"))) @@ -219,14 +224,13 @@ class ConditionalExperimentalMode: if not starpilot_toggles.conditional_stopped_lead and v_ego < self.SLOW_LEAD_CONTINUITY_MIN_EGO: self.slow_lead_filter.update(False) self.slow_lead_detected = False + self.slow_lead_continuity_until = 0.0 + self.prev_tracking_lead = tracking_lead return slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead stopped_lead = starpilot_toggles.conditional_stopped_lead and lead_speed < 1 - allow_raw_vision_slow_lead = not self.starpilot_planner.tracking_lead - raw_vision_slow_lead = bool( - starpilot_toggles.conditional_slower_lead and - allow_raw_vision_slow_lead and + vision_slow_lead_candidate = bool( lead_status and lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) and @@ -240,15 +244,31 @@ class ConditionalExperimentalMode: if lead_status and not slower_lead and not stopped_lead and closing_speed < (min_closing_speed * self.SLOW_LEAD_CLEAR_FASTER_FACTOR): self.slow_lead_filter.update(False) self.slow_lead_detected = False + self.slow_lead_continuity_until = 0.0 + self.prev_tracking_lead = tracking_lead return - if self.starpilot_planner.tracking_lead or raw_vision_slow_lead or stopped_lead: + if tracking_lead and (slower_lead or stopped_lead or vision_slow_lead_candidate): + self.slow_lead_continuity_until = now + self.SLOW_LEAD_CONTINUITY_HOLD_TIME + elif self.prev_tracking_lead and not tracking_lead and self.slow_lead_detected and vision_slow_lead_candidate: + self.slow_lead_continuity_until = now + self.SLOW_LEAD_CONTINUITY_HOLD_TIME + + raw_vision_slow_lead = bool( + starpilot_toggles.conditional_slower_lead and + not tracking_lead and + now < self.slow_lead_continuity_until and + vision_slow_lead_candidate + ) + + if tracking_lead or raw_vision_slow_lead or stopped_lead: self.slow_lead_filter.update(slower_lead or raw_vision_slow_lead or stopped_lead) self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold) else: self.slow_lead_filter.update(False) self.slow_lead_detected = False + self.prev_tracking_lead = tracking_lead + def stop_sign_and_light(self, v_ego, sm, model_time): now = time.monotonic()