mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-01 03:22:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

poodle

Browse Source
This commit is contained in:
firestar5683
2026-05-05 20:32:20 -05:00
parent e36a9c6c61
commit 0913a22dfe
6 changed files with 99 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+2 -2
View File
@@ -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),
}
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+2
View File
@@ -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()
opendbc_repo/opendbc/safety/modes/hyundai_canfd.h
+10
View File
@@ -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;
}
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+10
View File
@@ -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)))
selfdrive/controls/tests/test_conditional_experimental_mode.py
+50
View File
@@ -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(
starpilot/controls/lib/conditional_experimental_mode.py
+25 -5
View File
@@ -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()