mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-19 11:52:12 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make PI into a PID (#24151)

Browse Source 
* Make PI into a PID

* Cast like before
This commit is contained in:
HaraldSchafer
2022-04-07 11:34:45 -07:00
committed by GitHub
parent 10d4766f76
commit 634f7cebef
4 changed files with 42 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_pid.py
+6 -7
View File
@@ -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
selfdrive/controls/lib/longcontrol.py
+17 -5
View File
@@ -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)
selfdrive/controls/lib/pid.py
+15 -15
View File
@@ -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
selfdrive/thermald/fan_controller.py
+4 -4
View File
@@ -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])
))