IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ d2ce8a8

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-28 08:35:52 -05:00
parent 9206164707
commit ee1dca77c7
210 changed files with 19726 additions and 455 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
iqpilot/selfdrive/controls/lib/slc_vcruise.py
View File

@@ -189,6 +189,8 @@ class SLCVCruise:
self._user_max_speed = v_cruise_cluster
else:
self._user_max_speed = 0.0
if not slc_params["speed_limit_controller"]:
self.slc.reset_override(sm)
if slc_params["speed_limit_controller"]:
self.slc.update_limits(dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, slc_params)
self.pending_events = list(getattr(self.slc, 'pending_events', []))

46
iqpilot/selfdrive/controls/lib/speed_limit_controller.py
View File

@@ -261,10 +261,11 @@ class IQSpeedLimitAssist:
for btn in sm["carState"].buttonEvents:
if btn.pressed:
continue
if is_lower and btn.type in CONFIRM_LOWER_BUTTONS:
button_type = getattr(btn.type, "raw", btn.type)
if is_lower and button_type in CONFIRM_LOWER_BUTTONS:
confirmed = True
break
elif not is_lower and btn.type in CONFIRM_HIGHER_BUTTONS:
elif not is_lower and button_type in CONFIRM_HIGHER_BUTTONS:
confirmed = True
break
except (AttributeError, TypeError):
@@ -314,6 +315,10 @@ class SpeedLimitController:
self.override_slc = False
self.overridden_speed = 0.0
self._last_override_request_id = 0
self._blocked_override_gesture = 0
self._override_limit = None
self._override_set_speed = False
self._resolved_limit = 0.0
self._resolved_source = "None"
@@ -807,9 +812,44 @@ class SpeedLimitController:
self.pending_events.append(EventNameIQ.constructionZoneDetected)
self._czone_was_limiting = czone_limiting
def reset_override(self, sm):
self.override_slc = False
self.overridden_speed = 0.0
self._last_override_request_id = int(getattr(sm["iqCarState"], "slcSetSpeedRequestId", 0))
self._blocked_override_gesture = int(getattr(sm["iqCarState"], "slcSetSpeedGestureId", 0))
self._override_limit = None
def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm, slc_params, is_metric):
offset = self.get_offset(is_metric)
target = self._assist.target
set_speed_override = slc_params.get("speed_limit_controller_override_set_speed", False)
mode_changed = set_speed_override != self._override_set_speed
self._override_set_speed = set_speed_override
if set_speed_override:
request_id = int(getattr(sm["iqCarState"], "slcSetSpeedRequestId", 0))
gesture_id = int(getattr(sm["iqCarState"], "slcSetSpeedGestureId", 0))
request_speed = float(getattr(sm["iqCarState"], "slcSetSpeedRequestKph", 0.0)) * CV.KPH_TO_MS
new_request = request_id != self._last_override_request_id
limit = (target, self._assist.source)
reset = (mode_changed or limit != self._override_limit or self._assist.just_confirmed or
self._assist.state == SpeedLimitAssistState.preActive or
not bool(getattr(sm["selfdriveState"], "enabled", False)) or target <= 0 or self._resolved_source == "Construction")
cruise_speed = v_cruise + v_cruise_diff
above_limit = cruise_speed > target + offset + 1e-3
if reset or (self.override_slc and not above_limit):
self.reset_override(sm)
elif above_limit:
driver_increase = new_request and gesture_id != self._blocked_override_gesture and request_speed > target + offset + 1e-3
gas_override = sm["carState"].gasPressed and v_ego > target + offset
self.override_slc = self.override_slc or driver_increase or gas_override
self.overridden_speed = cruise_speed if self.override_slc else 0.0
self._last_override_request_id = request_id
self._override_limit = limit
return
if mode_changed:
self.reset_override(sm)
self.override_slc = self.overridden_speed > target + offset > 0
self.override_slc |= sm["carState"].gasPressed and v_ego > target + offset > 0
@@ -820,7 +860,5 @@ class SpeedLimitController:
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 + offset, v_cruise + v_cruise_diff))
elif slc_params.get("speed_limit_controller_override_set_speed", False):
self.overridden_speed = v_cruise + v_cruise_diff
else:
self.overridden_speed = 0.0

202
iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py
View File

@@ -5,8 +5,13 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
from datetime import datetime
from types import SimpleNamespace
import pytest
from iqpilot.cereal import car, custom
from iqpilot.common.constants import CV
from iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
from iqpilot.selfdrive.car.cruise import VCruiseHelper
from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
from iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
from iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
@@ -61,6 +66,9 @@ class _FakeSLC:
def update_override(self, *_args, **_kwargs):
self.update_override_calls += 1
def reset_override(self, _sm):
self.overridden_speed = 0.0
def get_offset(self, _is_metric):
return self._offset
@@ -261,6 +269,200 @@ def test_slc_vcruise_does_not_auto_raise_when_higher_confirmation_enabled():
assert out == v_cruise
@pytest.fixture(params=[True, False], ids=["metric", "imperial"])
def set_speed_slc(request, monkeypatch):
monkeypatch.setattr("iqpilot.selfdrive.controls.lib.slc_vcruise.Params", FakeParams)
slc = SLCVCruise()
slc._maybe_log_debug = lambda *_args: None
slc.slc.update_gps = lambda _sm: None
slc.slc._resolver.update_map_data = lambda *_args: None
params = _base_slc_params_controller() | {
"speed_limit_controller": True,
"speed_limit_mode": 3,
"show_speed_limits": False,
"is_metric": request.param,
"slc_online_filler": False,
"slc_fallback_experimental_mode": False,
"speed_limit_controller_override_manual": False,
"speed_limit_controller_override_set_speed": True,
}
slc._get_slc_params = lambda: params
unit = CV.KPH_TO_MS if request.param else CV.MPH_TO_MS
slc.slc._resolver.map_speed_limit = 50 * unit
sm = _FakeSM(_build_sm(v_ego_cluster=50 * unit))
sm["iqCarState"].slcSetSpeedRequestId = 0
sm["iqCarState"].slcSetSpeedGestureId = 0
sm["iqCarState"].slcSetSpeedRequestKph = 0.0
def step(speed, increase=False, new_gesture=False):
sm["carState"].vCruiseCluster = speed * unit * CV.MS_TO_KPH
if new_gesture:
sm["iqCarState"].slcSetSpeedGestureId += 1
if increase:
sm["iqCarState"].slcSetSpeedRequestId += 1
sm["iqCarState"].slcSetSpeedRequestKph = sm["carState"].vCruiseCluster
target = slc.update(sm["selfdriveState"].enabled, None, True, speed * unit, 50 * unit, sm)
return min(speed * unit, target) / unit
step(50)
return SimpleNamespace(slc=slc, params=params, sm=sm, unit=unit, step=step)
@pytest.mark.parametrize("confirm_higher", [False, True])
def test_set_speed_override_tracks_driver_adjustments(set_speed_slc, confirm_higher):
system = set_speed_slc
system.params["speed_limit_confirmation_higher"] = confirm_higher
assert system.step(50, new_gesture=True) == pytest.approx(50)
assert system.step(55, increase=True) == pytest.approx(55)
assert system.step(60, increase=True) == pytest.approx(60)
assert system.step(60) == pytest.approx(60)
assert system.step(55) == pytest.approx(55)
assert system.step(50) == pytest.approx(50)
assert not system.slc.slc.override_slc
assert system.step(45) == pytest.approx(45)
assert system.step(60) == pytest.approx(50)
assert system.step(61, increase=True, new_gesture=True) == pytest.approx(61)
def test_set_speed_override_ignores_automatic_speed_changes(set_speed_slc):
system = set_speed_slc
assert system.step(80) == pytest.approx(50)
system.slc.slc._resolver.map_speed_limit = 60 * system.unit
assert system.step(60) == pytest.approx(60)
assert system.step(80) == pytest.approx(60)
assert system.slc.slc.overridden_speed == 0
@pytest.mark.parametrize("limit", [40, 55])
def test_set_speed_override_resets_on_accepted_limit(set_speed_slc, limit):
system = set_speed_slc
assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
system.slc.slc._resolver.map_speed_limit = limit * system.unit
assert system.step(65, increase=True) == pytest.approx(limit)
assert system.step(70, increase=True) == pytest.approx(limit)
assert system.step(71, increase=True, new_gesture=True) == pytest.approx(71)
@pytest.mark.parametrize("limit,button", [(40, "decelCruise"), (55, "accelCruise")])
def test_set_speed_override_does_not_reuse_confirmation_gesture(set_speed_slc, limit, button):
system = set_speed_slc
system.params["speed_limit_confirmation_higher"] = True
system.params["speed_limit_confirmation_lower"] = True
system.slc.slc._resolver.map_speed_limit = limit * system.unit
system.step(50, new_gesture=True)
assert system.slc.assist_state == custom.IQPlan.SpeedLimit.AssistState.preActive
assert system.step(60, increase=True) == pytest.approx(50)
system.sm["carState"].buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
assert system.step(61, increase=True) == pytest.approx(limit)
system.sm["carState"].buttonEvents = []
assert system.step(65, increase=True) == pytest.approx(limit)
assert system.step(66, increase=True, new_gesture=True) == pytest.approx(66)
@pytest.mark.parametrize("reset", ["disengage", "information", "off", "missing_limit"])
def test_set_speed_override_cannot_survive_reset(set_speed_slc, reset):
system = set_speed_slc
assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
if reset == "disengage":
system.sm["selfdriveState"].enabled = False
elif reset in ("information", "off"):
system.params["speed_limit_controller"] = False
system.params["show_speed_limits"] = reset == "information"
else:
system.slc.slc._resolver.map_speed_limit = 0
system.step(60)
assert system.slc.slc.overridden_speed == 0
system.sm["selfdriveState"].enabled = True
system.params["speed_limit_controller"] = True
system.slc.slc._resolver.map_speed_limit = 50 * system.unit
assert system.step(60) == pytest.approx(50)
assert system.step(65, increase=True) == pytest.approx(50)
assert system.step(66, increase=True, new_gesture=True) == pytest.approx(66)
def test_set_speed_override_respects_offset(set_speed_slc):
system = set_speed_slc
system.slc.slc.params.put("speed_limit_offset1", 10)
system.slc.slc.params.put("speed_limit_offset2", 10)
system.slc.slc.params.put("speed_limit_offset3", 10)
system.slc.slc._offset_cache.clear()
system.step(50, new_gesture=True)
assert system.step(52, increase=True) == pytest.approx(52)
assert not system.slc.slc.override_slc
assert system.step(56, increase=True) == pytest.approx(56)
assert system.step(55) == pytest.approx(55)
assert not system.slc.slc.override_slc
def test_manual_override_still_requires_accelerator(set_speed_slc):
system = set_speed_slc
system.params["speed_limit_controller_override_set_speed"] = False
system.params["speed_limit_controller_override_manual"] = True
assert system.step(60, increase=True, new_gesture=True) == pytest.approx(50)
system.sm["carState"].gasPressed = True
system.sm["carState"].vEgoCluster = 55 * system.unit
system.slc.slc.update_override(60 * system.unit, 0, 55 * system.unit, 0, system.sm, system.params, system.params["is_metric"])
assert system.slc.slc.overridden_speed == pytest.approx(55 * system.unit)
system.sm["carState"].gasPressed = False
system.slc.slc.update_override(60 * system.unit, 0, 55 * system.unit, 0, system.sm, system.params, system.params["is_metric"])
assert system.slc.slc.overridden_speed == pytest.approx(55 * system.unit)
@pytest.mark.parametrize("pcm_cruise", [False, True])
def test_driver_increase_reaches_slc_without_transient_button_events(set_speed_slc, pcm_cruise):
system = set_speed_slc
helper = VCruiseHelper(car.CarParams(pcmCruise=pcm_cruise), custom.IQCarParams(pcmCruiseSpeed=True))
helper.set_speed_to_limit = False
helper.v_cruise_kph = helper.v_cruise_cluster_kph = 50 * system.unit * CV.MS_TO_KPH
state = car.CarState(cruiseState={"available": True, "speed": 50 * system.unit, "speedCluster": 50 * system.unit})
helper.update_v_cruise(state, True, system.params["is_metric"])
state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)]
helper.update_v_cruise(state, True, system.params["is_metric"])
state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=False)]
if pcm_cruise:
state.cruiseState.speed = state.cruiseState.speedCluster = 51 * system.unit
helper.update_v_cruise(state, True, system.params["is_metric"])
state.buttonEvents = []
for _ in range(5):
helper.update_v_cruise(state, True, system.params["is_metric"])
system.sm["iqCarState"] = custom.IQCarState.new_message(
slcSetSpeedRequestId=helper.slc_set_speed_request_id,
slcSetSpeedGestureId=helper.slc_set_speed_gesture_id,
slcSetSpeedRequestKph=helper.slc_set_speed_request_kph,
)
requested = helper.v_cruise_kph * CV.KPH_TO_MS / system.unit
assert requested > 50
assert system.step(requested) == pytest.approx(requested)
def test_set_speed_override_keeps_navigation_constraint(set_speed_slc):
system = set_speed_slc
assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
planner = LongitudinalPlannerIQ.__new__(LongitudinalPlannerIQ)
planner.slimit = system.slc
planner.iq_dynamic = SimpleNamespace(
set_slc_experimental_mode=lambda _mode: None, update=lambda _sm: None, force_stop_requested=lambda: False)
planner.force_stop_timer = 0.0
planner.override_force_stop_timer = 0.0
planner.override_force_stop = False
system.sm["iqNavState"] = SimpleNamespace(longitudinalEngaged=False, valid=False)
assert planner.update_targets(system.sm, 50 * system.unit, 60 * system.unit) == pytest.approx(60 * system.unit)
system.sm["iqNavState"] = SimpleNamespace(longitudinalEngaged=True, valid=True, speedTarget=40 * system.unit)
assert planner.update_targets(system.sm, 50 * system.unit, 60 * system.unit) == pytest.approx(40 * system.unit)
def test_set_speed_override_cannot_bypass_construction_zone(set_speed_slc):
system = set_speed_slc
assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
system.params["construction_zone_assist"] = True
system.params["construction_zone_speed"] = 40
system.sm["iqConstructionZone"] = SimpleNamespace(active=True)
system.sm.alive["iqConstructionZone"] = True
assert system.step(60) == pytest.approx(40)
assert system.step(65, increase=True, new_gesture=True) == pytest.approx(40)
assert not system.slc.slc.override_slc
class _FakeSM(dict):
def __init__(self, services, alive=None):
super().__init__(services)