From 65631f34061750958f586bdd755a974e89bfa742 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 23 Aug 2021 00:09:42 -0600 Subject: [PATCH 01/10] tune delay compensation --- selfdrive/controls/lib/longcontrol.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index cc2b4f15053a3d..d2e1a48455c62f 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -83,8 +83,9 @@ def update(self, active, CS, CP, long_plan, extras): # Interp control trajectory # TODO estimate car specific lag, use .15s for now if len(long_plan.speeds) == CONTROL_N: - accel_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.15, 0.3]) # 10 to 80 mph - v_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.speeds) + accel_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.2, 0.4]) # 10 to 80 mph + speed_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.05, 0.1]) + v_target = interp(speed_delay, T_IDXS[:CONTROL_N], long_plan.speeds) v_target_future = long_plan.speeds[-1] a_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.accels) else: From 2c686b1f139396d15726ae0f4329ac7e0077eaf7 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 23 Aug 2021 00:15:23 -0600 Subject: [PATCH 02/10] tweak tune --- selfdrive/car/toyota/interface.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9a64c44838a358..51de2fa1fbfa2f 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -392,14 +392,22 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu + # if ret.enableGasInterceptor: + # # Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap + # ret.gasMaxBP = [0., MIN_ACC_SPEED] + # ret.gasMaxV = [0.2, 0.5] + # ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] + # ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] + # ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] + # ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36] if ret.enableGasInterceptor: - # Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap - ret.gasMaxBP = [0., MIN_ACC_SPEED] - ret.gasMaxV = [0.2, 0.5] - ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] - ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36] + # Default longitudinal tune with tweaks + ret.longitudinalTuning.deadzoneBP = [0., 9.] + ret.longitudinalTuning.deadzoneV = [0., .15] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kpV = [3.6 * 0.9, 2.4 * 0.9, 1.5 * 0.9] + ret.longitudinalTuning.kiV = [0.54 * 0.8, 0.36 * 0.8] elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: # Improved longitudinal tune ret.longitudinalTuning.deadzoneBP = [0., 8.05] From b22b99f6e041c30034c34e32d801b7bae7d8fb4a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 23 Aug 2021 00:25:35 -0600 Subject: [PATCH 03/10] add derivative --- cereal | 2 +- selfdrive/car/toyota/interface.py | 2 + selfdrive/controls/lib/latcontrol_pid.py | 10 +-- selfdrive/controls/lib/longcontrol.py | 17 ++-- selfdrive/controls/lib/pid.py | 100 +---------------------- 5 files changed, 16 insertions(+), 115 deletions(-) diff --git a/cereal b/cereal index d20a28bafaa2a4..420e3b0fc27e20 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit d20a28bafaa2a4ac08fda0fbafe86e71b47c242a +Subproject commit 420e3b0fc27e202b19fe749d02283f3ba2fc8e0c diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 51de2fa1fbfa2f..4db8422839ffd3 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -406,8 +406,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kdBP = [0., 35.] ret.longitudinalTuning.kpV = [3.6 * 0.9, 2.4 * 0.9, 1.5 * 0.9] ret.longitudinalTuning.kiV = [0.54 * 0.8, 0.36 * 0.8] + ret.longitudinalTuning.kdV = [0.2, 0.1] elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: # Improved longitudinal tune ret.longitudinalTuning.deadzoneBP = [0., 8.05] diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 3db6781674ea5f..8dbdf88c3c9f43 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -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) self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned def reset(self): diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index d2e1a48455c62f..7f01d9339aca35 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -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 @@ -58,15 +58,12 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, class LongControl(): def __init__(self, CP, compute_gb): 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, - convert=compute_gb) + 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, + convert=compute_gb) self.v_pid = 0.0 self.last_output_gb = 0.0 diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 9d0b942ad6bb8d..f3aeed500bd002 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -13,7 +13,7 @@ def apply_deadzone(error, deadzone): error = 0. return error -class LatPIDController(): +class PIDController: def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain @@ -104,101 +104,3 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri 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, convert=None): - 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 - - 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.convert = convert - - 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 - - if self.convert is not None: - control = self.convert(control, speed=self.speed) - - # 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 - if self.convert is not None: - control = self.convert(control, speed=self.speed) - - 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 From b55f8820cb85534a03d45fa3844f30424673a904 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 23 Aug 2021 00:29:02 -0600 Subject: [PATCH 04/10] tweak derivative --- selfdrive/car/toyota/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 4db8422839ffd3..2247bd6b2cc636 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -406,10 +406,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kdBP = [0., 35.] + ret.longitudinalTuning.kdBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6 * 0.9, 2.4 * 0.9, 1.5 * 0.9] ret.longitudinalTuning.kiV = [0.54 * 0.8, 0.36 * 0.8] - ret.longitudinalTuning.kdV = [0.2, 0.1] + ret.longitudinalTuning.kdV = [0.05, 0.1, 0.3] elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: # Improved longitudinal tune ret.longitudinalTuning.deadzoneBP = [0., 8.05] From bcbc633356ea21dae523071af7149b3b7005aaeb Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 1 Sep 2021 23:54:17 -0500 Subject: [PATCH 05/10] pedal tuning is almost identical --- selfdrive/car/toyota/interface.py | 25 ++++++------------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 2247bd6b2cc636..02c688be89225f 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -392,25 +392,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu - # if ret.enableGasInterceptor: - # # Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap - # ret.gasMaxBP = [0., MIN_ACC_SPEED] - # ret.gasMaxV = [0.2, 0.5] - # ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - # ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] - # ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] - # ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36] - if ret.enableGasInterceptor: - # Default longitudinal tune with tweaks - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kdBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [3.6 * 0.9, 2.4 * 0.9, 1.5 * 0.9] - ret.longitudinalTuning.kiV = [0.54 * 0.8, 0.36 * 0.8] - ret.longitudinalTuning.kdV = [0.05, 0.1, 0.3] - elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: + if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2] and not ret.enableGasInterceptor: # Improved longitudinal tune ret.longitudinalTuning.deadzoneBP = [0., 8.05] ret.longitudinalTuning.deadzoneV = [.0, .14] @@ -429,6 +411,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiV = [0.54, 0.36] + ret.longitudinalTuning.kdV = [0.5, 1.2, 2.5] # TODO: why is this increasing? + + if ret.enableGasInterceptor: + # Default longitudinal tune with tweaks for pedal + ret.longitudinalTuning.kpV = [3.6 * 0.92, 2.4 * 0.95, 1.5] return ret From be343579bbee7cbada145b37b55c017fb2f472ec Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 2 Sep 2021 00:00:38 -0500 Subject: [PATCH 06/10] add derivative_period param to pid --- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/longcontrol.py | 1 + selfdrive/controls/lib/pid.py | 10 ++++++---- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 8dbdf88c3c9f43..a31b8a7570c9f5 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -10,7 +10,7 @@ def __init__(self, CP): 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) + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer, derivative_period=5) self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned def reset(self): diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 7f01d9339aca35..7d90fab44f47cb 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -63,6 +63,7 @@ def __init__(self, CP, compute_gb): (CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV), rate=RATE, sat_limit=0.8, + derivative_period=100, # 1 second convert=compute_gb) self.v_pid = 0.0 self.last_output_gb = 0.0 diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index f3aeed500bd002..946e2fde2bce0c 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -14,7 +14,7 @@ def apply_deadzone(error, deadzone): return error class PIDController: - def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, derivative_period=100, convert=None): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain self._k_d = k_d # derivative gain @@ -27,6 +27,8 @@ 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.derivative_period = derivative_period # frames in time for derivative calculation + assert self.derivative_period >= 0 self.convert = convert self.reset() @@ -72,8 +74,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.derivative_period: # makes sure list is long enough + d = (error - self.errors[-self.derivative_period]) / self.derivative_period # get deriv in terms of 100hz (tune scale doesn't change) d *= self.k_d if override: @@ -99,7 +101,7 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri self.saturated = self._check_saturation(control, check_saturation, error) self.errors.append(float(error)) - while len(self.errors) > 5: + while len(self.errors) > self.derivative_period: self.errors.pop(0) self.control = clip(control, self.neg_limit, self.pos_limit) From a71a31f8e73166974136d8a0fdb9608bde58eb18 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 2 Sep 2021 00:05:08 -0500 Subject: [PATCH 07/10] revert --- selfdrive/controls/lib/longcontrol.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 7d90fab44f47cb..ec23680345dd66 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -81,9 +81,8 @@ def update(self, active, CS, CP, long_plan, extras): # Interp control trajectory # TODO estimate car specific lag, use .15s for now if len(long_plan.speeds) == CONTROL_N: - accel_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.2, 0.4]) # 10 to 80 mph - speed_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.05, 0.1]) - v_target = interp(speed_delay, T_IDXS[:CONTROL_N], long_plan.speeds) + accel_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.15, 0.3]) # 10 to 80 mph + v_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.speeds) v_target_future = long_plan.speeds[-1] a_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.accels) else: From 6b8d88a1796d245a0092749efc58381966e6e202 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 2 Sep 2021 00:08:42 -0500 Subject: [PATCH 08/10] temporary --- selfdrive/controls/lib/longcontrol.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ec23680345dd66..252e2626ea83d5 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -84,7 +84,9 @@ def update(self, active, CS, CP, long_plan, extras): accel_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.15, 0.3]) # 10 to 80 mph v_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.speeds) v_target_future = long_plan.speeds[-1] - a_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.accels) + print('old a_target: {}'.format(round(interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.accels), 2))) + a_target = 2 * (v_target - long_plan.speeds[0]) / DEFAULT_LONG_LAG - long_plan.accels[0] + print('new a_target: {}'.format(round(a_target, 2))) else: v_target = 0.0 v_target_future = 0.0 From 2d01334ea8994f3beaeb897746f9e2c18bed6dbc Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 2 Sep 2021 00:09:52 -0500 Subject: [PATCH 09/10] live tuning --- common/op_params.py | 1 + selfdrive/controls/lib/pid.py | 1 + 2 files changed, 2 insertions(+) diff --git a/common/op_params.py b/common/op_params.py index 2a54c406f4cd84..44baefc0234664 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -100,6 +100,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\n' 'If you have a comma three, note that the default camera offset is -0.04!', live=True), + 'long_kd': Param(0.5, NUMBER, live=True), 'dynamic_follow': Param('stock', str, static=True, hidden=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' diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 946e2fde2bce0c..40a0c7be580acf 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -43,6 +43,7 @@ def k_i(self): @property def k_d(self): + return self.op_params.get('long_kd') return interp(self.speed, self._k_d[0], self._k_d[1]) def _check_saturation(self, control, check_saturation, error): From d9168e173fba7d65505b8ed4260d38da2ff683b2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 2 Sep 2021 02:51:29 -0500 Subject: [PATCH 10/10] fixes --- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/pid.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e8086f094f0c35..12c1f68b8a4432 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -668,7 +668,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) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 40a0c7be580acf..fbf7de6b32b365 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -15,6 +15,7 @@ def apply_deadzone(error, deadzone): class PIDController: def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, derivative_period=100, convert=None): + 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