Skip to content
This repository has been archived by the owner on Jan 9, 2022. It is now read-only.

Commit

Permalink
Unify PIDController class (commaai#470)
Browse files Browse the repository at this point in the history 
* use one picontroller class

* rename

* bump cereal

* incorrect bump

* incorrect bump

* incorrect bump

* tune period

* remove from params
  • Loading branch information
@sshane
sshane authored Sep 19, 2021
1 parent b16090b commit 02562a8
Show file tree
Hide file tree
Showing 6 changed files with 24 additions and 198 deletions.
4 changes: 1 addition & 3 deletions common/op_params.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,8 +112,6 @@ def __init__(self):
# 'lane_speed_alerts': Param('silent', str, 'Can be: (\'off\', \'silent\', \'audible\')\n'
# 'Whether you want openpilot to alert you of faster-traveling adjacent lanes'),
'upload_on_hotspot': Param(False, bool, 'If False, openpilot will not upload driving data while connected to your phone\'s hotspot'),
# 'enable_long_derivative': Param(False, bool, 'If you have longitudinal overshooting, enable this! This enables derivative-based\n'
# 'integral wind-down to help reduce overshooting within the long PID loop'),
'disengage_on_gas': Param(False, bool, 'Whether you want openpilot to disengage on gas input or not'),
'update_behavior': Param('alert', str, 'Can be: (\'off\', \'alert\', \'auto\') without quotes\n'
'off will never update, alert shows an alert on-screen\n'
Expand All @@ -138,7 +136,7 @@ def __init__(self):
'rav4TSS2_use_indi': Param(False, bool, 'Enable this to use INDI for lat with your TSS2 RAV4', static=True),
'standstill_hack': Param(False, bool, 'Some cars support stop and go, you just need to enable this', static=True)}

self._to_delete = ['long_accel_delay', 'accel_lag_compensation'] # a list of unused params you want to delete from users' params file
self._to_delete = ['enable_long_derivative'] # a list of unused params you want to delete from users' params file
self._to_reset = [] # a list of params you want reset to their default values
self._run_init() # restores, reads, and updates params

Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/controlsd.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -675,7 +675,7 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.id)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
Expand Down
10 changes: 5 additions & 5 deletions selfdrive/controls/lib/latcontrol_pid.py
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,16 @@
import math

from selfdrive.controls.lib.pid import LatPIDController
from selfdrive.controls.lib.pid import PIDController
from selfdrive.controls.lib.drive_helpers import get_steer_max
from cereal import log


class LatControlPID():
def __init__(self, CP):
self.pid = LatPIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer)
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer, derivative_period=0.1)
self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned

def reset(self):
Expand Down
16 changes: 7 additions & 9 deletions selfdrive/controls/lib/longcontrol.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
from common.numpy_fast import clip, interp
from selfdrive.controls.lib.drive_helpers import CONTROL_N
from selfdrive.modeld.constants import T_IDXS
from selfdrive.controls.lib.pid import LongPIDController
from selfdrive.controls.lib.pid import PIDController
from selfdrive.controls.lib.dynamic_gas import DynamicGas
from common.op_params import opParams

Expand Down Expand Up @@ -62,14 +62,12 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
class LongControl():
def __init__(self, CP):
self.long_control_state = LongCtrlState.off # initialized to off
# kdBP = [0., 16., 35.]
# kdV = [0.08, 1.215, 2.51]

self.pid = LongPIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
([0], [0]),
rate=RATE,
sat_limit=0.8)
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV),
rate=RATE,
sat_limit=0.8,
derivative_period=0.5)
self.v_pid = 0.0
self.last_output_accel = 0.0

Expand Down
186 changes: 8 additions & 178 deletions selfdrive/controls/lib/pid.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,10 @@ def apply_deadzone(error, deadzone):
error = 0.
return error

class LatPIDController():
def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8):

class PIDController:
def __init__(self, k_p=0., k_i=0., k_d=0., k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, derivative_period=1.):
self.op_params = opParams()
self._k_p = k_p # proportional gain
self._k_i = k_i # integral gain
self._k_d = k_d # derivative gain
Expand All @@ -35,6 +37,7 @@ def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=1
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.sat_limit = sat_limit
self._d_period = round(derivative_period * rate) # period of time for derivative calculation (seconds converted to frames)

self.reset()

Expand Down Expand Up @@ -79,8 +82,8 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri
self.f = feedforward * self.k_f

d = 0
if len(self.errors) >= 5: # makes sure list is long enough
d = (error - self.errors[-5]) / 5 # get deriv in terms of 100hz (tune scale doesn't change)
if len(self.errors) >= self._d_period: # makes sure we have enough history for period
d = (error - self.errors[-self._d_period]) / self._d_period # get deriv in terms of 100hz (tune scale doesn't change)
d *= self.k_d

if override:
Expand All @@ -97,184 +100,11 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri
self.i = i

control = self.p + self.f + self.i + d

self.saturated = self._check_saturation(control, check_saturation, error)

self.errors.append(float(error))
while len(self.errors) > 5:
while len(self.errors) > self._d_period:
self.errors.pop(0)

self.control = clip(control, self.neg_limit, self.pos_limit)
return self.control


class LongPIDController:
def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8):
self.op_params = opParams()
self._k_p = k_p # proportional gain
self._k_i = k_i # integral gain
self._k_d = k_d # derivative 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.max_accel_d = 0.4 * CV.MPH_TO_MS

self.pos_limit = pos_limit
self.neg_limit = neg_limit

self.sat_count_rate = 1.0 / rate
self.i_unwind_rate = 0.3 / rate
self.rate = 1.0 / rate
self.sat_limit = sat_limit

self.reset()

@property
def k_p(self):
return interp(self.speed, self._k_p[0], self._k_p[1])

@property
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 _check_saturation(self, control, check_saturation, error):
saturated = (control < self.neg_limit) or (control > self.pos_limit)

if saturated and check_saturation and abs(error) > 0.1:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate

self.sat_count = clip(self.sat_count, 0.0, 1.0)

return self.sat_count > self.sat_limit

def reset(self):
self.p = 0.0
self.id = 0.0
self.f = 0.0
self.sat_count = 0.0
self.saturated = False
self.control = 0
self.last_setpoint = 0.0
self.last_error = 0.0

def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
self.speed = speed

error = float(apply_deadzone(setpoint - measurement, deadzone))

self.p = error * self.k_p
self.f = feedforward * self.k_f

if override:
self.id -= self.i_unwind_rate * float(np.sign(self.id))
else:
i = self.id + error * self.k_i * self.rate
control = self.p + self.f + i

# Update when changing i will move the control away from the limits
# or when i will move towards the sign of the error
if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
(error <= 0 and (control >= self.neg_limit or i > 0.0))) and \
not freeze_integrator:
self.id = i

# if self.op_params.get('enable_long_derivative'):
# if abs(setpoint - self.last_setpoint) / self.rate < self.max_accel_d: # if setpoint isn't changing much
# d = self.k_d * (error - self.last_error)
# if (self.id > 0 and self.id + d >= 0) or (self.id < 0 and self.id + d <= 0): # if changing integral doesn't make it cross zero
# self.id += d

control = self.p + self.f + self.id

self.saturated = self._check_saturation(control, check_saturation, error)

self.last_setpoint = float(setpoint)
self.last_error = float(error)

self.control = clip(control, self.neg_limit, self.pos_limit)
return self.control

class PIController():
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8):
self._k_p = k_p # proportional gain
self._k_i = k_i # integral 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]]

self.pos_limit = pos_limit
self.neg_limit = neg_limit

self.sat_count_rate = 1.0 / rate
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.sat_limit = sat_limit

self.reset()

@property
def k_p(self):
return interp(self.speed, self._k_p[0], self._k_p[1])

@property
def k_i(self):
return interp(self.speed, self._k_i[0], self._k_i[1])

def _check_saturation(self, control, check_saturation, error):
saturated = (control < self.neg_limit) or (control > self.pos_limit)

if saturated and check_saturation and abs(error) > 0.1:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate

self.sat_count = clip(self.sat_count, 0.0, 1.0)

return self.sat_count > self.sat_limit

def reset(self):
self.p = 0.0
self.i = 0.0
self.f = 0.0
self.sat_count = 0.0
self.saturated = False
self.control = 0

def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
self.speed = speed

error = float(apply_deadzone(setpoint - measurement, deadzone))
self.p = error * self.k_p
self.f = feedforward * self.k_f

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

# Update when changing i will move the control away from the limits
# or when i will move towards the sign of the error
if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or
(error <= 0 and (control >= self.neg_limit or i > 0.0))) and \
not freeze_integrator:
self.i = i

control = self.p + self.f + self.i
self.saturated = self._check_saturation(control, check_saturation, error)

self.control = clip(control, self.neg_limit, self.pos_limit)
return self.control
4 changes: 2 additions & 2 deletions selfdrive/thermald/thermald.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
from common.realtime import DT_TRML, sec_since_boot
from common.dict_helpers import strip_deprecated_keys
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.pid import PIDController
from selfdrive.hardware import EON, TICI, PC, HARDWARE
from selfdrive.loggerd.config import get_available_percent
from selfdrive.pandad import get_expected_signature
Expand Down Expand Up @@ -194,7 +194,7 @@ def thermald_thread():
thermal_config = HARDWARE.get_thermal_config()

# TODO: use PI controller for UNO
controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
controller = PIDController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))

if params.get_bool("IsOnroad"):
cloudlog.event("onroad flag not cleared")
Expand Down

0 comments on commit 02562a8

Please sign in to comment.