diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index c58576959c..672a6b606b 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -56,6 +56,12 @@ class CarInterfaceBase(): def init(CP, logcan, sendcan): pass + @staticmethod + def get_steer_feedforward(desired_angle, v_ego): + # Proportional to realigning tire momentum: lateral acceleration. + # TODO: something with lateralPlan.curvatureRates + return desired_angle * (v_ego**2) + # returns a set of default params to avoid repetition in car specific params @staticmethod def get_std_params(candidate, fingerprint): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fa18bfd9e2..4df2f91926 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -127,7 +127,7 @@ class Controls: if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP) elif 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 df065b4a00..b495571385 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -6,11 +6,12 @@ from cereal import log class LatControlPID(): - def __init__(self, CP): + def __init__(self, CP, CI): self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, sat_limit=CP.steerLimitTimer) + self.get_steer_feedforward = CI.get_steer_feedforward def reset(self): self.pid.reset() @@ -23,7 +24,7 @@ class LatControlPID(): angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg - pid_log.angleError = angle_steers_des - CS.steeringAngleDeg + pid_log.angleError = angle_steers_des - CS.steeringAngleDeg if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False @@ -33,9 +34,8 @@ class LatControlPID(): self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max - # TODO: feedforward something based on lat_plan.rateSteers - steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque - steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) + # offset does not contribute to resistive torque + steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) deadzone = 0.0