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/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/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9a64c44838a358..02c688be89225f 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -392,15 +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] - 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] @@ -419,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 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/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 3db6781674ea5f..a31b8a7570c9f5 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, 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 cc2b4f15053a3d..252e2626ea83d5 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,13 @@ 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, + derivative_period=100, # 1 second + convert=compute_gb) self.v_pid = 0.0 self.last_output_gb = 0.0 @@ -86,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 diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 9d0b942ad6bb8d..fbf7de6b32b365 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -13,8 +13,9 @@ 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, convert=None): +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 @@ -27,6 +28,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() @@ -41,6 +44,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): @@ -72,8 +76,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,106 +103,8 @@ 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) 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