diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index c40443d7e7..24d2faa0db 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/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) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 029d16e59e..4442e6cbe4 100755 --- a/selfdrive/controls/controlsd.py +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 7cd04240d0..a97ed3e54e 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/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): - self.sat_count_rate = 1.0 * DT_CTRL + def __init__(self, CP, CI, dt): + 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. diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index ac35151487..cea9f74814 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/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): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand == "tesla" diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 00a083509f..d8eae451ed 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -6,8 +6,8 @@ from openpilot.common.pid import PIDController class LatControlPID(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + 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) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 5a2814e089..bcaf934586 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + 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() diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0ce06dc996..c564e9d3e5 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/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