IQ.Pilot Release Commit @ 589e633

iqpilot/common/params_keys.h

@@ -174,6 +174,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"IQCarParamsCache", {CLEAR_ON_MANAGER_START, BYTES}},
     {"IQCarParamsPersistent", {PERSISTENT, BYTES}},
     {"IQCarParamsPersistentV2", {PERSISTENT, BYTES}},
+    {"IQLongLearnedFactors", {PERSISTENT, JSON}},
     {"CarPlatformBundle", {PERSISTENT, JSON}},
     {"Konn3ktVwOdometers", {PERSISTENT, JSON}},
     {"Konn3ktVehicleOdometers", {PERSISTENT, JSON}},

iqpilot/selfdrive/car/card.py

@@ -2,6 +2,8 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
+import json
+import math
 import os
 import time
 import threading
@@ -86,7 +88,7 @@ class Car:
 
   def __init__(self, CI=None, RI=None) -> None:
     self.can_sock = messaging.sub_sock('can', timeout=20)
-    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'testJoystick'] + ['iqCarControl', 'iqPlan'])
+    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'testJoystick', 'modelV2'] + ['iqCarControl', 'iqPlan'])
     self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'radarTracks', 'iqPerfTrace'] + ['iqCarParams', 'iqCarState'])
 
     self.can_rcv_cum_timeout_counter = 0
@@ -178,6 +180,9 @@ class Car:
         else:
           cloudlog.warning("Saved SecOC key is invalid")
 
+    if controller_available:
+      self._seed_learned_factors()
+
     # Write previous route's CarParams
     prev_cp = self.params.get("CarParamsPersistent")
     if prev_cp is not None:
@@ -259,9 +264,43 @@ class Car:
 
     return CS, CS_IQ, RD
 
+  def _learned_factor_attrs(self):
+    if self.CI.CC is None:
+      return ()
+    return tuple(attr for attr in ("gasfactor", "windfactor") if hasattr(self.CI.CC, attr))
+
+  def _stored_learned_factors(self) -> dict:
+    try:
+      stored = json.loads(self.params.get("IQLongLearnedFactors") or b"{}")
+    except ValueError:
+      stored = {}
+    return stored if isinstance(stored, dict) else {}
+
+  def _seed_learned_factors(self):
+    attrs = self._learned_factor_attrs()
+    if not attrs:
+      return
+    factors = self._stored_learned_factors().get(str(self.CP.carFingerprint), {})
+    for attr in attrs:
+      value = factors.get(attr)
+      if isinstance(value, int | float) and math.isfinite(value):
+        setattr(self.CI.CC, attr, float(value))
+
+  def _save_learned_factors(self):
+    attrs = self._learned_factor_attrs()
+    if not attrs:
+      return
+    stored = self._stored_learned_factors()
+    stored[str(self.CP.carFingerprint)] = {attr: float(getattr(self.CI.CC, attr)) for attr in attrs}
+    self.params.put_nonblocking("IQLongLearnedFactors", json.dumps(stored))
+
   def state_publish(self, CS: car.CarState, CS_IQ: custom.IQCarState, RD: structs.RadarDataT | None):
     """carState and carParams publish loop"""
 
+    # persist live-learned longitudinal factors so they survive across drives
+    if self.sm.frame > 0 and self.sm.frame % int(60. / DT_CTRL) == 0:
+      self._save_learned_factors()
+
     # carParams - logged every 50 seconds (> 1 per segment)
     if self.sm.frame % int(50. / DT_CTRL) == 0:
       cp_send = messaging.new_message('carParams')
@@ -323,8 +362,9 @@ class Car:
       cc_iq = convert_iq_car_control_compact(CC_IQ, include_leads=self._needs_iq_lead_data)
       convert_us = (time.monotonic_ns() - started) // 1000
 
+      model = self.sm['modelV2'] if self.sm.valid['modelV2'] else None
       started = time.monotonic_ns()
-      self.last_actuators_output, can_sends = self.CI.apply(CC, cc_iq, now_nanos)
+      self.last_actuators_output, can_sends = self.CI.apply(CC, cc_iq, now_nanos, model)
       apply_us = (time.monotonic_ns() - started) // 1000
 
       started = time.monotonic_ns()

iqpilot/selfdrive/car/tests/test_learned_factor_persistence.py

@@ -0,0 +1,82 @@
+import json
+from types import SimpleNamespace
+
+from iqpilot.selfdrive.car.card import Car
+
+
+class DummyParams:
+  def __init__(self):
+    self.values: dict[str, object] = {}
+
+  def get(self, key: str):
+    return self.values.get(key)
+
+  def put_nonblocking(self, key: str, value) -> None:
+    self.values[key] = value
+
+
+class HondaLikeController:
+  def __init__(self):
+    self.gasfactor = 1.0
+    self.windfactor = 1.0
+
+
+def make_car(controller):
+  car = object.__new__(Car)
+  car.params = DummyParams()
+  car.CI = SimpleNamespace(CC=controller)
+  car.CP = SimpleNamespace(carFingerprint="HONDA_CRV_6G")
+  return car
+
+
+class TestLearnedFactorPersistence:
+  def test_save_then_seed_round_trip(self):
+    car = make_car(HondaLikeController())
+    car.CI.CC.gasfactor = 1.37
+    car.CI.CC.windfactor = 0.84
+    car._save_learned_factors()
+
+    fresh = make_car(HondaLikeController())
+    fresh.params.values = car.params.values
+    fresh._seed_learned_factors()
+    assert fresh.CI.CC.gasfactor == 1.37
+    assert fresh.CI.CC.windfactor == 0.84
+
+  def test_factors_keyed_per_fingerprint(self):
+    car = make_car(HondaLikeController())
+    car.CI.CC.gasfactor = 2.0
+    car._save_learned_factors()
+
+    other = make_car(HondaLikeController())
+    other.params.values = car.params.values
+    other.CP = SimpleNamespace(carFingerprint="HONDA_CIVIC_BOSCH")
+    other._seed_learned_factors()
+    assert other.CI.CC.gasfactor == 1.0
+
+    other.CI.CC.gasfactor = 0.5
+    other._save_learned_factors()
+    stored = json.loads(other.params.values["IQLongLearnedFactors"])
+    assert stored["HONDA_CRV_6G"]["gasfactor"] == 2.0
+    assert stored["HONDA_CIVIC_BOSCH"]["gasfactor"] == 0.5
+
+  def test_seed_ignores_corrupt_or_nonfinite_values(self):
+    car = make_car(HondaLikeController())
+    car.params.values["IQLongLearnedFactors"] = "not json"
+    car._seed_learned_factors()
+    assert car.CI.CC.gasfactor == 1.0
+
+    car.params.values["IQLongLearnedFactors"] = json.dumps({"HONDA_CRV_6G": {"gasfactor": float("nan"), "windfactor": "x"}})
+    car._seed_learned_factors()
+    assert car.CI.CC.gasfactor == 1.0
+    assert car.CI.CC.windfactor == 1.0
+
+  def test_noop_for_controllers_without_factors(self):
+    car = make_car(SimpleNamespace())
+    car._seed_learned_factors()
+    car._save_learned_factors()
+    assert "IQLongLearnedFactors" not in car.params.values
+
+    car = make_car(None)
+    car._seed_learned_factors()
+    car._save_learned_factors()
+    assert "IQLongLearnedFactors" not in car.params.values

iqpilot/selfdrive/controls/lib/latcontrol_torque.py

@@ -11,6 +11,7 @@ import numpy as np
 from iqpilot.cereal import log, custom  # noqa: F401  (custom kept available for downstream imports)
 from iqdbc.car import structs
 from iqdbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from iqdbc.car.toyota.values import ToyotaFlags
 from iqdbc.lvbs.car.interfaces import LatControlInputs
 from iqdbc.lvbs.car.iq_lateral import get_friction as get_friction_in_torque_space
 from iqpilot.common.basedir import BASEDIR
@@ -556,6 +557,8 @@ class LatControlTorque(LatControl):
     self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
     self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
     self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+    self.setpoint_lead_enabled = CP.brand == "toyota" and bool(CP.flags & ToyotaFlags.TSS2)
+    self.setpoint_lead_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
     self.lateral_acceleration_slew_limiter = LateralAccelerationSlewLimiter(Params().get_bool("IQLateralAccelSlew"))
     self.curvature_lookahead_enabled = Params().get_bool("IQLateralCurvatureLookahead")
 
@@ -593,6 +596,10 @@ class LatControlTorque(LatControl):
     delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
     expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
     setpoint = expected_lateral_accel
+    if self.setpoint_lead_enabled:
+      # the delayed setpoint mutes P/I for lat_delay after a ramp starts; lead by the filtered ramp rate so torque-capped TSS2 EPS turns in on time
+      request_ramp_rate = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
+      setpoint += self.setpoint_lead_filter.update(request_ramp_rate) * lat_delay
     error = setpoint - measurement
 
     lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))