From 305a7a6bc92db02ac56a80383eb9f1802c22e062 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Thu, 7 Apr 2022 11:34:45 -0700 Subject: [PATCH] Make PI into a PID (#24151) * Make PI into a PID * Cast like before old-commit-hash: 634f7cebef415cda913517c157c0a1d025b6f2b4 --- selfdrive/controls/lib/latcontrol_pid.py | 13 +++++----- selfdrive/controls/lib/longcontrol.py | 22 ++++++++++++---- selfdrive/controls/lib/pid.py | 32 ++++++++++++------------ selfdrive/thermald/fan_controller.py | 8 +++--- 4 files changed, 43 insertions(+), 32 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index c711fa2648..67d17e088c 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,6 +1,6 @@ import math -from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from cereal import log @@ -8,7 +8,7 @@ from cereal import log class LatControlPID(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) - self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), + 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=1.0, neg_limit=-1.0) self.get_steer_feedforward = CI.get_steer_feedforward_function() @@ -24,9 +24,10 @@ class LatControlPID(LatControl): angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg + error = angle_steers_des - CS.steeringAngleDeg pid_log.steeringAngleDesiredDeg = angle_steers_des - pid_log.angleError = angle_steers_des - CS.steeringAngleDeg + pid_log.angleError = error if CS.vEgo < MIN_STEER_SPEED or not active: output_steer = 0.0 pid_log.active = False @@ -38,10 +39,8 @@ class LatControlPID(LatControl): # offset does not contribute to resistive torque steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) - deadzone = 0.0 - - output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed, - feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) + output_steer = self.pid.update(error, override=CS.steeringPressed, + feedforward=steer_feedforward, speed=CS.vEgo) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f91ddcf44e..0fda7789aa 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,7 +1,7 @@ from cereal import car from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.drive_helpers import CONTROL_N from selfdrive.modeld.constants import T_IDXS @@ -12,6 +12,16 @@ ACCEL_MIN_ISO = -3.5 # m/s^2 ACCEL_MAX_ISO = 2.0 # m/s^2 +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + + def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_target_future, brake_pressed, cruise_standstill): """Update longitudinal control state machine""" @@ -43,9 +53,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, class LongControl(): def __init__(self, CP): self.long_control_state = LongCtrlState.off # initialized to off - self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), - (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL) + self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), + (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), + k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL) self.v_pid = 0.0 self.last_output_accel = 0.0 @@ -101,7 +111,9 @@ class LongControl(): deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) freeze_integrator = prevent_overshoot - output_accel = self.pid.update(self.v_pid, CS.vEgo, speed=CS.vEgo, deadzone=deadzone, feedforward=a_target, freeze_integrator=freeze_integrator) + error = self.v_pid - CS.vEgo + error_deadzone = apply_deadzone(error, deadzone) + output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=a_target, freeze_integrator=freeze_integrator) if prevent_overshoot: output_accel = min(output_accel, 0.0) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index c91eb692cb..b5ef735680 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -3,24 +3,19 @@ from numbers import Number from common.numpy_fast import clip, interp -def apply_deadzone(error, deadzone): - if error > deadzone: - error -= deadzone - elif error < - deadzone: - error += deadzone - else: - error = 0. - return error - -class PIController(): - def __init__(self, k_p, k_i, k_f=0., pos_limit=None, neg_limit=None, rate=100): + +class PIDController(): + def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=None, neg_limit=None, rate=100): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain + self._k_d = k_d # feedforward gain self.k_f = k_f # feedforward gain if isinstance(self._k_p, Number): self._k_p = [[0], [self._k_p]] if isinstance(self._k_i, Number): self._k_i = [[0], [self._k_i]] + if isinstance(self._k_d, Number): + self._k_d = [[0], [self._k_d]] self.pos_limit = pos_limit self.neg_limit = neg_limit @@ -38,24 +33,29 @@ class PIController(): def k_i(self): return interp(self.speed, self._k_i[0], self._k_i[1]) + @property + def k_d(self): + return interp(self.speed, self._k_d[0], self._k_d[1]) + def reset(self): self.p = 0.0 self.i = 0.0 + self.d = 0.0 self.f = 0.0 self.control = 0 - def update(self, setpoint, measurement, speed=0.0, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): self.speed = speed - error = float(apply_deadzone(setpoint - measurement, deadzone)) - self.p = error * self.k_p + self.p = float(error) * self.k_p self.f = feedforward * self.k_f + self.d = error_rate * self.k_d if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: i = self.i + error * self.k_i * self.i_rate - control = self.p + self.f + i + control = self.p + i + self.d + self.f # Update when changing i will move the control away from the limits # or when i will move towards the sign of the error @@ -64,7 +64,7 @@ class PIController(): not freeze_integrator: self.i = i - control = self.p + self.f + self.i + control = self.p + self.i + self.d + self.f self.control = clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index 2fe52d0cb8..f111b4edf7 100644 --- a/selfdrive/thermald/fan_controller.py +++ b/selfdrive/thermald/fan_controller.py @@ -6,7 +6,7 @@ from abc import ABC, abstractmethod from common.realtime import DT_TRML from common.numpy_fast import interp from selfdrive.swaglog import cloudlog -from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid import PIDController class BaseFanController(ABC): @abstractmethod @@ -83,7 +83,7 @@ class TiciFanController(BaseFanController): cloudlog.info("Setting up TICI fan handler") self.last_ignition = False - self.controller = PIController(k_p=0, k_i=2e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) + self.controller = PIDController(k_p=0, k_i=2e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) def update(self, max_cpu_temp: float, ignition: bool) -> int: self.controller.neg_limit = -(80 if ignition else 30) @@ -92,9 +92,9 @@ class TiciFanController(BaseFanController): if ignition != self.last_ignition: self.controller.reset() + error = 75 - max_cpu_temp fan_pwr_out = -int(self.controller.update( - setpoint=75, - measurement=max_cpu_temp, + error=error, feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80]) ))