IQ.Pilot Release Commit @ 482798a

iqpilot/common/params_keys.h

@@ -398,6 +398,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"OfflineRoutingOnly", {PERSISTENT, BOOL, "0"}},
     {"OfflineRoutingHost", {PERSISTENT, STRING, "http://127.0.0.1:8002"}},
     {"EnableCurvatureController", {PERSISTENT, BOOL, "0"}},
+    {"EnableSmoothSteer", {PERSISTENT, BOOL, "0"}},
     {"EnableSpeedLimitControl", {PERSISTENT, BOOL, "0"}},
     {"MapCurveSpeedController", {PERSISTENT, BOOL, "0"}},
     {"VisionCurveSpeedController", {PERSISTENT, BOOL, "0"}},

iqpilot/common/pt2.py

@@ -0,0 +1,40 @@
+import math
+
+
+class PT2Filter:
+  def __init__(self, w0: float, zeta: float, dt: float):
+    self.w0 = w0
+    self.zeta = zeta
+    self.dt = dt
+    self.a1, self.a2, self.b0, self.b1, self.b2 = self._design(w0, zeta, dt)
+    self.y1 = 0.0
+    self.y2 = 0.0
+    self.u1 = 0.0
+    self.u2 = 0.0
+
+  @staticmethod
+  def _design(w0: float, zeta: float, dt: float):
+    # bilinear transform of H(s) = w0^2 / (s^2 + 2*zeta*w0*s + w0^2)
+    alpha = 2.0 / dt
+    a2_den = alpha**2 + (2.0 * zeta * w0 * alpha) + w0**2
+    a1_den = (-2.0 * alpha**2) + (2.0 * w0**2)
+    a0_den = alpha**2 - (2.0 * zeta * w0 * alpha) + w0**2
+    return (a1_den / a2_den, a0_den / a2_den,
+            w0**2 / a2_den, 2.0 * w0**2 / a2_den, w0**2 / a2_den)
+
+  def reset(self, value: float = 0.0) -> None:
+    self.y1 = value
+    self.y2 = value
+    self.u1 = value
+    self.u2 = value
+
+  def update(self, u: float) -> float:
+    y = (-self.a1 * self.y1) - (self.a2 * self.y2) + (self.b0 * u) + (self.b1 * self.u1) + (self.b2 * self.u2)
+    self.y2 = self.y1
+    self.y1 = y
+    self.u2 = self.u1
+    self.u1 = u
+    return y
+
+  def steady_state_steps(self) -> int:
+    return math.ceil((4.0 / (self.zeta * self.w0)) / self.dt)