IQ.Pilot Release Commit @ 6d177d4

iqdbc_repo/iqdbc/car/interfaces.py

@@ -332,6 +332,12 @@ class CarStateBase(ABC):
     x0=[[0.0], [0.0]]
     K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
     self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
+    self.v_ego_clu_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
+
+    self.softHoldActive = 0
+    self.is_metric = True
+    self.lkas_enabled = False
+    self.modelV2 = None
 
   @abstractmethod
   def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
@@ -348,6 +354,22 @@ class CarStateBase(ABC):
     v_ego_x = self.v_ego_kf.update(v_ego_raw)
     return float(v_ego_x[0]), float(v_ego_x[1])
 
+  def update_clu_speed_kf(self, v_ego_raw):
+    if abs(v_ego_raw - self.v_ego_clu_kf.x[0][0]) > 2.0:
+      self.v_ego_clu_kf.set_x([[v_ego_raw], [0.0]])
+
+    v_ego_x = self.v_ego_clu_kf.update(v_ego_raw)
+    return float(v_ego_x[0]), float(v_ego_x[1])
+
+  def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS):
+    factor = unit * self.CP.wheelSpeedFactor
+    wheel_speeds = structs.CarState.WheelSpeeds()
+    wheel_speeds.fl = fl * factor
+    wheel_speeds.fr = fr * factor
+    wheel_speeds.rl = rl * factor
+    wheel_speeds.rr = rr * factor
+    return wheel_speeds
+
   def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
     """Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
     iterations"""