sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Make PI into a PID (#24151)

Browse Source 
* Make PI into a PID

* Cast like before
old-commit-hash: 634f7cebef
taco
HaraldSchafer 3 years ago committed by GitHub
parent b19c75d189
commit 305a7a6bc9
4 changed files with 43 additions and 32 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 13
      selfdrive/controls/lib/latcontrol_pid.py
  2. 22
      selfdrive/controls/lib/longcontrol.py
  3. 32
      selfdrive/controls/lib/pid.py
  4. 8
      selfdrive/thermald/fan_controller.py

13
selfdrive/controls/lib/latcontrol_pid.py
Unescape 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

22
selfdrive/controls/lib/longcontrol.py
Unescape 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)

32
selfdrive/controls/lib/pid.py
Unescape 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

8
selfdrive/thermald/fan_controller.py
Unescape 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])
))

Write Preview
Loading…
Cancel
Save