Lateral controllers: pass dt (delta time) explictly (#36281)

selfdrive/car/tests/test_car_interfaces.py

@@ -54,8 +54,8 @@ class TestCarInterfaces:
     #  hypothesis also slows down significantly with just one more message draw
     LongControl(car_params)
     if car_params.steerControlType == CarParams.SteerControlType.angle:
-      LatControlAngle(car_params, car_interface)
+      LatControlAngle(car_params, car_interface, DT_CTRL)
     elif car_params.lateralTuning.which() == 'pid':
-      LatControlPID(car_params, car_interface)
+      LatControlPID(car_params, car_interface, DT_CTRL)
     elif car_params.lateralTuning.which() == 'torque':
-      LatControlTorque(car_params, car_interface)
+      LatControlTorque(car_params, car_interface, DT_CTRL)

selfdrive/controls/controlsd.py

@@ -6,7 +6,7 @@ from cereal import car, log
 import cereal.messaging as messaging
 from openpilot.common.constants import CV
 from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
+from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog
 
 from opendbc.car.car_helpers import interfaces
@@ -51,11 +51,11 @@ class Controls:
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      self.LaC = LatControlAngle(self.CP, self.CI)
+      self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL)
     elif self.CP.lateralTuning.which() == 'pid':
-      self.LaC = LatControlPID(self.CP, self.CI)
+      self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
     elif self.CP.lateralTuning.which() == 'torque':
-      self.LaC = LatControlTorque(self.CP, self.CI)
+      self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)
 
   def update(self):
     self.sm.update(15)

selfdrive/controls/lib/latcontrol.py

@@ -1,12 +1,11 @@
 import numpy as np
 from abc import abstractmethod, ABC
 
-from openpilot.common.realtime import DT_CTRL
-
 
 class LatControl(ABC):
-  def __init__(self, CP, CI):
+  def __init__(self, CP, CI, dt):
-    self.sat_count_rate = 1.0 * DT_CTRL
+    self.dt = dt
+    self.sat_count_rate = 1.0 * self.dt
     self.sat_limit = CP.steerLimitTimer
     self.sat_count = 0.
     self.sat_check_min_speed = 10.

selfdrive/controls/lib/latcontrol_angle.py

@@ -8,8 +8,8 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
 
 class LatControlAngle(LatControl):
-  def __init__(self, CP, CI):
+  def __init__(self, CP, CI, dt):
-    super().__init__(CP, CI)
+    super().__init__(CP, CI, dt)
     self.sat_check_min_speed = 5.
     self.use_steer_limited_by_safety = CP.brand == "tesla"
 

selfdrive/controls/lib/latcontrol_pid.py

@@ -6,8 +6,8 @@ from openpilot.common.pid import PIDController
 
 
 class LatControlPID(LatControl):
-  def __init__(self, CP, CI):
+  def __init__(self, CP, CI, dt):
-    super().__init__(CP, CI)
+    super().__init__(CP, CI, dt)
     self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                              (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
                              k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)

selfdrive/controls/lib/latcontrol_torque.py

@@ -23,8 +23,8 @@ LOW_SPEED_Y = [15, 13, 10, 5]
 
 
 class LatControlTorque(LatControl):
-  def __init__(self, CP, CI):
+  def __init__(self, CP, CI, dt):
-    super().__init__(CP, CI)
+    super().__init__(CP, CI, dt)
     self.torque_params = CP.lateralTuning.torque.as_builder()
     self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
     self.lateral_accel_from_torque = CI.lateral_accel_from_torque()

selfdrive/controls/tests/test_latcontrol.py

@@ -7,6 +7,7 @@ from opendbc.car.toyota.values import CAR as TOYOTA
 from opendbc.car.nissan.values import CAR as NISSAN
 from opendbc.car.gm.values import CAR as GM
 from opendbc.car.vehicle_model import VehicleModel
+from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
@@ -22,7 +23,7 @@ class TestLatControl:
     CI = CarInterface(CP)
     VM = VehicleModel(CP)
 
-    controller = controller(CP.as_reader(), CI)
+    controller = controller(CP.as_reader(), CI, DT_CTRL)
 
     CS = car.CarState.new_message()
     CS.vEgo = 30