From 53a2d1758afa5449f05ece07a0d4c480f6202c72 Mon Sep 17 00:00:00 2001 From: Tim Wilson Date: Sat, 12 Mar 2022 15:58:52 -0700 Subject: [PATCH 1/2] volt: qadmus' sigmoidal feedforward https://github.com/commaai/openpilot/commit/2e0bc9d3653defb51e229d45e0c68447c51785a4, D back in PID controller, and new volt PID tune no live PID tuning for custom feedforward cars (volt) --- cereal/car.capnp | 11 +++-- selfdrive/car/gm/interface.py | 62 +++++++++++++++++++----- selfdrive/car/interfaces.py | 10 ++++ selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 49 ++++++++++--------- selfdrive/controls/lib/longcontrol.py | 4 +- selfdrive/controls/lib/pid.py | 32 ++++++++++-- selfdrive/kegman_conf.py | 6 ++- 8 files changed, 131 insertions(+), 45 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 1efac2d338c663..f0abbb69bcb450 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -441,7 +441,9 @@ struct CarParams { kpV @1 :List(Float32); kiBP @2 :List(Float32); kiV @3 :List(Float32); - kf @4 :Float32; + kdBP @4 :List(Float32) = [0.]; + kdV @5 :List(Float32) = [0.]; + kf @6 :Float32; } struct LongitudinalPIDTuning { @@ -449,8 +451,11 @@ struct CarParams { kpV @1 :List(Float32); kiBP @2 :List(Float32); kiV @3 :List(Float32); - deadzoneBP @4 :List(Float32); - deadzoneV @5 :List(Float32); + kdBP @4 :List(Float32) = [0.]; + kdV @5 :List(Float32) = [0.]; + kf @8 :Float32; + deadzoneBP @6 :List(Float32); + deadzoneV @7 :List(Float32); } struct LateralINDITuning { diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index bfb4cf46843223..e01b64b8628307 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,4 +1,6 @@ #!/usr/bin/env python3 + +from math import fabs, sin from cereal import car from common.numpy_fast import interp from selfdrive.config import Conversions as CV @@ -58,6 +60,21 @@ def calc_accel_override(a_ego, a_target, v_ego, v_target): return float(max(max_accel, a_target / FOLLOW_AGGRESSION)) * min(speedLimiter, accelLimiter) + # Volt determined by iteratively plotting and minimizing error for f(angle, speed) = steer. + @staticmethod + def get_steer_feedforward_volt(desired_angle, v_ego): + # maps [-inf,inf] to [-1,1]: sigmoid(34.4 deg) = sigmoid(1) = 0.5 + # 1 / 0.02904609 = 34.4 deg ~= 36 deg ~= 1/10 circle? Arbitrary? + desired_angle *= 0.02904609 + sigmoid = desired_angle / (1 + fabs(desired_angle)) + return 0.10006696 * sigmoid * (v_ego + 3.12485927) + + def get_steer_feedforward_function(self): + if self.CP.carFingerprint == CAR.VOLT: + return self.get_steer_feedforward_volt + else: + return CarInterfaceBase.get_steer_feedforward_default + @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) @@ -65,6 +82,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.safetyModel = car.CarParams.SafetyModel.gm ret.enableCruise = False # stock cruise control is kept off + + ret.stoppingControl = True + ret.startAccel = 0.8 + + ret.steerLimitTimer = 0.4 + ret.radarTimeStep = 1/15 # GM radar runs at 15Hz instead of standard 20Hz + # GM port is a community feature # TODO: make a port that uses a car harness and it only intercepts the camera ret.communityFeature = True @@ -84,14 +108,37 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.steerRateCost = 1.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kpV = [2.4, 1.5] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.36] + if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = -1 ret.mass = 1607. + STD_CARGO_KG ret.wheelbase = 2.69 - ret.steerRatio = 15.7 + ret.steerRatio = 17.7 # Stock 15.7, LiveParameters + ret.steerRateCost = 1.0 + tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters ret.steerRatioRear = 0. - ret.centerToFront = ret.wheelbase * 0.4 # wild guess + ret.centerToFront = 0.45 * ret.wheelbase # from Volt Gen 1 + + ret.lateralTuning.pid.kpBP = [0., 40.] + ret.lateralTuning.pid.kpV = [0.0, .20] + ret.lateralTuning.pid.kiBP = [0.0, 40.] + ret.lateralTuning.pid.kiV = [0.025, 0.022] + ret.lateralTuning.pid.kdBP = [i * CV.MPH_TO_MS for i in [15., 30., 55.]] + ret.lateralTuning.pid.kdV = [0.15, 0.26, 0.32] + ret.lateralTuning.pid.kf = 1. # !!! ONLY for sigmoid feedforward !!! + ret.steerActuatorDelay = 0.2 + + # Only tuned to reduce oscillations. TODO. + ret.longitudinalTuning.kpV = [1.7, 1.3] + ret.longitudinalTuning.kiBP = [5., 35.] + ret.longitudinalTuning.kiV = [0.32, 0.34] + ret.longitudinalTuning.kdV = [0.8, 0.2] + ret.longitudinalTuning.kdBP = [5., 25.] elif candidate == CAR.MALIBU: # supports stop and go, but initial engage must be above 18mph (which include conservatism) @@ -144,17 +191,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.longitudinalTuning.kpBP = [5., 35.] - ret.longitudinalTuning.kpV = [2.4, 1.5] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.36] - - ret.stoppingControl = True - ret.startAccel = 0.8 - - ret.steerLimitTimer = 0.4 - ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz - return ret # returns a car.CarState diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 497d38c0b01211..9ef426f46bc455 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -47,6 +47,16 @@ def compute_gb(accel, speed): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None): raise NotImplementedError + + @staticmethod + def get_steer_feedforward_default(desired_angle, v_ego): + # Proportional to realigning tire momentum: lateral acceleration. + # TODO: something with lateralPlan.curvatureRates + return desired_angle * (v_ego**2) + + @staticmethod + def get_steer_feedforward_function(): + return CarInterfaceBase.get_steer_feedforward_default # returns a set of default params to avoid repetition in car specific params @staticmethod diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 832582482a476b..1d804184b5c840 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -111,7 +111,7 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.VM = VehicleModel(self.CP) if self.CP.lateralTuning.which() == 'pid': - self.LaC = LatControlPID(self.CP) + self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP) elif self.CP.lateralTuning.which() == 'lqr': diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index c3ad89b94d2927..d06cdf07bc48f8 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,4 +1,4 @@ -from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.drive_helpers import get_steer_max from cereal import car from cereal import log @@ -6,13 +6,16 @@ class LatControlPID(): - def __init__(self, CP): + def __init__(self, CP, CI): self.kegman = kegman_conf(CP) + self.CI = CI self.deadzone = float(self.kegman.conf['deadzone']) - self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), + 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, neg_limit=-1.0, - sat_limit=CP.steerLimitTimer) + sat_limit=CP.steerLimitTimer, derivative_period=0.1) + self.get_steer_feedforward = CI.get_steer_feedforward_function() self.angle_steers_des = 0. self.mpc_frame = 0 @@ -20,20 +23,25 @@ def reset(self): self.pid.reset() def live_tune(self, CP): - self.mpc_frame += 1 - if self.mpc_frame % 300 == 0: - # live tuning through /data/openpilot/tune.py overrides interface.py settings - self.kegman = kegman_conf() - if self.kegman.conf['tuneGernby'] == "1": - self.steerKpV = [float(self.kegman.conf['Kp'])] - self.steerKiV = [float(self.kegman.conf['Ki'])] - self.steerKf = float(self.kegman.conf['Kf']) - self.pid = PIController((CP.lateralTuning.pid.kpBP, self.steerKpV), - (CP.lateralTuning.pid.kiBP, self.steerKiV), - k_f=self.steerKf, pos_limit=1.0) - self.deadzone = float(self.kegman.conf['deadzone']) + if self.get_steer_feedforward == self.CI.get_steer_feedforward_default: + self.mpc_frame += 1 + if self.mpc_frame % 300 == 0: + # live tuning through /data/openpilot/tune.py overrides interface.py settings + self.kegman = kegman_conf() + if self.kegman.conf['tuneGernby'] == "1": + self.steerKpV = [float(self.kegman.conf['Kp'])] + self.steerKiV = [float(self.kegman.conf['Ki'])] + self.steerKdV = [float(self.kegman.conf['Kd'])] + # custom feedforward values are not allowed to be tuned. + self.steerKf = float(self.kegman.conf['Kf']) + self.pid = PIDController((CP.lateralTuning.pid.kpBP, self.steerKpV), + (CP.lateralTuning.pid.kiBP, self.steerKiV), + (CP.lateralTuning.pid.kdBP, self.steerKdV), + k_f=self.steerKf, pos_limit=self.pid.pos_limit, neg_limit=self.pid.neg_limit, + sat_limit=CP.steerLimitTimer, derivative_period=0.1) + self.deadzone = float(self.kegman.conf['deadzone']) - self.mpc_frame = 0 + self.mpc_frame = 0 def update(self, active, CS, CP, lat_plan): @@ -52,11 +60,8 @@ def update(self, active, CS, CP, lat_plan): steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max - steer_feedforward = self.angle_steers_des # feedforward desired angle - if CP.steerControlType == car.CarParams.SteerControlType.torque: - # TODO: feedforward something based on lat_plan.rateSteers - steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque - steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) + + steer_feedforward = self.get_steer_feedforward(self.angle_steers_des, CS.vEgo) deadzone = self.deadzone diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6b279109aaceb0..89e204417c8fea 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,7 +1,7 @@ from cereal import log from common.numpy_fast import clip, interp from common.params import Params -from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid import PIDController from selfdrive.kegman_conf import kegman_conf kegman = kegman_conf() @@ -59,7 +59,7 @@ 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 - self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), + self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=RATE, sat_limit=0.8, diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 916b95c9ea0c4b..ae77b83f190539 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -1,4 +1,5 @@ import numpy as np +from numbers import Number from common.numpy_fast import clip, interp def apply_deadzone(error, deadzone): @@ -10,11 +11,18 @@ def apply_deadzone(error, deadzone): error = 0. return error -class PIController(): - def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): +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, convert=None, derivative_period=1.): 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.pos_limit = pos_limit self.neg_limit = neg_limit @@ -25,6 +33,9 @@ def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, s self.sat_limit = sat_limit self.convert = convert + self._d_period = round(derivative_period * rate) # period of time for derivative calculation (seconds converted to frames) + self._d_period_recip = 1. / self._d_period + self.reset() @property @@ -35,6 +46,10 @@ 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]) + def _check_saturation(self, control, check_saturation, error): saturated = (control < self.neg_limit) or (control > self.pos_limit) @@ -54,6 +69,7 @@ def reset(self): self.sat_count = 0.0 self.saturated = False self.control = 0 + self.errors = [] def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): self.speed = speed @@ -61,6 +77,12 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri error = float(apply_deadzone(setpoint - measurement, deadzone)) self.p = error * self.k_p self.f = feedforward * self.k_f + + kd = self.k_d + d = 0 + if len(self.errors) >= self._d_period and kd > 0.: # makes sure we have enough history for period + d = (error - self.errors[-self._d_period]) * self._d_period_recip # get deriv in terms of 100hz (tune scale doesn't change) + d *= kd if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) @@ -78,11 +100,15 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri not freeze_integrator: self.i = i - control = self.p + self.f + self.i + control = self.p + self.f + self.i + d if self.convert is not None: control = self.convert(control, speed=self.speed) self.saturated = self._check_saturation(control, check_saturation, error) + self.errors.append(float(error)) + while len(self.errors) > self._d_period: + self.errors.pop(0) + self.control = clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py index 553e0cfd258ae3..332ad1ca5322b8 100644 --- a/selfdrive/kegman_conf.py +++ b/selfdrive/kegman_conf.py @@ -21,6 +21,9 @@ def init_config(self, CP): if self.conf['Ki'] == "-1": self.conf['Ki'] = str(round(CP.lateralTuning.pid.kiV[0],3)) write_conf = True + if self.conf.get('Kd',"-1") == "-1": + self.conf['Kd'] = str(round(CP.lateralTuning.pid.kdV[0],3)) + write_conf = True if self.conf['Kf'] == "-1": self.conf['Kf'] = str('{:f}'.format(CP.lateralTuning.pid.kf)) write_conf = True @@ -53,6 +56,7 @@ def read_config(self): self.config.update({"tuneGernby":"1"}) self.config.update({"Kp":"-1"}) self.config.update({"Ki":"-1"}) + self.config.update({"Kd":"-1"}) self.element_updated = True if "liveParams" not in self.config: @@ -150,7 +154,7 @@ def read_config(self): self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"60", "battChargeMax":"70", \ "wheelTouchSeconds":"180", "accelerationMode":"1","battPercOff":"25", "carVoltageMinEonShutdown":"11800", \ "brakeStoppingTarget":"0.25", "tuneGernby":"1", "AutoHold":"0",\ - "Kp":"-1", "Ki":"-1", "liveParams":"1", "leadDistance":"5", "deadzone":"0.0", \ + "Kp":"-1", "Ki":"-1", "Kd":"-1", "liveParams":"1", "leadDistance":"5", "deadzone":"0.0", \ "1barBP0":"-0.1", "1barBP1":"2.25", "2barBP0":"-0.1", "2barBP1":"2.5", "3barBP0":"0.0", \ "3barBP1":"3.0", "1barMax":"2.1", "2barMax":"2.1", "3barMax":"2.1", \ "1barHwy":"0.4", "2barHwy":"0.3", "3barHwy":"0.1", \ From 42d8783084187f4d9575c48e44d1658674e8229c Mon Sep 17 00:00:00 2001 From: Tim Wilson Date: Sun, 13 Mar 2022 18:53:20 -0600 Subject: [PATCH 2/2] curvature-rate-based feedforward offset per https://github.com/qadmus/openpilot/commit/a7e2dae00342d37fc256cfc6ab0850c8cc525a24 --- selfdrive/controls/lib/latcontrol_pid.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index d06cdf07bc48f8..2056d1cb15c7af 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,5 +1,6 @@ from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.drive_helpers import get_steer_max +from selfdrive.config import Conversions as CV from cereal import car from cereal import log from selfdrive.kegman_conf import kegman_conf @@ -63,6 +64,14 @@ def update(self, active, CS, CP, lat_plan): steer_feedforward = self.get_steer_feedforward(self.angle_steers_des, CS.vEgo) + # torque for steer rate. ~0 angle, steer rate ~= steer command. + steer_rate_actual = CS.steeringRateDeg + steer_rate_desired = lat_plan.lat_plan.steeringAngleDeg + speed_mph = CS.vEgo * CV.MS_TO_MPH + steer_rate_max = 0.0389837 * speed_mph**2 - 5.34858 * speed_mph + 223.831 + + steer_feedforward += ((steer_rate_desired - steer_rate_actual) / steer_rate_max) + deadzone = self.deadzone check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed