IQ.Pilot Release Commit @ 589e633

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