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

stuff

Browse Source
This commit is contained in:
firestar5683
2026-06-26 12:53:23 -05:00
parent 4ce9be2752
commit 836b0278eb
5 changed files with 94 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+5 -5
View File
@@ -532,11 +532,11 @@ IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14
IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04
IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5
IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.14
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 0.13
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.035
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.16
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.10
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.18
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 1.05
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.22
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.24
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.14
IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088
IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06
selfdrive/controls/tests/test_conditional_experimental_mode.py
+42
View File
@@ -462,6 +462,48 @@ def test_post_stop_speed_trigger_is_suppressed_after_red_light_release(monkeypat
assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"]
def test_post_stop_slow_lead_trigger_is_suppressed_after_red_light_release(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
toggles.conditional_lead = True
toggles.conditional_slower_lead = True
toggles.conditional_stopped_lead = True
standstill_sm = make_update_sm(standstill=True)
moving_sm = make_update_sm(standstill=False)
now = [100.0]
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0])
def hold_red_light(*args, **kwargs):
cem.stop_light_detected = True
def clear_red_light(*args, **kwargs):
cem.stop_light_detected = False
cem.stop_light_model_detected = False
def detect_slow_lead(*args, **kwargs):
cem.slow_lead_detected = True
monkeypatch.setattr(cem, "stop_sign_and_light", hold_red_light)
cem.update(0.0, standstill_sm, toggles)
assert cem.experimental_mode
monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light)
monkeypatch.setattr(cem, "slow_lead", detect_slow_lead)
launch_v_ego = 8.0 * CV.MPH_TO_MS
now[0] = 100.1
cem.update(launch_v_ego, moving_sm, toggles)
assert not cem.experimental_mode
assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"]
now[0] = 102.5
cem.update(launch_v_ego, moving_sm, toggles)
assert cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"]
def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
selfdrive/controls/tests/test_starpilot_vcruise.py
+38
View File
@@ -170,6 +170,44 @@ def test_slc_lead_drop_relaxed_target_bails_out_for_threatening_lead():
assert relaxed == pytest.approx(raw_target)
def test_slc_lead_drop_relaxed_target_softens_for_far_slower_lead_if_new_limit_is_still_below_lead_speed():
raw_target = 45.0 * CV.MPH_TO_MS
previous_target = 56.0 * CV.MPH_TO_MS
v_ego = 56.0 * CV.MPH_TO_MS
lead = SimpleNamespace(status=True, dRel=113.0, vLead=50.0 * CV.MPH_TO_MS, aLeadK=-0.15)
relaxed = get_slc_lead_drop_relaxed_target(
raw_target,
previous_target,
v_ego,
tracking_lead=True,
lead=lead,
override_active=False,
source="Map Data",
)
assert raw_target < relaxed < previous_target
def test_slc_lead_drop_relaxed_target_still_bails_if_lead_is_slower_than_new_limit():
raw_target = 45.0 * CV.MPH_TO_MS
previous_target = 56.0 * CV.MPH_TO_MS
v_ego = 56.0 * CV.MPH_TO_MS
lead = SimpleNamespace(status=True, dRel=113.0, vLead=39.0 * CV.MPH_TO_MS, aLeadK=-0.05)
relaxed = get_slc_lead_drop_relaxed_target(
raw_target,
previous_target,
v_ego,
tracking_lead=True,
lead=lead,
override_active=False,
source="Map Data",
)
assert relaxed == pytest.approx(raw_target)
def test_slc_lead_drop_relaxed_target_bails_out_without_tracking_lead():
raw_target = 55.0 * CV.MPH_TO_MS
previous_target = 65.0 * CV.MPH_TO_MS
starpilot/controls/lib/conditional_experimental_mode.py
+7 -7
View File
@@ -57,7 +57,7 @@ class ConditionalExperimentalMode:
SLOW_LEAD_FORCE_CLEAR_TIME = 0.75
SLOW_LEAD_MIN_CLOSING_SPEED = 0.75
SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5
POST_STOP_SPEED_TRIGGER_SUPPRESS_TIME = 2.0
POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0
TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS
TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0
@@ -107,7 +107,7 @@ class ConditionalExperimentalMode:
self._prev_ce_status = None
self.prev_standstill = False
self.prev_standstill_stop_hold = False
self.post_stop_speed_trigger_suppress_until = 0.0
self.post_stop_launch_trigger_suppress_until = 0.0
def update(self, v_ego, sm, starpilot_toggles):
now = time.monotonic()
@@ -116,7 +116,7 @@ class ConditionalExperimentalMode:
released_standstill_stop_hold = self.prev_standstill and self.prev_standstill_stop_hold and not standstill
if released_standstill_stop_hold:
self.post_stop_speed_trigger_suppress_until = now + self.POST_STOP_SPEED_TRIGGER_SUPPRESS_TIME
self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self.prev_experimental_mode = False
@@ -204,9 +204,9 @@ class ConditionalExperimentalMode:
return bool(self.stop_light_detected or force_stop_active or model_stopped)
def check_conditions(self, v_ego, sm, starpilot_toggles):
speed_trigger_suppressed = time.monotonic() < self.post_stop_speed_trigger_suppress_until
below_speed = not speed_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead
below_speed_with_lead = not speed_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead
launch_trigger_suppressed = time.monotonic() < self.post_stop_launch_trigger_suppress_until
below_speed = not launch_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead
below_speed_with_lead = not launch_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead
if below_speed or below_speed_with_lead:
self.status_value = CEStatus["SPEED"]
return True
@@ -221,7 +221,7 @@ class ConditionalExperimentalMode:
self.status_value = CEStatus["CURVATURE"]
return True
if starpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31:
if not launch_trigger_suppressed and starpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31:
self.status_value = CEStatus["LEAD"]
return True
starpilot/controls/lib/starpilot_vcruise.py
+2 -2
View File
@@ -16,7 +16,7 @@ STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0
SLC_LEAD_DROP_RELAXATION_MIN_SPEED = 20.0 * CV.MPH_TO_MS
SLC_LEAD_DROP_RELAXATION_MIN_DISTANCE = 30.0
SLC_LEAD_DROP_RELAXATION_MIN_HEADWAY = 1.2
SLC_LEAD_DROP_RELAXATION_MAX_CLOSING_SPEED = 0.35
SLC_LEAD_DROP_RELAXATION_MAX_POST_DROP_CLOSING_SPEED = 0.35
SLC_LEAD_DROP_RELAXATION_MAX_LEAD_BRAKE = 0.25
SLC_LEAD_DROP_RELAXATION_OVERSPEED_BP = [0.0, 5.0 * CV.MPH_TO_MS, 10.0 * CV.MPH_TO_MS, 15.0 * CV.MPH_TO_MS]
SLC_LEAD_DROP_RELAXATION_DECEL_V = [0.7, 0.9, 1.15, 1.35]
@@ -109,7 +109,7 @@ def get_slc_lead_drop_relaxed_target(raw_target, previous_target, v_ego, trackin
return raw_target
v_lead = float(getattr(lead, "vLead", 0.0))
if v_lead < float(v_ego) - SLC_LEAD_DROP_RELAXATION_MAX_CLOSING_SPEED:
if v_lead < float(raw_target) - SLC_LEAD_DROP_RELAXATION_MAX_POST_DROP_CLOSING_SPEED:
return raw_target
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))