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
Unified 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 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 selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
from cereal import log from cereal import log
@ -8,7 +8,7 @@ from cereal import log
class LatControlPID(LatControl): class LatControlPID(LatControl):
def __init__(self, CP, CI): def __init__(self, CP, CI):
super().__init__(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), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0) k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0)
self.get_steer_feedforward = CI.get_steer_feedforward_function() 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_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 angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
error = angle_steers_des - CS.steeringAngleDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des 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: if CS.vEgo < MIN_STEER_SPEED or not active:
output_steer = 0.0 output_steer = 0.0
pid_log.active = False pid_log.active = False
@ -38,10 +39,8 @@ class LatControlPID(LatControl):
# offset does not contribute to resistive torque # offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
deadzone = 0.0 output_steer = self.pid.update(error, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo)
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True pid_log.active = True
pid_log.p = self.pid.p pid_log.p = self.pid.p
pid_log.i = self.pid.i pid_log.i = self.pid.i

22
selfdrive/controls/lib/longcontrol.py
Unescape View File

@ -1,7 +1,7 @@
from cereal import car from cereal import car
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from common.realtime import DT_CTRL 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.controls.lib.drive_helpers import CONTROL_N
from selfdrive.modeld.constants import T_IDXS 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 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, def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
v_target_future, brake_pressed, cruise_standstill): v_target_future, brake_pressed, cruise_standstill):
"""Update longitudinal control state machine""" """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(): class LongControl():
def __init__(self, CP): def __init__(self, CP):
self.long_control_state = LongCtrlState.off # initialized to off self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL) k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
self.v_pid = 0.0 self.v_pid = 0.0
self.last_output_accel = 0.0 self.last_output_accel = 0.0
@ -101,7 +111,9 @@ class LongControl():
deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot 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: if prevent_overshoot:
output_accel = min(output_accel, 0.0) 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 from common.numpy_fast import clip, interp
def apply_deadzone(error, deadzone):
if error > deadzone: class PIDController():
error -= deadzone def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=None, neg_limit=None, rate=100):
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):
self._k_p = k_p # proportional gain self._k_p = k_p # proportional gain
self._k_i = k_i # integral gain self._k_i = k_i # integral gain
self._k_d = k_d # feedforward gain
self.k_f = k_f # feedforward gain self.k_f = k_f # feedforward gain
if isinstance(self._k_p, Number): if isinstance(self._k_p, Number):
self._k_p = [[0], [self._k_p]] self._k_p = [[0], [self._k_p]]
if isinstance(self._k_i, Number): if isinstance(self._k_i, Number):
self._k_i = [[0], [self._k_i]] 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.pos_limit = pos_limit
self.neg_limit = neg_limit self.neg_limit = neg_limit
@ -38,24 +33,29 @@ class PIController():
def k_i(self): def k_i(self):
return interp(self.speed, self._k_i[0], self._k_i[1]) 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): def reset(self):
self.p = 0.0 self.p = 0.0
self.i = 0.0 self.i = 0.0
self.d = 0.0
self.f = 0.0 self.f = 0.0
self.control = 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 self.speed = speed
error = float(apply_deadzone(setpoint - measurement, deadzone)) self.p = float(error) * self.k_p
self.p = error * self.k_p
self.f = feedforward * self.k_f self.f = feedforward * self.k_f
self.d = error_rate * self.k_d
if override: if override:
self.i -= self.i_unwind_rate * float(np.sign(self.i)) self.i -= self.i_unwind_rate * float(np.sign(self.i))
else: else:
i = self.i + error * self.k_i * self.i_rate 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 # Update when changing i will move the control away from the limits
# or when i will move towards the sign of the error # or when i will move towards the sign of the error
@ -64,7 +64,7 @@ class PIController():
not freeze_integrator: not freeze_integrator:
self.i = i 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) self.control = clip(control, self.neg_limit, self.pos_limit)
return self.control 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.realtime import DT_TRML
from common.numpy_fast import interp from common.numpy_fast import interp
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.pid import PIDController
class BaseFanController(ABC): class BaseFanController(ABC):
@abstractmethod @abstractmethod
@ -83,7 +83,7 @@ class TiciFanController(BaseFanController):
cloudlog.info("Setting up TICI fan handler") cloudlog.info("Setting up TICI fan handler")
self.last_ignition = False 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: def update(self, max_cpu_temp: float, ignition: bool) -> int:
self.controller.neg_limit = -(80 if ignition else 30) self.controller.neg_limit = -(80 if ignition else 30)
@ -92,9 +92,9 @@ class TiciFanController(BaseFanController):
if ignition != self.last_ignition: if ignition != self.last_ignition:
self.controller.reset() self.controller.reset()
error = 75 - max_cpu_temp
fan_pwr_out = -int(self.controller.update( fan_pwr_out = -int(self.controller.update(
setpoint=75, error=error,
measurement=max_cpu_temp,
feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80]) feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
)) ))

Write Preview
Loading…
Cancel
Save