IQ.Pilot Release Commit @ d2ce8a8

iqpilot/selfdrive/controls/controlsd.py

@@ -24,6 +24,7 @@ from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from iqpilot.selfdrive.controls.lib.latcontrol_torque_pq import LatControlTorquePQ
 from iqpilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorqueV0, is_vw_mqb_torque
 from iqpilot.selfdrive.controls.lib.longcontrol import LongControl
+from iqpilot.selfdrive.controls.steering_fault_recovery import SteeringFaultRecovery
 from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
 from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
@@ -73,6 +74,7 @@ class Controls(IQControlsLayer):
     self.pm = messaging.PubMaster(['carControl', 'controlsState', 'iqPerfTrace'] + self.iq_pub_services)
 
     self.steer_limited_by_safety = False
+    self.steering_fault_recovery = SteeringFaultRecovery()
     self.curvature = 0.0
     self.desired_curvature = 0.0
     self.roll_compensation = 0.0
@@ -208,7 +210,8 @@ class Controls(IQControlsLayer):
     # Get which state to use for active lateral control
     _lat_active = self.iq_lateral_allowed(self.sm)
 
-    CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+    steering_fault_recovered = self.steering_fault_recovery.update(CS.steerFaultTemporary, CS.steerFaultPermanent)
+    CC.latActive = _lat_active and steering_fault_recovered and \
                    (not standstill or self.CP.steerAtStandstill)
     # long control may stay active through a gas override on platforms that opt in
     override_longitudinal = any(e.overrideLongitudinal for e in self.sm['onroadEvents'])

iqpilot/selfdrive/controls/lib/slc_vcruise.py

@@ -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', []))

iqpilot/selfdrive/controls/lib/speed_limit_controller.py

@@ -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

iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py

@@ -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)

iqpilot/selfdrive/controls/steering_fault_recovery.py

@@ -0,0 +1,18 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from iqpilot.common.realtime import DT_CTRL
+
+STEER_FAULT_RECOVERY_FRAMES = int(1.0 / DT_CTRL)
+
+
+class SteeringFaultRecovery:
+  def __init__(self) -> None:
+    self.clear_frames = STEER_FAULT_RECOVERY_FRAMES
+
+  def update(self, temporary: bool, permanent: bool) -> bool:
+    if temporary or permanent:
+      self.clear_frames = 0
+    else:
+      self.clear_frames = min(self.clear_frames + 1, STEER_FAULT_RECOVERY_FRAMES)
+    return self.clear_frames == STEER_FAULT_RECOVERY_FRAMES

iqpilot/selfdrive/controls/tests/test_steering_fault_recovery.py

@@ -0,0 +1,28 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from iqpilot.selfdrive.controls.steering_fault_recovery import STEER_FAULT_RECOVERY_FRAMES, SteeringFaultRecovery
+
+
+def test_steering_fault_recovery_starts_ready():
+  recovery = SteeringFaultRecovery()
+  assert recovery.update(False, False)
+
+
+def test_temporary_fault_requires_continuous_clear_interval():
+  recovery = SteeringFaultRecovery()
+  assert not recovery.update(True, False)
+  for _ in range(STEER_FAULT_RECOVERY_FRAMES - 1):
+    assert not recovery.update(False, False)
+  assert recovery.update(False, False)
+
+
+def test_repeated_fault_restarts_recovery_interval():
+  recovery = SteeringFaultRecovery()
+  assert not recovery.update(False, True)
+  for _ in range(STEER_FAULT_RECOVERY_FRAMES - 1):
+    assert not recovery.update(False, False)
+  assert not recovery.update(True, False)
+  for _ in range(STEER_FAULT_RECOVERY_FRAMES - 1):
+    assert not recovery.update(False, False)
+  assert recovery.update(False, False)