mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-27 17:42:04 +08:00
Tom Bombadil
This commit is contained in:
firestar5683
@@ -7,6 +7,16 @@ VISION_LEAD_TRACK_MIN_DISTANCE = 25.0
|
||||
VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75
|
||||
VISION_LEAD_TRACK_CLOSING_GAIN = 0.20
|
||||
VISION_LEAD_TRACK_CLOSING_CAP = 2.50
|
||||
TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.35
|
||||
TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.65
|
||||
TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75
|
||||
TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN = 1.05
|
||||
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START = 2.75
|
||||
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END = 3.10
|
||||
TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET = 0.90
|
||||
TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET = 1.60
|
||||
TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.65
|
||||
TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.75
|
||||
RADARLESS_MATCHED_FOLLOW_MIN_SPEED = 22.0
|
||||
RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED = 2.0
|
||||
RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY = 0.95
|
||||
@@ -56,10 +66,11 @@ def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lea
|
||||
|
||||
|
||||
def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float,
|
||||
v_cruise: float | None = None) -> float:
|
||||
v_cruise: float | None = None, y_rel: float | None = None) -> float:
|
||||
gap_error = lead_distance - desired_gap
|
||||
actual_hw = lead_distance / max(v_ego, 1e-3)
|
||||
desired_hw = desired_gap / max(v_ego, 1e-3)
|
||||
headway_margin = actual_hw - desired_hw
|
||||
|
||||
if v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED:
|
||||
return 0.0
|
||||
@@ -71,14 +82,34 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga
|
||||
# Encourage ACC to treat a tracked lead as the active constraint when we're
|
||||
# hanging far above the requested time gap, but don't override cruise for a
|
||||
# truly distant lead or one we're already closing on decisively.
|
||||
if actual_hw <= max(desired_hw + 0.3, 1.72):
|
||||
if headway_margin <= TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN:
|
||||
return 0.0
|
||||
if actual_hw >= max(desired_hw + 1.6, 3.0):
|
||||
fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN,
|
||||
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw)
|
||||
fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN,
|
||||
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw)
|
||||
if headway_margin >= fade_end_margin:
|
||||
return 0.0
|
||||
if closing_speed > max(2.5, 0.12 * v_ego):
|
||||
return 0.0
|
||||
|
||||
return min(gap_error * 0.65, max(14.0, 0.75 * v_ego))
|
||||
entry_factor = min(1.0, max(0.0, (headway_margin - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN) /
|
||||
max(TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, 1e-3)))
|
||||
exit_factor = 1.0
|
||||
if headway_margin > fade_start_margin:
|
||||
exit_factor = min(1.0, max(0.0, (fade_end_margin - headway_margin) / max(fade_end_margin - fade_start_margin, 1e-3)))
|
||||
|
||||
lateral_factor = 1.0
|
||||
if y_rel is not None:
|
||||
lateral_offset = abs(float(y_rel))
|
||||
if lateral_offset >= TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET:
|
||||
return 0.0
|
||||
if lateral_offset > TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET:
|
||||
lateral_factor = min(1.0, max(0.0, (TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - lateral_offset) /
|
||||
max(TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET, 1e-3)))
|
||||
|
||||
bias_cap = max(14.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego)
|
||||
return min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * entry_factor * exit_factor * lateral_factor
|
||||
|
||||
|
||||
def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float,
|
||||
|
||||
@@ -99,6 +99,12 @@ IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_HEADWAY_ABOVE_TARGET = 0.40
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_LEAD_BRAKE = 0.25
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_PULLAWAY_SPEED = 1.5
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 10.0
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED = 15.0
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET = 0.55
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET = -0.15
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_LEAD_BRAKE = 0.25
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_PULLAWAY_SPEED = 0.75
|
||||
IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX = 10.0
|
||||
|
||||
# Function to get parameter value based on current speed
|
||||
def get_speed_based_param(speed_mph, param_array):
|
||||
@@ -722,6 +728,37 @@ class LongitudinalMpc:
|
||||
|
||||
return prev_source
|
||||
|
||||
def get_identical_radar_duplicate_cruise_bias(self, lead_one, lead_two, v_ego, t_follow):
|
||||
if float(v_ego) < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED:
|
||||
return 0.0
|
||||
if not self.leads_share_identical_radar_track(lead_one, lead_two):
|
||||
return 0.0
|
||||
|
||||
lead = lead_one if lead_one.status else lead_two
|
||||
if lead is None or not lead.status:
|
||||
return 0.0
|
||||
|
||||
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
|
||||
headway_margin = actual_headway - float(t_follow)
|
||||
if headway_margin < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET:
|
||||
return 0.0
|
||||
if headway_margin > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET:
|
||||
return 0.0
|
||||
|
||||
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
|
||||
if lead_brake > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_LEAD_BRAKE:
|
||||
return 0.0
|
||||
|
||||
lead_delta = float(lead.vLead) - float(v_ego)
|
||||
if lead_delta > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_PULLAWAY_SPEED:
|
||||
return 0.0
|
||||
|
||||
return float(np.interp(
|
||||
headway_margin,
|
||||
[IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET, 0.0, IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET],
|
||||
[IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX, IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX * 0.85, 0.0],
|
||||
))
|
||||
|
||||
def set_accel_limits(self, min_a, max_a):
|
||||
# TODO this sets a max accel limit, but the minimum limit is only for cruise decel
|
||||
# needs refactor
|
||||
@@ -769,7 +806,16 @@ class LongitudinalMpc:
|
||||
if optional_far_lead_comfort and tracking_lead and lead_one.status:
|
||||
desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow)
|
||||
closing_speed = max(0.0, v_ego - lead_one.vLead)
|
||||
cruise_obstacle += get_tracked_lead_catchup_bias(v_ego, lead_one.dRel, desired_gap, closing_speed, v_cruise=v_cruise)
|
||||
cruise_obstacle += get_tracked_lead_catchup_bias(
|
||||
v_ego,
|
||||
lead_one.dRel,
|
||||
desired_gap,
|
||||
closing_speed,
|
||||
v_cruise=v_cruise,
|
||||
y_rel=float(getattr(lead_one, "yRel", 0.0)),
|
||||
)
|
||||
if optional_far_lead_comfort:
|
||||
cruise_obstacle += self.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow)
|
||||
if optional_far_lead_comfort:
|
||||
lead_0_bias, lead_1_bias = self.get_near_duplicate_lead_source_hysteresis(prev_source, lead_one, lead_two, v_ego)
|
||||
lead_0_obstacle = lead_0_obstacle + lead_0_bias
|
||||
|
||||
@@ -26,6 +26,20 @@ def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap():
|
||||
assert bias > 14.0
|
||||
|
||||
|
||||
def test_tracked_lead_catchup_bias_reduces_for_laterally_offset_lead():
|
||||
centered = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=0.2)
|
||||
offset = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=1.95)
|
||||
assert centered > 0.0
|
||||
assert offset == 0.0
|
||||
|
||||
|
||||
def test_tracked_lead_catchup_bias_fades_before_very_far_gap_cutoff():
|
||||
near_upper = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9)
|
||||
smaller_gap = get_tracked_lead_catchup_bias(34.0, 96.0, 73.0, 1.9)
|
||||
assert near_upper > 0.0
|
||||
assert near_upper < smaller_gap
|
||||
|
||||
|
||||
def test_tracked_lead_catchup_bias_stays_off_once_at_set_speed():
|
||||
bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1, v_cruise=31.4)
|
||||
assert bias == 0.0
|
||||
|
||||
@@ -3039,6 +3039,36 @@ def test_identical_radar_duplicate_cruise_hold_skips_clear_pullaway():
|
||||
assert sticky is None
|
||||
|
||||
|
||||
def test_identical_radar_duplicate_cruise_bias_penalizes_near_target_follow():
|
||||
v_ego = 23.8
|
||||
t_follow = 1.15
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
lead_one = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0)
|
||||
lead_two = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0)
|
||||
lead_one.radarTrackId = 2493
|
||||
lead_two.radarTrackId = 2493
|
||||
|
||||
bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow)
|
||||
|
||||
assert bias > 0.0
|
||||
|
||||
|
||||
def test_identical_radar_duplicate_cruise_bias_skips_far_pullaway_follow():
|
||||
v_ego = 23.8
|
||||
t_follow = 1.15
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
lead_one = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0)
|
||||
lead_two = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0)
|
||||
lead_one.radarTrackId = 2493
|
||||
lead_two.radarTrackId = 2493
|
||||
|
||||
bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow)
|
||||
|
||||
assert bias == 0.0
|
||||
|
||||
|
||||
def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads():
|
||||
v_ego = 27.0
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
|
||||
@@ -37,6 +37,7 @@ class StarPilotCard:
|
||||
getattr(self.CP, "carFingerprint", None) in (HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC) and
|
||||
bool(hyundai_flags & HyundaiFlags.NON_SCC)
|
||||
)
|
||||
self.hyundai_lkas_aol_requires_engagement = getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID
|
||||
self.hyundai_aol_needs_engagement = self.CP.brand == "hyundai" and not (hyundai_flags & HyundaiFlags.CANFD) and not kia_forte_non_scc
|
||||
self.hyundai_aol_ready = False
|
||||
self.prev_active = False
|
||||
@@ -113,6 +114,12 @@ class StarPilotCard:
|
||||
def update(self, carState, starpilotCarState, sm, starpilot_toggles):
|
||||
self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled")
|
||||
button_event_types = [self._button_type_raw(be) for be in carState.buttonEvents]
|
||||
hyundai_lkas_aol_can_toggle = (
|
||||
not self.hyundai_lkas_aol_requires_engagement or
|
||||
self.hyundai_aol_ready or
|
||||
sm["selfdriveState"].active or
|
||||
carState.cruiseState.enabled
|
||||
)
|
||||
|
||||
if self.hyundai_aol_needs_engagement:
|
||||
if carState.gearShifter in NON_DRIVING_GEARS:
|
||||
@@ -124,6 +131,8 @@ class StarPilotCard:
|
||||
if self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol:
|
||||
for be, be_type in zip(carState.buttonEvents, button_event_types, strict=False):
|
||||
if be_type == ButtonType.lkas and be.pressed and starpilot_toggles.always_on_lateral_lkas:
|
||||
if not hyundai_lkas_aol_can_toggle:
|
||||
continue
|
||||
if self.hyundai_aol_needs_engagement:
|
||||
self.hyundai_aol_ready = True
|
||||
self.always_on_lateral_allowed = not self.always_on_lateral_allowed
|
||||
|
||||
@@ -146,6 +146,51 @@ def test_hyundai_lkas_button_can_start_aol_before_normal_engagement(monkeypatch,
|
||||
assert ret.pauseLateral is False
|
||||
|
||||
|
||||
def test_sonata_hybrid_lkas_button_does_not_start_aol_before_engagement(monkeypatch, tmp_path):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
|
||||
monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
|
||||
|
||||
card = spc.StarPilotCard(
|
||||
SimpleNamespace(brand="hyundai", carFingerprint=spc.HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID),
|
||||
SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
|
||||
)
|
||||
|
||||
car_state = make_car_state(available=False, enabled=False, button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)])
|
||||
starpilot_car_state = SimpleNamespace(distancePressed=False)
|
||||
sm = make_sm()
|
||||
toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True)
|
||||
|
||||
ret = card.update(car_state, starpilot_car_state, sm, toggles)
|
||||
|
||||
assert ret.alwaysOnLateralAllowed is False
|
||||
assert ret.alwaysOnLateralEnabled is False
|
||||
|
||||
|
||||
def test_sonata_hybrid_lkas_button_can_toggle_aol_after_engagement(monkeypatch, tmp_path):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
|
||||
monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
|
||||
|
||||
card = spc.StarPilotCard(
|
||||
SimpleNamespace(brand="hyundai", carFingerprint=spc.HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID),
|
||||
SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
|
||||
)
|
||||
|
||||
starpilot_car_state = SimpleNamespace(distancePressed=False)
|
||||
sm = make_sm()
|
||||
toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True)
|
||||
|
||||
engaged_state = make_car_state(available=True, enabled=True)
|
||||
card.update(engaged_state, starpilot_car_state, sm, toggles)
|
||||
|
||||
lkas_state = make_car_state(available=False, enabled=False, button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)])
|
||||
ret = card.update(lkas_state, starpilot_car_state, sm, toggles)
|
||||
|
||||
assert ret.alwaysOnLateralAllowed is True
|
||||
assert ret.alwaysOnLateralEnabled is True
|
||||
|
||||
|
||||
def test_hyundai_aol_does_not_auto_start_from_cruise_availability(monkeypatch, tmp_path):
|
||||
monkeypatch.setattr(spc, "Params", FakeParams)
|
||||
monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
|
||||
|
||||
Reference in New Issue
Block a user