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

plan stan man ban

Browse Source
This commit is contained in:
firestar5683
2026-05-06 14:06:47 -05:00
parent c79e2b3ad7
commit 1f68e0deaf
6 changed files with 126 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+6
View File
@@ -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
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+10 -3
View File
@@ -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):
selfdrive/controls/lib/longitudinal_planner.py
+13 -3
View File
@@ -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(
selfdrive/controls/tests/test_conditional_experimental_mode.py
+60
View File
@@ -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()
selfdrive/controls/tests/test_longitudinal_planner.py
+22 -1
View File
@@ -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
starpilot/controls/lib/conditional_experimental_mode.py
+15 -2
View File
@@ -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