Refactorious III

This commit is contained in:
firestarsdog
2026-09-10 22:31:48 -04:00
parent ecbd6362f7
commit 2f4b88c231
2 changed files with 249 additions and 216 deletions
@@ -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,90 @@ 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))
# The button arrives before the corresponding cruise-speed update. This + accepts the
# pending 50 mph limit, but its delayed 55 mph set-speed update must not arm an override.
confirm_sm = make_sm(gas_pressed=False, accel_pressed=True, v_cruise_kph=45 * CV.MPH_TO_KPH)
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(45), mph(45), confirm_sm)
controller.update_override(mph(45), 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
delayed_speed_sm = make_sm(gas_pressed=False, v_cruise_kph=55 * CV.MPH_TO_KPH)
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(55), mph(45), delayed_speed_sm)
controller.update_override(mph(55), 0.0, mph(45), 0.0, delayed_speed_sm)
assert not controller.override_slc
assert controller.overridden_speed == 0
# A second fresh + is allowed to establish the override.
second_press_sm = make_sm(gas_pressed=False, accel_pressed=True, v_cruise_kph=60 * CV.MPH_TO_KPH)
controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(60), mph(45), second_press_sm)
controller.update_override(mph(60), 0.0, mph(45), 0.0, second_press_sm)
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 +669,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 +683,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 +705,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 +723,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 +743,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
@@ -431,8 +414,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 +438,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 +456,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 +496,64 @@ 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
# The button and its vCruise update can arrive in adjacent frames. Clear a consumed
# confirmation only after this frame has seen the speed change or button release.
if set_speed_input_consumed and (set_speed_changed or not sm["starpilotCarState"].accelPressed):
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()