mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-11 10:43:46 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 56907864c3 |
@@ -75,7 +75,7 @@ def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=Fal
|
||||
"carControl": SimpleNamespace(longActive=long_active),
|
||||
"carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0, vCruise=v_cruise_kph),
|
||||
"liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
|
||||
"mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, waySelectionType=0),
|
||||
"mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, waySelectionType=0, roadName=""),
|
||||
"selfdriveState": SimpleNamespace(enabled=enabled),
|
||||
"starpilotCarState": SimpleNamespace(accelPressed=accel_pressed, decelPressed=decel_pressed),
|
||||
}
|
||||
@@ -315,106 +315,62 @@ def test_display_only_applies_large_delta_guard():
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_new_source_limit_clears_override_until_gas_release():
|
||||
controller = make_controller()
|
||||
def test_set_speed_override_survives_source_changes_and_fallback_until_driver_clears():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=False,
|
||||
speed_limit_controller_override_set_speed=True,
|
||||
speed_limit_priority1="Map Data",
|
||||
speed_limit_priority2="Dashboard",
|
||||
slc_fallback_set_speed=True,
|
||||
)
|
||||
try:
|
||||
controller.source = "Dashboard"
|
||||
controller.target = mph(55)
|
||||
controller.target = mph(45)
|
||||
controller.previous_source = "Dashboard"
|
||||
controller.previous_target = mph(55)
|
||||
controller.overridden_speed = mph(65)
|
||||
controller.previous_target = mph(45)
|
||||
controller.last_valid_limit = mph(45)
|
||||
|
||||
sm = make_sm(gas_pressed=True)
|
||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
||||
|
||||
assert controller.target == pytest.approx(mph(45))
|
||||
assert controller.source == "Dashboard"
|
||||
assert controller.overridden_speed == 0
|
||||
assert not controller.override_slc
|
||||
|
||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
||||
|
||||
assert controller.overridden_speed == 0
|
||||
assert not controller.override_slc
|
||||
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
||||
|
||||
assert controller.overridden_speed == pytest.approx(mph(65))
|
||||
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
|
||||
# --- Dropout / Fallback Test Condition ---
|
||||
controller.starpilot_toggles = make_toggles(slc_fallback_set_speed=True)
|
||||
# No limit available → falls back to v_cruise (75 mph) with source "None".
|
||||
# Override persists because target_to_use resolves to last_valid_limit (45 mph) which is
|
||||
# below overridden_speed (65 mph) — the sticky override_slc chain stays True.
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
||||
# Dashboard 45 -> Map Data 45 is not a new speed zone.
|
||||
map_sm = make_sm(gas_pressed=False)
|
||||
map_sm["mapdOut"].speedLimit = mph(45)
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), map_sm)
|
||||
controller.update_override(mph(55), 0.0, mph(50), 0.0, map_sm)
|
||||
assert controller.source == "Map Data"
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
assert controller.override_slc
|
||||
|
||||
assert controller.target == pytest.approx(mph(75))
|
||||
# A temporary fallback, and even a complete source dropout, do not clear the override.
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.source == "None"
|
||||
assert controller.overridden_speed == pytest.approx(mph(65))
|
||||
assert controller.target == pytest.approx(mph(55))
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
assert controller.override_slc
|
||||
|
||||
# Recovery to a confirmed limit (55 mph) clears the override: this is a genuinely new
|
||||
# speed zone (55 != last_valid 45), so clear_override_for_source_limit fires correctly.
|
||||
controller.update_limits(mph(55), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
|
||||
controller.starpilot_toggles.slc_fallback_set_speed = False
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.target == 0
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
assert controller.override_slc
|
||||
|
||||
assert controller.target == pytest.approx(mph(55))
|
||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.source == "Dashboard"
|
||||
assert controller.overridden_speed == 0
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
assert controller.override_slc
|
||||
|
||||
# Set-speed fallback does not clear passively, but a fresh - to the retained target does.
|
||||
controller.starpilot_toggles.slc_fallback_set_speed = True
|
||||
controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(45), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
assert not controller.override_slc
|
||||
|
||||
# --- Override Clipping Check (set-speed fallback) ---
|
||||
# Separate controller: active override, then fallback to v_cruise that is BELOW the override.
|
||||
# overridden_speed clips to v_cruise (override_slc stays True via sticky chain, but
|
||||
# np.clip clamps overridden_speed to the new target+offset).
|
||||
clip_controller = make_controller(slc_fallback_set_speed=True)
|
||||
try:
|
||||
clip_controller.source = "Dashboard"
|
||||
clip_controller.target = mph(55)
|
||||
clip_controller.previous_source = "Dashboard"
|
||||
clip_controller.previous_target = mph(55)
|
||||
clip_controller.last_valid_limit = mph(55)
|
||||
clip_controller.override_slc = True
|
||||
clip_controller.overridden_speed = mph(65)
|
||||
|
||||
sm_no_gas = make_sm(gas_pressed=False)
|
||||
# v_cruise = 30 mph (below last_valid 55), so target_to_use returns mph(30).
|
||||
# override_slc sticky: overridden_speed=65 > 30+0=30 > 0 — still True from chain.
|
||||
# np.clip(65, 30, 30) = 30, so overridden_speed clips to mph(30).
|
||||
clip_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(30), mph(30), sm_no_gas)
|
||||
clip_controller.update_override(mph(30), 0.0, mph(30), 0.0, sm_no_gas)
|
||||
|
||||
assert clip_controller.target == pytest.approx(mph(30))
|
||||
# Clipped to v_cruise — not locked at mph(55) or mph(65)
|
||||
assert clip_controller.overridden_speed == pytest.approx(mph(30))
|
||||
assert clip_controller.override_slc
|
||||
finally:
|
||||
clip_controller.shutdown()
|
||||
|
||||
# --- Lost Speed Limit (no fallback) clears target to 0 ---
|
||||
# When all limit sources drop to 0 with no fallback, target becomes 0
|
||||
# and override_slc is False (target_to_use=0, chain evaluates False).
|
||||
lost_controller = make_controller(slc_fallback_set_speed=False, slc_fallback_previous_speed_limit=False)
|
||||
try:
|
||||
lost_controller.source = "Dashboard"
|
||||
lost_controller.target = mph(45)
|
||||
lost_controller.previous_source = "Dashboard"
|
||||
lost_controller.previous_target = mph(45)
|
||||
|
||||
sm_on = make_sm(gas_pressed=False)
|
||||
lost_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm_on)
|
||||
lost_controller.update_override(mph(75), 0.0, mph(65), 0.0, sm_on)
|
||||
|
||||
assert lost_controller.target == 0
|
||||
assert lost_controller.overridden_speed == 0
|
||||
assert not lost_controller.override_slc
|
||||
finally:
|
||||
lost_controller.shutdown()
|
||||
assert controller.overridden_speed == 0
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
@@ -487,50 +443,43 @@ def test_unconfirmed_lower_limit_keeps_existing_override():
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_higher_limit_does_not_clear_override():
|
||||
controller = make_controller()
|
||||
def test_set_speed_override_handles_higher_limit_changes():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=False,
|
||||
speed_limit_controller_override_set_speed=True,
|
||||
)
|
||||
try:
|
||||
controller.source = "Dashboard"
|
||||
controller.target = mph(35)
|
||||
controller.previous_source = "Dashboard"
|
||||
controller.previous_target = mph(35)
|
||||
controller.overridden_speed = mph(55)
|
||||
controller.override_slc = True
|
||||
controller.last_valid_limit = mph(35)
|
||||
|
||||
sm = make_sm(gas_pressed=True)
|
||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(55), sm)
|
||||
controller.update_override(mph(75), 0.0, mph(55), 0.0, sm)
|
||||
controller.update_override(mph(35), 0.0, mph(35), 0.0, make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(35), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
|
||||
# A higher limit below the selected override preserves it.
|
||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
|
||||
assert controller.target == pytest.approx(mph(45))
|
||||
assert controller.source == "Dashboard"
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
assert controller.override_slc
|
||||
|
||||
controller_overridden_below = make_controller()
|
||||
try:
|
||||
controller_overridden_below.source = "Dashboard"
|
||||
controller_overridden_below.target = mph(35)
|
||||
controller_overridden_below.previous_source = "Dashboard"
|
||||
controller_overridden_below.previous_target = mph(35)
|
||||
controller_overridden_below.overridden_speed = mph(40)
|
||||
controller_overridden_below.override_slc = True
|
||||
|
||||
sm = make_sm(gas_pressed=True)
|
||||
controller_overridden_below.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(55), sm)
|
||||
controller_overridden_below.update_override(mph(75), 0.0, mph(55), 0.0, sm)
|
||||
|
||||
assert controller_overridden_below.target == pytest.approx(mph(45))
|
||||
assert controller_overridden_below.overridden_speed == 0
|
||||
assert not controller_overridden_below.override_slc
|
||||
finally:
|
||||
controller_overridden_below.shutdown()
|
||||
# A higher effective target that reaches the override clears it without re-arming.
|
||||
controller.update_limits(mph(55), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.target == pytest.approx(mph(55))
|
||||
assert controller.overridden_speed == 0
|
||||
assert not controller.override_slc
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_set_speed_mode_overrides_on_raise_without_gas():
|
||||
# Max Set Speed mode: raising the set speed (+/-) above the posted limit must override
|
||||
# the SLC hold with no gas pedal, targeting the set speed.
|
||||
def test_set_speed_override_follows_driver_wheel_intent():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=False,
|
||||
speed_limit_controller_override_set_speed=True,
|
||||
@@ -540,19 +489,29 @@ def test_set_speed_mode_overrides_on_raise_without_gas():
|
||||
controller.target = mph(45)
|
||||
controller.last_valid_limit = mph(45)
|
||||
|
||||
# Baseline frame at the limit establishes the previous set speed (no rising edge yet).
|
||||
# Gas never creates a persistent wheel override.
|
||||
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(45), 0.0, mph(55), 0.0, make_sm(gas_pressed=True))
|
||||
assert not controller.override_slc
|
||||
assert controller.overridden_speed == 0
|
||||
|
||||
# Driver presses + to 60 (rising edge): override arms and targets the set speed.
|
||||
controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||
# A fresh + above the effective SLC target arms the override.
|
||||
controller.update_override(mph(55), 0.0, mph(55), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(mph(60))
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
|
||||
# Holding 60 with no further press: override stays latched.
|
||||
controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(mph(60))
|
||||
|
||||
controller.update_override(mph(50), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(mph(50))
|
||||
|
||||
# Returning to the effective SLC target ends the persistent override.
|
||||
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||
assert not controller.override_slc
|
||||
assert controller.overridden_speed == 0
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
@@ -576,6 +535,10 @@ def test_set_speed_mode_waits_until_above_slc_target_with_offset():
|
||||
controller.update_override(35 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(35 * CV.KPH_TO_MS)
|
||||
|
||||
controller.update_override(33 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
|
||||
assert not controller.override_slc
|
||||
assert controller.overridden_speed == 0
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
@@ -598,22 +561,82 @@ def test_set_speed_override_clears_on_new_speed_zone():
|
||||
controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
|
||||
# New lower zone (35): update_limits clears the override for the new segment.
|
||||
controller.update_limits(mph(35), datetime.now(timezone.utc), False, mph(60), mph(58), make_sm(gas_pressed=False))
|
||||
# A 1 mph lower zone takes the same-limit fast path but still clears the override.
|
||||
controller.update_limits(mph(44), datetime.now(timezone.utc), False, mph(60), mph(58), make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.target == pytest.approx(mph(35))
|
||||
# Set speed unchanged at 60 -> no rising edge -> override stays cleared (car slows to 35).
|
||||
assert controller.target == pytest.approx(mph(44))
|
||||
# Set speed unchanged at 60 -> no rising edge -> override stays cleared (car slows to 44).
|
||||
assert not controller.override_slc
|
||||
assert controller.overridden_speed == 0
|
||||
|
||||
# A fresh + press (60 -> 65) re-arms against the new limit.
|
||||
controller.update_override(mph(65), 0.0, mph(35), 0.0, make_sm(gas_pressed=False))
|
||||
controller.update_override(mph(65), 0.0, mph(44), 0.0, make_sm(gas_pressed=False))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(mph(65))
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_confirmation_accel_press_does_not_arm_set_speed_override():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=False,
|
||||
speed_limit_controller_override_set_speed=True,
|
||||
speed_limit_confirmation_higher=True,
|
||||
)
|
||||
try:
|
||||
controller.source = "Dashboard"
|
||||
controller.target = mph(45)
|
||||
controller.previous_source = "Dashboard"
|
||||
controller.previous_target = mph(45)
|
||||
controller.last_valid_limit = mph(45)
|
||||
|
||||
controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
|
||||
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(45), mph(45), make_sm(gas_pressed=False))
|
||||
assert controller.source == "None"
|
||||
assert controller.unconfirmed_speed_limit == pytest.approx(mph(50))
|
||||
|
||||
# This + accepts the pending 50 mph limit, but must not also arm a 55 mph override.
|
||||
confirm_sm = make_sm(gas_pressed=False, accel_pressed=True)
|
||||
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(55), mph(45), confirm_sm)
|
||||
controller.update_override(mph(55), 0.0, mph(45), 0.0, confirm_sm)
|
||||
assert controller.source == "Dashboard"
|
||||
assert controller.target == pytest.approx(mph(50))
|
||||
assert not controller.override_slc
|
||||
assert controller.overridden_speed == 0
|
||||
|
||||
# A second fresh + is allowed to establish the override.
|
||||
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(60), mph(45), make_sm(gas_pressed=False, accel_pressed=True))
|
||||
controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False, accel_pressed=True))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(mph(60))
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_adopt_speed_limit_clears_complete_override_state():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=False,
|
||||
speed_limit_controller_override_set_speed=True,
|
||||
)
|
||||
try:
|
||||
controller.source = "Dashboard"
|
||||
controller.target = mph(45)
|
||||
controller.previous_source = "Dashboard"
|
||||
controller.previous_target = mph(45)
|
||||
controller.last_valid_limit = mph(45)
|
||||
controller.override_slc = True
|
||||
controller.overridden_speed = mph(55)
|
||||
controller._slc_adopt_counter = 3
|
||||
controller.starpilot_planner.params_memory.values["SLCAdoptSpeedLimit"] = True
|
||||
|
||||
controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False))
|
||||
|
||||
assert controller.overridden_speed == 0
|
||||
assert not controller.override_slc
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_redneck_set_speed_mode_overrides_in_both_directions():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=False,
|
||||
@@ -638,8 +661,7 @@ def test_redneck_set_speed_mode_overrides_in_both_directions():
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_gas_pedal_mode_ignores_set_speed_without_gas():
|
||||
# Set With Gas Pedal mode: a high set speed alone must NOT override; gas is still required.
|
||||
def test_manual_override_tracks_current_speed_and_ends_on_release():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=True,
|
||||
speed_limit_controller_override_set_speed=False,
|
||||
@@ -653,9 +675,17 @@ def test_gas_pedal_mode_ignores_set_speed_without_gas():
|
||||
assert not controller.override_slc
|
||||
assert controller.overridden_speed == 0
|
||||
|
||||
controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=True))
|
||||
controller.update_override(mph(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True))
|
||||
assert controller.override_slc
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
|
||||
# The temporary override follows the current speed rather than a historical peak.
|
||||
controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=True))
|
||||
assert controller.overridden_speed == pytest.approx(mph(50))
|
||||
|
||||
controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=False))
|
||||
assert not controller.override_slc
|
||||
assert controller.overridden_speed == 0
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
@@ -667,17 +697,17 @@ def test_manual_override_survives_brief_enabled_flicker():
|
||||
controller.target = mph(45)
|
||||
controller.previous_source = "Dashboard"
|
||||
controller.previous_target = mph(45)
|
||||
controller.overridden_speed = mph(55)
|
||||
controller.override_slc = True
|
||||
controller.last_valid_limit = mph(45)
|
||||
controller.update_override(mph(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True))
|
||||
|
||||
disabled_sm = make_sm(gas_pressed=False, enabled=False)
|
||||
disabled_sm = make_sm(gas_pressed=True, enabled=False)
|
||||
for _ in range(int(0.5 / DT_MDL)):
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, disabled_sm)
|
||||
controller.update_override(mph(60), 0.0, mph(55), 0.0, disabled_sm)
|
||||
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
assert controller.override_slc
|
||||
|
||||
controller.update_override(mph(75), 0.0, mph(65), 0.0, make_sm(gas_pressed=False, enabled=True))
|
||||
controller.update_override(mph(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True, enabled=True))
|
||||
|
||||
assert controller.overridden_speed == pytest.approx(mph(55))
|
||||
assert controller.override_slc
|
||||
@@ -685,16 +715,19 @@ def test_manual_override_survives_brief_enabled_flicker():
|
||||
controller.shutdown()
|
||||
|
||||
|
||||
def test_manual_override_clears_after_sustained_disengage():
|
||||
controller = make_controller()
|
||||
def test_override_clears_after_sustained_disengage():
|
||||
controller = make_controller(
|
||||
speed_limit_controller_override_manual=False,
|
||||
speed_limit_controller_override_set_speed=True,
|
||||
)
|
||||
try:
|
||||
controller.source = "Dashboard"
|
||||
controller.target = mph(45)
|
||||
controller.previous_source = "Dashboard"
|
||||
controller.previous_target = mph(45)
|
||||
controller.last_valid_limit = mph(45)
|
||||
controller.overridden_speed = mph(55)
|
||||
controller.override_slc = True
|
||||
controller.override_requires_gas_release = True
|
||||
|
||||
disabled_sm = make_sm(gas_pressed=False, enabled=False)
|
||||
for _ in range(int(1.0 / DT_MDL) + 1):
|
||||
@@ -702,6 +735,5 @@ def test_manual_override_clears_after_sustained_disengage():
|
||||
|
||||
assert controller.overridden_speed == 0
|
||||
assert not controller.override_slc
|
||||
assert not controller.override_requires_gas_release
|
||||
finally:
|
||||
controller.shutdown()
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
# PFEIFER - SLC - Modified by FrogAi
|
||||
import calendar
|
||||
import json
|
||||
import numpy as np
|
||||
import requests
|
||||
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
@@ -51,9 +50,9 @@ class SpeedLimitController:
|
||||
|
||||
self.calling_mapbox = False
|
||||
self.override_slc = False
|
||||
self.override_requires_gas_release = False
|
||||
self.override_disable_timer = 0.0
|
||||
self._prev_v_cruise = None
|
||||
self._set_speed_override_input_consumed = False
|
||||
|
||||
self.denied_target = 0
|
||||
self.map_speed_limit = 0
|
||||
@@ -118,47 +117,25 @@ class SpeedLimitController:
|
||||
def offset(self):
|
||||
return self.get_offset(self.target)
|
||||
|
||||
@property
|
||||
def override_mode_enabled(self):
|
||||
if self.starpilot_toggles is None:
|
||||
return False
|
||||
return self.starpilot_toggles.speed_limit_controller_override_manual or self.starpilot_toggles.speed_limit_controller_override_set_speed
|
||||
|
||||
def override_active(self, v_ego, gas_pressed):
|
||||
target_to_use = self.target_to_use
|
||||
target_with_offset = target_to_use + self.get_offset(target_to_use)
|
||||
if target_with_offset <= 0 or not self.override_mode_enabled:
|
||||
return False
|
||||
bidirectional_set_speed = (
|
||||
getattr(self.starpilot_toggles, "redneck_cruise", False) and
|
||||
getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False)
|
||||
)
|
||||
return (
|
||||
(bidirectional_set_speed and self.overridden_speed > 0) or
|
||||
self.overridden_speed > target_with_offset or
|
||||
(gas_pressed and v_ego > target_with_offset)
|
||||
)
|
||||
|
||||
def low_vision_limit_filtered(self, limit):
|
||||
return (
|
||||
getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_filter", False) and
|
||||
0 < limit <= max(getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_threshold", 0), 0)
|
||||
)
|
||||
|
||||
def clear_override_for_source_limit(self, desired_source, desired_target, had_override):
|
||||
if desired_source == "None" or desired_target <= 0:
|
||||
return
|
||||
if not had_override and self.overridden_speed <= 0:
|
||||
return
|
||||
if abs(desired_target - self.last_valid_limit) < 0.1:
|
||||
return
|
||||
|
||||
# A new posted limit starts a new segment, so the previous segment's gas override
|
||||
# should not carry through until the driver releases and reapplies the pedal.
|
||||
def clear_override(self):
|
||||
self.override_slc = False
|
||||
self.overridden_speed = 0
|
||||
if had_override:
|
||||
self.override_requires_gas_release = True
|
||||
|
||||
def clear_persistent_override_for_limit_change(self, previous_limit, new_limit):
|
||||
if not self.starpilot_toggles.speed_limit_controller_override_set_speed or self.overridden_speed <= 0:
|
||||
return
|
||||
if previous_limit <= 0 or new_limit <= 0 or abs(new_limit - previous_limit) < 0.1:
|
||||
return
|
||||
|
||||
new_target_with_offset = new_limit + self.get_offset(new_limit)
|
||||
if new_limit < previous_limit or self.overridden_speed <= new_target_with_offset:
|
||||
self.clear_override()
|
||||
|
||||
def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm):
|
||||
if not self.starpilot_planner.gps_valid or not self.mapbox_token or abs(sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45:
|
||||
@@ -295,10 +272,15 @@ class SpeedLimitController:
|
||||
|
||||
def handle_limit_change(self, desired_source, desired_target, current_road_name, v_ego, sm):
|
||||
self.speed_limit_changed_timer += DT_MDL
|
||||
had_override = self.override_active(v_ego, sm["carState"].gasPressed)
|
||||
previous_limit = self.last_valid_limit if self.last_valid_limit > 0 else self.target
|
||||
|
||||
long_active = sm["carControl"].longActive
|
||||
speed_limit_accepted = sm["starpilotCarState"].accelPressed and long_active
|
||||
accepted_by_accel_button = sm["starpilotCarState"].accelPressed and long_active
|
||||
confirmation_required = desired_source != "None" and (
|
||||
(desired_target < self.target and self.starpilot_toggles.speed_limit_confirmation_lower) or
|
||||
(desired_target > self.target and self.starpilot_toggles.speed_limit_confirmation_higher)
|
||||
)
|
||||
speed_limit_accepted = accepted_by_accel_button
|
||||
if not speed_limit_accepted and self._slc_adopt_counter % 4 == 0:
|
||||
speed_limit_accepted = self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted")
|
||||
speed_limit_denied = sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30 and long_active)
|
||||
@@ -309,7 +291,9 @@ class SpeedLimitController:
|
||||
if speed_limit_accepted:
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
self.clear_override_for_source_limit(desired_source, desired_target, had_override)
|
||||
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||
if accepted_by_accel_button and confirmation_required:
|
||||
self._set_speed_override_input_consumed = True
|
||||
|
||||
self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
|
||||
|
||||
@@ -323,13 +307,12 @@ class SpeedLimitController:
|
||||
elif desired_target < self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_lower):
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
self.clear_override_for_source_limit(desired_source, desired_target, had_override)
|
||||
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||
|
||||
elif desired_target > self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_higher):
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
if 0 < self.overridden_speed <= self.target + self.get_offset(self.target):
|
||||
self.clear_override_for_source_limit(desired_source, desired_target, had_override)
|
||||
self.clear_persistent_override_for_limit_change(previous_limit, desired_target)
|
||||
|
||||
elif desired_target == self.target:
|
||||
self.source = desired_source
|
||||
@@ -349,6 +332,7 @@ class SpeedLimitController:
|
||||
self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target))
|
||||
|
||||
def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, display_only=False):
|
||||
self._set_speed_override_input_consumed = False
|
||||
self.update_map_speed_limit(v_ego, sm)
|
||||
vision_enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False)
|
||||
self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if vision_enabled else 0
|
||||
@@ -431,8 +415,7 @@ class SpeedLimitController:
|
||||
if display_only:
|
||||
self.speed_limit_changed_timer = 0
|
||||
self.unconfirmed_speed_limit = 0
|
||||
self.overridden_speed = 0
|
||||
self.override_requires_gas_release = False
|
||||
self.clear_override()
|
||||
|
||||
if desired_target >= 1:
|
||||
self.source = desired_source
|
||||
@@ -456,6 +439,8 @@ class SpeedLimitController:
|
||||
self.speed_limit_changed_timer = 0
|
||||
self.unconfirmed_speed_limit = 0
|
||||
if desired_source != self.source or desired_target != self.target:
|
||||
if not is_fallback:
|
||||
self.clear_persistent_override_for_limit_change(current_speed, desired_target)
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
if desired_source != "None" and desired_target > 0:
|
||||
@@ -472,7 +457,7 @@ class SpeedLimitController:
|
||||
if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"):
|
||||
self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")
|
||||
if desired_target > 0:
|
||||
self.overridden_speed = 0
|
||||
self.clear_override()
|
||||
self.denied_target = 0
|
||||
self.source = desired_source
|
||||
self.target = desired_target
|
||||
@@ -512,55 +497,61 @@ class SpeedLimitController:
|
||||
self.map_speed_limit = self.next_speed_limit
|
||||
|
||||
def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm):
|
||||
# Detect +/- changes on the raw set speed (button-driven, no cluster jitter). Requiring a
|
||||
# fresh edge is what makes the override clear per speed zone — once a new posted limit wipes
|
||||
# it, a steady set speed will not re-arm.
|
||||
# Detect +/- changes on the raw set speed (button-driven, no cluster jitter). A fresh edge
|
||||
# keeps a cleared override from re-arming while the selected speed stays high.
|
||||
prev_v_cruise = self._prev_v_cruise
|
||||
self._prev_v_cruise = v_cruise
|
||||
set_speed_changed = prev_v_cruise is not None and abs(v_cruise - prev_v_cruise) > SET_SPEED_RAISE_EPS
|
||||
set_speed_raised = prev_v_cruise is not None and v_cruise > prev_v_cruise + SET_SPEED_RAISE_EPS
|
||||
set_speed_input_consumed = self._set_speed_override_input_consumed
|
||||
self._set_speed_override_input_consumed = False
|
||||
|
||||
if not sm["selfdriveState"].enabled:
|
||||
self.override_disable_timer += DT_MDL
|
||||
if self.override_disable_timer >= SLC_OVERRIDE_DISABLE_CLEAR_TIME:
|
||||
self.override_slc = False
|
||||
self.overridden_speed = 0
|
||||
self.override_requires_gas_release = False
|
||||
self.clear_override()
|
||||
return
|
||||
|
||||
self.override_disable_timer = 0.0
|
||||
|
||||
if not sm["carState"].gasPressed:
|
||||
self.override_requires_gas_release = False
|
||||
|
||||
target_to_use = self.target_to_use
|
||||
offset = self.get_offset(target_to_use)
|
||||
target_with_offset = target_to_use + self.get_offset(target_to_use)
|
||||
|
||||
if self.starpilot_toggles.speed_limit_controller_override_manual:
|
||||
if sm["carState"].gasPressed and v_ego > target_with_offset > 0:
|
||||
self.override_slc = True
|
||||
self.overridden_speed = v_ego + v_ego_diff
|
||||
else:
|
||||
self.clear_override()
|
||||
return
|
||||
|
||||
if not self.starpilot_toggles.speed_limit_controller_override_set_speed:
|
||||
self.clear_override()
|
||||
return
|
||||
|
||||
set_speed = v_cruise + v_cruise_diff
|
||||
bidirectional_set_speed = (
|
||||
getattr(self.starpilot_toggles, "redneck_cruise", False) and
|
||||
getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False)
|
||||
)
|
||||
self.override_slc = self.override_slc and (
|
||||
(bidirectional_set_speed and self.overridden_speed > 0) or
|
||||
self.overridden_speed > target_to_use + offset > 0
|
||||
)
|
||||
self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > target_to_use + offset > 0
|
||||
# Redneck Max Set Speed mode uses +/- as a direct, bidirectional SLC override. The normal
|
||||
# mode retains its existing upward-only behavior for full-long cars.
|
||||
self.override_slc |= (
|
||||
self.starpilot_toggles.speed_limit_controller_override_set_speed and
|
||||
target_to_use + offset > 0 and
|
||||
set_speed > 0 and
|
||||
((bidirectional_set_speed and set_speed_changed) or
|
||||
(not bidirectional_set_speed and set_speed_raised and set_speed > target_to_use + offset > 0))
|
||||
)
|
||||
bidirectional_set_speed = getattr(self.starpilot_toggles, "redneck_cruise", False)
|
||||
if bidirectional_set_speed:
|
||||
if self.override_slc and set_speed > 0:
|
||||
self.overridden_speed = set_speed
|
||||
elif target_with_offset > 0 and set_speed > 0 and set_speed_changed and not set_speed_input_consumed:
|
||||
self.override_slc = True
|
||||
self.overridden_speed = set_speed
|
||||
else:
|
||||
self.clear_override()
|
||||
return
|
||||
|
||||
if self.override_slc:
|
||||
if self.starpilot_toggles.speed_limit_controller_override_manual:
|
||||
if sm["carState"].gasPressed:
|
||||
self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed)
|
||||
self.overridden_speed = float(np.clip(self.overridden_speed, target_to_use + offset, v_cruise + v_cruise_diff))
|
||||
elif self.starpilot_toggles.speed_limit_controller_override_set_speed:
|
||||
# A fallback transition alone preserves the override; a fresh set-speed change may clear it.
|
||||
if set_speed <= 0 or (
|
||||
target_with_offset > 0 and set_speed <= target_with_offset and
|
||||
(self.source != "None" or set_speed_changed)
|
||||
):
|
||||
self.clear_override()
|
||||
else:
|
||||
self.overridden_speed = set_speed
|
||||
elif target_with_offset > 0 and set_speed_raised and set_speed > target_with_offset and not set_speed_input_consumed:
|
||||
self.override_slc = True
|
||||
self.overridden_speed = set_speed
|
||||
else:
|
||||
self.overridden_speed = 0
|
||||
self.clear_override()
|
||||
|
||||
Reference in New Issue
Block a user