IQ.Pilot Release Commit @ 67fd9c2

iqpilot/selfdrive/controls/lib/latcontrol_torque.py

@@ -186,6 +186,8 @@ class NNTorqueModel:
 
 PLAN_SAMPLE_START = 5
 LAG_EXTRA_S = 0.0
+JERK_AHEAD_TAU_S = 0.15  # low-pass on the model-derived jerk feed-forward (kills big-model accel.y noise)
+JERK_PARAM_REFRESH = 100  # cycles (~1s at 100Hz)
 
 BASE_P = 0.8
 BASE_I = 0.15
@@ -264,6 +266,15 @@ class PilotLateralBrain:
     self.friction_look_ahead_bp = [9.0, 30.0]
     self.lat_jerk_friction_factor = 0.4
     self.lat_accel_friction_factor = 0.7
+    # The model-derived jerk term is a frame-to-frame derivative of model_v2.acceleration.y; on
+    # spatial/big models that array is slightly noisy and the raw derivative drives in-lane steering
+    # oscillation (sunny/stock has no such term). Low-pass it, and expose a live gain so it can be
+    # tuned to 0 (== stock friction ff) without a software push.
+    self._jerk_lp = FirstOrderFilter(0.0, JERK_AHEAD_TAU_S, 0.01)
+    self._jerk_gain = 1.0
+    self._jerk_param_frame = 0
+    self._jerk_param_ok = True
+    self._params = Params()
 
     self.t_diffs = np.diff(ModelConstants.T_IDXS)
     self.desired_lat_jerk_time = cp.steerActuatorDelay + LAG_EXTRA_S
@@ -294,6 +305,17 @@ class PilotLateralBrain:
     self.jerk_ahead = 0.0
 
   def update_calculations(self, car_state, vehicle_model, desired_lat_accel):
+    self._jerk_param_frame += 1
+    if self._jerk_param_ok and self._jerk_param_frame % JERK_PARAM_REFRESH == 0:
+      try:
+        raw = self._params.get("IQLatJerkGain")
+        self._jerk_gain = float(raw) if raw not in (None, b"", "") else 1.0
+      except (ValueError, TypeError):
+        self._jerk_gain = 1.0
+      except Exception:
+        # param key absent (params not rebuilt) — never let a param read touch lateral control
+        self._jerk_param_ok = False
+        self._jerk_gain = 1.0
     self._reset_jerk_estimates(car_state, vehicle_model)
     if not self.model_valid:
       return
@@ -305,6 +327,7 @@ class PilotLateralBrain:
     forecast = _pointwise_jerk(accel_y, self.t_diffs)
     window = forecast[PLAN_SAMPLE_START:self._horizon_index(car_state.vEgo)]
     self.jerk_ahead = sign_locked_min(window, desired_jerk)
+    self.jerk_ahead = self._jerk_lp.update(self.jerk_ahead) * self._jerk_gain
 
     if self.jerk_ahead == 0.0:
       self.jerk_now = 0.0

iqpilot/selfdrive/controls/tests/test_lat_jerk_lowpass.py

@@ -0,0 +1,81 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+import numpy as np
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+
+CAR_NAME = TOYOTA.TOYOTA_COROLLA_TSS2
+
+
+def _brain():
+  CI_cls = interfaces[CAR_NAME]
+  CP = CI_cls.get_non_essential_params(CAR_NAME)
+  CP_IQ = CI_cls.get_non_essential_params_iq(CP, CAR_NAME)
+  CI = CI_cls(CP, CP_IQ)
+  iqpilot_interfaces.apply_iq_car_config(CI)
+  ctrl = LatControlTorque(CP.as_reader(), convert_to_capnp(CP_IQ).as_reader(), CI, DT_CTRL)
+  return ctrl.nnff_assist, VehicleModel(CP)
+
+
+def _model(rng):
+  # same-sign accel ramp (so sign_locked_min yields a real jerk) whose slope jitters frame to
+  # frame the way a spatial big model's path does — this is what drives jerk_ahead to swing.
+  n = max(CONTROL_N, 33)
+  slope = abs(0.5 + rng.normal(0, 0.25))
+  m = log.ModelDataV2.new_message()
+  m.acceleration.y = (slope * np.arange(n) * 0.1).tolist()
+  m.orientation.x = [0.0] * n
+  return m
+
+
+def _cs():
+  cs = car.CarState.new_message()
+  cs.vEgo = 25.0
+  cs.steeringRateDeg = 0.0
+  return cs
+
+
+def _run(lp_on):
+  brain, VM = _brain()
+  rng = np.random.default_rng(7)
+  cs = _cs()
+  out = []
+  for _ in range(400):
+    brain.update_model_v2(_model(rng))
+    if not lp_on:
+      brain._jerk_lp.update = lambda x: x  # bypass low-pass == pre-fix behavior
+    brain.update_calculations(cs, VM, 0.0)
+    out.append(brain.jerk_ahead)
+  return np.array(out)
+
+
+def test_lowpass_cuts_jerk_command_swing():
+  old = _run(lp_on=False)
+  new = _run(lp_on=True)
+  # the path must actually exercise the jerk feed-forward (guard against a vacuous test)
+  assert np.abs(np.diff(old)).mean() > 0.02, "input did not exercise jerk_ahead"
+  old_swing = np.abs(np.diff(old)).mean()
+  new_swing = np.abs(np.diff(new)).mean()
+  # low-pass must cut the frame-to-frame jerk swing (the wheel oscillation) by a large margin
+  assert new_swing < 0.3 * old_swing, (old_swing, new_swing)
+
+
+def test_gain_zero_matches_stock():
+  brain, VM = _brain()
+  brain._jerk_param_ok = False
+  brain._jerk_gain = 0.0
+  rng = np.random.default_rng(1)
+  cs = _cs()
+  for _ in range(60):
+    brain.update_model_v2(_model(rng))
+    brain.update_calculations(cs, VM, 0.0)
+  assert brain.jerk_ahead == 0.0  # no model-jerk term == sunny/stock feedforward