diff --git a/cereal/car.capnp b/cereal/car.capnp index be31f6628d8efe..aaed374f422f11 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -301,8 +301,12 @@ struct CarParams { tireStiffnessRear @14 :Float32; # [N/rad] rear tire coeff of stiff # Kp and Ki for the lateral control - steerKp @15 :Float32; - steerKi @16 :Float32; + steerKpBP @42 :List(Float32); + steerKpV @43 :List(Float32); + steerKiBP @44 :List(Float32); + steerKiV @45 :List(Float32); + steerKpDEPRECATED @15 :Float32; + steerKiDEPRECATED @16 :Float32; steerKf @25 :Float32; # Kp and Ki for the longitudinal control diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index adac338e60a9bd..18323b834ccdd4 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -151,6 +151,7 @@ def get_params(candidate, fingerprint): tireStiffnessFront_civic = 85400 tireStiffnessRear_civic = 90000 + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] if candidate == CAR.CIVIC: stop_and_go = True ret.mass = mass_civic @@ -159,7 +160,7 @@ def get_params(candidate, fingerprint): ret.steerRatio = 13.0 # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] - ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] + ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [3.6, 2.4, 1.5] @@ -173,7 +174,7 @@ def get_params(candidate, fingerprint): ret.steerRatio = 15.3 # Acura at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310'] - ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] + ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -185,7 +186,7 @@ def get_params(candidate, fingerprint): ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.3 - ret.steerKp, ret.steerKi = 0.8, 0.24 + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -197,7 +198,7 @@ def get_params(candidate, fingerprint): ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 - ret.steerKp, ret.steerKi = 0.8, 0.24 + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -209,7 +210,7 @@ def get_params(candidate, fingerprint): ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 - ret.steerKp, ret.steerKi = 0.6, 0.18 + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -221,7 +222,7 @@ def get_params(candidate, fingerprint): ret.wheelbase = 2.81 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 - ret.steerKp, ret.steerKi = 0.38, 0.11 + ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 53cfbb180a5d88..acd22e9ae25c7d 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -72,12 +72,13 @@ def get_params(candidate, fingerprint): tireStiffnessFront_civic = 85400 tireStiffnessRear_civic = 90000 + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 14.5 # TODO: find exact value for Prius ret.mass = 3045./2.205 + std_cargo - ret.steerKp, ret.steerKi = 0.6, 0.05 + ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 2. elif candidate in [CAR.RAV4, CAR.RAV4H]: @@ -85,7 +86,7 @@ def get_params(candidate, fingerprint): ret.wheelbase = 2.65 ret.steerRatio = 14.5 # Rav4 2017 ret.mass = 3650./2.205 + std_cargo # mean between normal and hybrid - ret.steerKp, ret.steerKi = 0.6, 0.05 + ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. elif candidate == CAR.COROLLA: @@ -93,7 +94,7 @@ def get_params(candidate, fingerprint): ret.wheelbase = 2.70 ret.steerRatio = 17.8 ret.mass = 2860./2.205 + std_cargo # mean between normal and hybrid - ret.steerKp, ret.steerKi = 0.2, 0.05 + ret.steerKpV, ret.steerKiV = [[0.2], [0.05]] ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. elif candidate == CAR.LEXUS_RXH: @@ -101,7 +102,7 @@ def get_params(candidate, fingerprint): ret.wheelbase = 2.79 ret.steerRatio = 16. # official specs say 14.8, but it does not seem right ret.mass = 4481./2.205 + std_cargo # mean between min and max - ret.steerKp, ret.steerKi = 0.6, 0.1 + ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = .8 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 74020a59204465..d2737a0496f310 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -26,7 +26,9 @@ def get_steer_max(CP, v_ego): class LatControl(object): def __init__(self, VM): - self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0) + self.pid = PIController((VM.CP.steerKpBP, VM.CP.steerKpV), + (VM.CP.steerKiBP, VM.CP.steerKiV), + k_f=VM.CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(VM.CP.steerRateCost) @@ -103,7 +105,7 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel) - output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward) + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego) self.sat_flag = self.pid.saturated return output_steer diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 40ae836b9ece5c..b25275d668c28e 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -1,6 +1,5 @@ import numpy as np from common.numpy_fast import clip, interp -import numbers def apply_deadzone(error, deadzone): if error > deadzone: @@ -30,21 +29,11 @@ def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, s @property def k_p(self): - if isinstance(self._k_p, numbers.Number): - kp = self._k_p - else: - kp = interp(self.speed, self._k_p[0], self._k_p[1]) - - return kp + return interp(self.speed, self._k_p[0], self._k_p[1]) @property def k_i(self): - if isinstance(self._k_i, numbers.Number): - ki = self._k_i - else: - ki = interp(self.speed, self._k_i[0], self._k_i[1]) - - return ki + return interp(self.speed, self._k_i[0], self._k_i[1]) def _check_saturation(self, control, override, error): saturated = (control < self.neg_limit) or (control > self.pos_limit)