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..b551bf01f89b28 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,25 @@ 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, + 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.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 bd1bf90262c7b3..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 @@ -34,9 +35,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):