diff --git a/common/pid.py b/common/pid.py index 721a7c9d65..99142280ca 100644 --- a/common/pid.py +++ b/common/pid.py @@ -14,8 +14,7 @@ class PIDController: if isinstance(self._k_d, Number): self._k_d = [[0], [self._k_d]] - self.pos_limit = pos_limit - self.neg_limit = neg_limit + self.set_limits(pos_limit, neg_limit) self.i_rate = 1.0 / rate self.speed = 0.0 @@ -41,6 +40,10 @@ class PIDController: self.f = 0.0 self.control = 0 + def set_limits(self, pos_limit, neg_limit): + self.pos_limit = pos_limit + self.neg_limit = neg_limit + def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): self.speed = speed self.p = float(error) * self.k_p diff --git a/opendbc_repo b/opendbc_repo index 18f8c0e757..74bfaa2c75 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 18f8c0e757906c779a16b70817b2c0fb5020f406 +Subproject commit 74bfaa2c750f2eca584dd6b58c1b862ba59d29d8 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c368a5da43..dffe85c473 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -3,7 +3,6 @@ import numpy as np from cereal import log from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction -from opendbc.car.interfaces import LatControlInputs from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController @@ -27,15 +26,22 @@ class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.torque_params = CP.lateralTuning.torque.as_builder() - self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, - k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.torque_from_lateral_accel = CI.torque_from_lateral_accel() + self.lateral_accel_from_torque = CI.lateral_accel_from_torque() + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, + k_f=self.torque_params.kf) + self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction + self.update_limits() + + def update_limits(self): + self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), + self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralTorqueState.new_message() @@ -57,27 +63,25 @@ class LatControlTorque(LatControl): setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation - torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - gravity_adjusted=False) - torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - gravity_adjusted=False) - pid_log.error = float(torque_from_setpoint - torque_from_measurement) - ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, - gravity_adjusted=True) + + # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly + pid_log.error = float(setpoint - measurement) + ff = gravity_adjusted_lateral_accel ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 - output_torque = self.pid.update(pid_log.error, + output_lataccel = self.pid.update(pid_log.error, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator) + output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.d = float(self.pid.d) pid_log.f = float(self.pid.f) - pid_log.output = float(-output_torque) + pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(actual_lateral_accel) pid_log.desiredLateralAccel = float(desired_lateral_accel) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) diff --git a/selfdrive/controls/lib/tests/__init__.py b/selfdrive/controls/lib/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py similarity index 88% rename from selfdrive/controls/lib/tests/test_latcontrol.py rename to selfdrive/controls/tests/test_latcontrol.py index f32e20b1e5..0ce06dc996 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -5,6 +5,7 @@ from opendbc.car.car_helpers import interfaces from opendbc.car.honda.values import CAR as HONDA 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.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque @@ -13,7 +14,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle class TestLatControl: - @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), (NISSAN.NISSAN_LEAF, LatControlAngle)]) + @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), + (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)]) def test_saturation(self, car_name, controller): CarInterface = interfaces[car_name] CP = CarInterface.get_non_essential_params(car_name) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index dd1f0b6a06..a4297096c0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4536719353fc9eeb4b9c57692cd72766a2f86397 \ No newline at end of file +6d3219bca9f66a229b38a5382d301a92b0147edb \ No newline at end of file