Lateral PID: move steer feedforward to CarInterface (#22411)

* move steer feedforward to car interface, in car parameters

* LatControlPID.get_steer_feedforward()

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):

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':

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
+      # offset does not contribute to resistive torque
-      steer_feedforward = angle_steers_des_no_offset  # offset does not contribute to resistive torque
+      steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
-      steer_feedforward *= CS.vEgo**2  # proportional to realigning tire momentum (~ lateral accel)
 
       deadzone = 0.0