From 25b08dff3575399a5ad46a7fd8f0c66715d783a6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 14 May 2022 04:01:48 -0700 Subject: [PATCH 1/4] add derivative to torque controller with live tuning --- common/op_params.py | 1 + selfdrive/car/toyota/tunes.py | 9 +++++---- selfdrive/controls/lib/latcontrol_torque.py | 14 +++++++++++++- selfdrive/controls/lib/pid.py | 6 +++--- 4 files changed, 22 insertions(+), 8 deletions(-) diff --git a/common/op_params.py b/common/op_params.py index fca9c0757f2b88..d1153472d69a7d 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -99,6 +99,7 @@ def __init__(self): self.fork_params = { # 'camera_offset': Param(-0.04 if TICI else 0.06, NUMBER, 'Your camera offset to use in lane_planner.py, live=True), + 'torque_derivative': Param(5.0, NUMBER, 'Derivative used in the lateral torque controller', live=True), 'global_df_mod': Param(1.0, NUMBER, 'The multiplier for the current distance used by dynamic follow. The range is limited from 0.85 to 2.5\n' 'Smaller values will get you closer, larger will get you farther\n' 'This is multiplied by any profile that\'s active. Set to 1. to disable', live=True), diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index 5fe4716b74d80c..0ca824b879ff9a 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -53,13 +53,14 @@ def set_long_tune(tune, name): ###### LAT ###### -def set_lat_tune(tune, name, params, MAX_LAT_ACCEL=2.5, FRICTION=.1, kif=(1.0, 0.1, 1.0)): +def set_lat_tune(tune, name, params, MAX_LAT_ACCEL=2.5, FRICTION=.1): if name == LatTunes.TORQUE: tune.init('torque') tune.torque.useSteeringAngle = True - tune.torque.kp = kif[0] / MAX_LAT_ACCEL - tune.torque.kf = kif[2] / MAX_LAT_ACCEL - tune.torque.ki = kif[1] / MAX_LAT_ACCEL + tune.torque.kp = 1.0 / MAX_LAT_ACCEL + tune.torque.kf = 1.0 / MAX_LAT_ACCEL + tune.torque.ki = 0.1 / MAX_LAT_ACCEL + tune.torque.kd = 5.0 / MAX_LAT_ACCEL tune.torque.friction = FRICTION elif name == LatTunes.INDI_PRIUS: diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 9a1ac2f9ea1a67..7abb7cea533b66 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,3 +1,4 @@ +from collections import deque import math from cereal import log @@ -6,6 +7,8 @@ from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY +from common.op_params import opParams + # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to # torque applied to the steering rack. It does not correlate to @@ -31,6 +34,8 @@ def __init__(self, CP, CI): self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle self.friction = CP.lateralTuning.torque.friction self.kf = CP.lateralTuning.torque.kf + self.op_params = opParams() + self.errors = deque([0] * 10, maxlen=10) # 10 frames def reset(self): super().reset() @@ -55,19 +60,26 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature + error = setpoint - measurement - pid_log.error = error + error_rate = (error - self.errors[0]) / len(self.errors) + self.errors.append(error) + # live tune for now + self.pid.k_d = self.op_params.get('torque_derivative') ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY # convert friction into lateral accel units for feedforward friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) ff += friction_compensation / self.kf output_torque = self.pid.update(error, + error_rate=error_rate, override=CS.steeringPressed, feedforward=ff, speed=CS.vEgo, freeze_integrator=CS.steeringRateLimited) pid_log.active = True + pid_log.error = error + pid_log.error_rate = error_rate pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.d = self.pid.d diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index bd1bf90262c7b3..d1f4d74f0ca948 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -34,9 +34,9 @@ def k_p(self): 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]) + # @property + # def k_d(self): + # return interp(self.speed, self._k_d[0], self._k_d[1]) @property def error_integral(self): From 26bf152d519fef01f069e06bd919904a59863cde Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 14 May 2022 04:02:52 -0700 Subject: [PATCH 2/4] one line --- selfdrive/controls/lib/latcontrol_torque.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7abb7cea533b66..e99568fd91d249 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -71,8 +71,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi # convert friction into lateral accel units for feedforward friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) ff += friction_compensation / self.kf - output_torque = self.pid.update(error, - error_rate=error_rate, + output_torque = self.pid.update(error, error_rate=error_rate, override=CS.steeringPressed, feedforward=ff, speed=CS.vEgo, freeze_integrator=CS.steeringRateLimited) From 249aabbf649f4d4eb9d901807d1f7d47ac80ee17 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 14 May 2022 04:15:23 -0700 Subject: [PATCH 3/4] temp fix --- selfdrive/thermald/fan_controller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index b1c7013297f5c4..f1bb17ea926141 100644 --- a/selfdrive/thermald/fan_controller.py +++ b/selfdrive/thermald/fan_controller.py @@ -19,6 +19,7 @@ def __init__(self) -> None: self.last_ignition = False self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) + self.controller.k_d = 0 def update(self, max_cpu_temp: float, ignition: bool) -> int: self.controller.neg_limit = -(80 if ignition else 30) From a2a676afd6f2c946a0bcedffdd26a8fa18e11b37 Mon Sep 17 00:00:00 2001 From: sshane Date: Sat, 14 May 2022 08:55:25 -0700 Subject: [PATCH 4/4] couple of fixes --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- selfdrive/controls/lib/pid.py | 1 + selfdrive/thermald/fan_controller.py | 1 - 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e99568fd91d249..b551bf01f89b28 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -78,7 +78,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi pid_log.active = True pid_log.error = error - pid_log.error_rate = error_rate + pid_log.errorRate = error_rate pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.d = self.pid.d diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index d1f4d74f0ca948..64807f2f18725e 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -16,6 +16,7 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, self._k_i = [[0], [self._k_i]] if isinstance(self._k_d, Number): self._k_d = [[0], [self._k_d]] + self.k_d = 0. self.pos_limit = pos_limit self.neg_limit = neg_limit diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index f1bb17ea926141..b1c7013297f5c4 100644 --- a/selfdrive/thermald/fan_controller.py +++ b/selfdrive/thermald/fan_controller.py @@ -19,7 +19,6 @@ def __init__(self) -> None: self.last_ignition = False self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) - self.controller.k_d = 0 def update(self, max_cpu_temp: float, ignition: bool) -> int: self.controller.neg_limit = -(80 if ignition else 30)