IQ.Pilot Release Commit @ 27f668a

iqpilot/selfdrive/controls/controlsd.py

@@ -235,6 +235,7 @@ class Controls(IQControlsLayer):
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
+    actuators.speed = float(max(long_plan.speeds, default=0.0))
 
     if not CC.latActive:
       self.LaC.reset()

iqpilot/selfdrive/iqmodeld/modeld_selector.py

@@ -63,6 +63,11 @@ def resolve_model_name(params, keys) -> str:
     from iqpilot.selfdrive.iqmodeld.egpu_model import DEFAULT_EGPU_MODEL, resolve_egpu_model
     resolved = resolve_egpu_model(params, allow_refresh=False)
     return resolved["key"] if resolved else DEFAULT_EGPU_MODEL
+  if params.get_bool("IQEmacSmallModel"):
+    from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+    bundle = get_active_bundle(params)
+    if bundle is not None and (bundle.internalName or bundle.displayName):
+      return bundle.internalName or bundle.displayName
   name = params.get("IQEmacModel") or b"lebrowski"
   return name.decode() if isinstance(name, bytes) else name
 
@@ -168,7 +173,7 @@ def _patch_and_send(pm: PubMaster, payload: dict, frame_drop_perc: float, select
   if mismatch is None:
     mismatch = source_lag > 0
 
-  big = payload.get("source") in BIG_SOURCES
+  big = bool(payload.get("big", payload.get("source") in BIG_SOURCES))
   model_msg = log_from_bytes(msgs["modelV2"]).as_builder()
   if mismatch:
     model_msg.modelV2.frameId = target

iqpilot/selfdrive/iqmodeld/tests/test_egpu_worker.py

@@ -272,6 +272,23 @@ class TestChannelContract:
     assert sent["modelV2"].modelV2.frameDropPerc == 0.0
     assert sent["cameraOdometry"].valid
 
+  def test_selector_big_flag_follows_payload_then_source(self):
+    from iqpilot.selfdrive.iqmodeld.modeld_selector import _patch_and_send
+
+    sent = {}
+
+    class PM:
+      def send(self, service, msg):
+        sent[service] = msg
+
+    payload = make_big_channel_payload(42, True, 0.03, 25.0, self._real_msgs())
+    _patch_and_send(PM(), payload, frame_drop_perc=0.0, selector_dropped=0, target=42, source_lag=0)
+    assert sent["modelV2"].modelV2.big and sent["drivingModelData"].drivingModelData.big
+
+    small_on_mac = {**make_big_channel_payload(43, True, 0.03, 25.0, self._real_msgs()), "source": "mac_big", "big": False}
+    _patch_and_send(PM(), small_on_mac, frame_drop_perc=0.0, selector_dropped=0, target=43, source_lag=0)
+    assert not sent["modelV2"].modelV2.big and not sent["drivingModelData"].drivingModelData.big
+
   def test_selector_lag_patches_frame_id(self):
     from iqpilot.selfdrive.iqmodeld.modeld_selector import _patch_and_send
 

iqpilot/selfdrive/longitudinal_settings.py

@@ -9,6 +9,7 @@ LONGITUDINAL_MODE_STOCK = 0
 LONGITUDINAL_MODE_CHILL = 1
 LONGITUDINAL_MODE_DYNAMIC = 2
 LONGITUDINAL_MODE_PILOT = 3
+IQ_LONGITUDINAL_MODES = (LONGITUDINAL_MODE_CHILL, LONGITUDINAL_MODE_DYNAMIC, LONGITUDINAL_MODE_PILOT)
 
 PERSONALITY_AGGRESSIVE = log.LongitudinalPersonality.schema.enumerants["aggressive"]
 PERSONALITY_STANDARD = log.LongitudinalPersonality.schema.enumerants["standard"]
@@ -60,6 +61,20 @@ def apply_longitudinal_mode(params, mode: int) -> None:
     raise ValueError(f"invalid longitudinal mode: {mode}")
 
 
+def longitudinal_mode_needs_cycle(previous: int, mode: int) -> bool:
+  return (previous == LONGITUDINAL_MODE_STOCK) != (mode == LONGITUDINAL_MODE_STOCK)
+
+
+def next_longitudinal_mode(current: int, onroad: bool, iq_modes_available: bool) -> int:
+  if onroad:
+    order = list(IQ_LONGITUDINAL_MODES)
+  else:
+    order = [LONGITUDINAL_MODE_STOCK] + (list(IQ_LONGITUDINAL_MODES) if iq_modes_available else [])
+  if current not in order:
+    return current if onroad else order[0]
+  return order[(order.index(current) + 1) % len(order)]
+
+
 def get_follow_distance_state(params) -> tuple[int | None, bool]:
   mode = get_longitudinal_mode(params)
   if mode == LONGITUDINAL_MODE_STOCK:

iqpilot/selfdrive/selfdrived/tests/test_longitudinal_settings.py

@@ -7,6 +7,7 @@ import pytest
 from iqpilot.selfdrive.longitudinal_settings import (
   LONGITUDINAL_MODE_CHILL,
   LONGITUDINAL_MODE_DYNAMIC,
+  LONGITUDINAL_MODE_PILOT,
   LONGITUDINAL_MODE_STOCK,
   PERSONALITY_AGGRESSIVE,
   PERSONALITY_RELAXED,
@@ -16,6 +17,8 @@ from iqpilot.selfdrive.longitudinal_settings import (
   get_follow_distance_state,
   get_longitudinal_mode,
   get_runtime_personality,
+  longitudinal_mode_needs_cycle,
+  next_longitudinal_mode,
   set_valid_personality,
 )
 
@@ -108,3 +111,31 @@ def test_dynamic_and_pilot_enable_valid_personality_selection():
   params.values["IQDynamicMode"] = False
 
   assert get_follow_distance_state(params) == (PERSONALITY_STANDARD, True)
+
+
+def test_onroad_cycles_only_between_iq_modes():
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_CHILL, True, True) == LONGITUDINAL_MODE_DYNAMIC
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_DYNAMIC, True, True) == LONGITUDINAL_MODE_PILOT
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_PILOT, True, True) == LONGITUDINAL_MODE_CHILL
+
+
+def test_onroad_stock_acc_is_locked():
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_STOCK, True, True) == LONGITUDINAL_MODE_STOCK
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_STOCK, True, False) == LONGITUDINAL_MODE_STOCK
+
+
+def test_offroad_cycles_through_stock_acc():
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_PILOT, False, True) == LONGITUDINAL_MODE_STOCK
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_STOCK, False, True) == LONGITUDINAL_MODE_CHILL
+
+
+def test_offroad_without_iq_modes_only_offers_stock_acc():
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_STOCK, False, False) == LONGITUDINAL_MODE_STOCK
+  assert next_longitudinal_mode(LONGITUDINAL_MODE_PILOT, False, False) == LONGITUDINAL_MODE_STOCK
+
+
+def test_cycle_only_when_crossing_stock_boundary():
+  assert longitudinal_mode_needs_cycle(LONGITUDINAL_MODE_STOCK, LONGITUDINAL_MODE_CHILL)
+  assert longitudinal_mode_needs_cycle(LONGITUDINAL_MODE_PILOT, LONGITUDINAL_MODE_STOCK)
+  assert not longitudinal_mode_needs_cycle(LONGITUDINAL_MODE_CHILL, LONGITUDINAL_MODE_PILOT)
+  assert not longitudinal_mode_needs_cycle(LONGITUDINAL_MODE_DYNAMIC, LONGITUDINAL_MODE_CHILL)

iqpilot/selfdrive/ui/mici/layouts/settings/iq_widgets.py

@@ -6,13 +6,17 @@ from iqpilot.common.params import Params, UnknownKeyName
 from iqpilot.selfdrive.longitudinal_settings import (
   LONGITUDINAL_MODE_DYNAMIC,
   LONGITUDINAL_MODE_PILOT,
+  LONGITUDINAL_MODE_STOCK,
   PERSONALITY_VALUES,
   apply_longitudinal_mode,
   get_follow_distance_state,
   get_longitudinal_mode,
+  longitudinal_mode_needs_cycle,
+  next_longitudinal_mode,
   set_valid_personality,
 )
 from iqpilot.selfdrive.ui.mici.widgets.stock_button import BigButton, BigMultiToggle, BigToggle, BigParamControl
+from iqpilot.selfdrive.ui.ui_state import ui_state
 from iqpilot.system.ui.lib.multilang import tr
 
 
@@ -138,24 +142,48 @@ class IQModeSelector(BigMultiToggle):
     super().__init__(tr("IQ Mode"), self._display_options)
     self._params = Params()
     self._mode_callback = mode_callback
+    self._mode = LONGITUDINAL_MODE_STOCK
+    self._iq_modes_available = False
     self.refresh()
+    self.set_enabled(lambda: self._next() != self._mode)
 
   def _index(self) -> int:
     return get_longitudinal_mode(self._params)
 
   def is_dynamic(self) -> bool:
-    return self._index() == 2
+    return self._mode == LONGITUDINAL_MODE_DYNAMIC
+
+  def _toyota_factory_long_forced(self) -> bool:
+    cp = ui_state.CP
+    return bool(cp is not None and cp.brand == "toyota" and self._params.get_bool("IQToyotaFactoryLong"))
+
+  def _read_iq_modes_available(self) -> bool:
+    cp = ui_state.CP
+    alpha_available = bool(cp is not None and cp.alphaLongitudinalAvailable)
+    return alpha_available or self._params.get_bool("AlphaLongitudinalEnabled") or self._toyota_factory_long_forced()
+
+  def _next(self) -> int:
+    return next_longitudinal_mode(self._mode, ui_state.is_onroad(), self._iq_modes_available)
 
   def refresh(self):
-    self.set_value(self._display_options[self._index()])
+    self._mode = self._index()
+    self._iq_modes_available = self._read_iq_modes_available()
+    self.set_value(self._display_options[self._mode])
 
   def _apply(self, idx: int):
+    previous = self._mode
+    toyota_forced = self._toyota_factory_long_forced()
     apply_longitudinal_mode(self._params, idx)
-    self._params.put_bool("OnroadCycleRequested", True)
+    if idx != LONGITUDINAL_MODE_STOCK and toyota_forced:
+      self._params.put_bool("IQToyotaFactoryLong", False)
+    if longitudinal_mode_needs_cycle(previous, idx) or (idx != LONGITUDINAL_MODE_STOCK and toyota_forced):
+      self._params.put_bool("OnroadCycleRequested", True)
 
   def _handle_mouse_release(self, mouse_pos):
-    nxt = (self._index() + 1) % len(self.OPTIONS)
+    nxt = self._next()
+    if nxt == self._mode:
+      return
     self._apply(nxt)
-    self.set_value(self._display_options[nxt])
+    self.refresh()
     if self._mode_callback:
       self._mode_callback()

iqpilot/selfdrive/ui/mici/layouts/settings/models.py

@@ -128,6 +128,8 @@ class ModelsLayoutMici(NavScroller):
     self._big = BigButton(tr("big model"))
     self._big.set_click_callback(self._show_big_models)
 
+    self._small_on_mac = BigParamControl(tr("active model on eMac"), "IQEmacSmallModel", toggle_callback=self._small_on_mac_toggled)
+
     self._cancel = BigButton(tr("stop download"))
     self._cancel.set_click_callback(self._cancel_model_request)
     self._cancel.set_visible(self._is_downloading)
@@ -158,7 +160,8 @@ class ModelsLayoutMici(NavScroller):
     self._lane_speed = MappedParamToggle(tr("lane turn speed"), "IQLaneTurnValue", [tr("slow"), tr("normal"), tr("fast")], _LANE_TURN_VALUES)
     self._lane_speed.set_visible(lambda: self._lane_turn._checked)
 
-    self._main_items = [self._current, self._big, self._cancel, self._supercombo, self._vision, self._policy, self._redownload, self._refresh, self._clear,
+    self._main_items = [self._current, self._big, self._small_on_mac, self._cancel, self._supercombo, self._vision, self._policy,
+                        self._redownload, self._refresh, self._clear,
                         self._steer_delay, self._sw_delay, self._lane_turn, self._lane_speed]
     self._scroller.add_widgets(self._main_items)
 
@@ -394,9 +397,28 @@ class ModelsLayoutMici(NavScroller):
     except (TypeError, ValueError):
       return None
 
+  def _small_on_mac_toggled(self, checked: bool) -> None:
+    p = ui_state.params
+    if checked:
+      p.put_bool("IQEmacEnabled", True)
+    else:
+      p.put_bool("IQEmacEnabled", bool(p.get("IQEmacModel")))
+
+  def _small_on_mac_value(self) -> str:
+    try:
+      active = self.model_manager.activeBundle
+      name = _display_model_name(active) if active and active.ref else ""
+    except Exception:
+      name = ""
+    return f"{name} ({tr('eMac')})" if name else tr("active model")
+
   def _big_model_value(self) -> str:
     p = ui_state.params
     dock = bool(getattr(ui_state.sm["deviceState"], "egpuDockPresent", False))
+    if p.get_bool("IQEmacEnabled") and p.get_bool("IQEmacSmallModel"):
+      progress = self._big_setup_progress()
+      value = self._small_on_mac_value()
+      return f"{value} {int(progress * 100)}%" if progress is not None and progress < 1.0 else value
     if not p.get_bool("IQEmacEnabled") and not dock:
       return tr("Off")
     key = p.get("IQEmacModel")
@@ -577,5 +599,5 @@ class ModelsLayoutMici(NavScroller):
 
   def show_event(self):
     super().show_event()
-    for w in (self._steer_delay, self._sw_delay, self._lane_turn, self._lane_speed):
+    for w in (self._steer_delay, self._sw_delay, self._lane_turn, self._lane_speed, self._small_on_mac):
       w.refresh()