refactor angle to use LatControlPID with zero gains

selfdrive/car/interfaces.py

@@ -155,6 +155,7 @@ class CarInterfaceBase(ABC):
 
     # standard ALC params
     ret.steerControlType = car.CarParams.SteerControlType.torque
+    # TODO: set PID gains?
     ret.minSteerSpeed = 0.
     ret.wheelSpeedFactor = 1.0
 

selfdrive/car/toyota/interface.py

@@ -14,6 +14,16 @@ class CarInterface(CarInterfaceBase):
   def get_pid_accel_limits(CP, current_speed, cruise_speed):
     return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
 
+  @staticmethod
+  def get_angle_feedforward(desired_angle, v_ego):
+    return desired_angle
+
+  def get_steer_feedforward_function(self):
+    if self.CP.steerControlType == car.CarParams.SteerControlType.torque:
+      return CarInterfaceBase.get_steer_feedforward_default
+    else:
+      return self.get_angle_feedforward
+
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long):
     ret.carName = "toyota"
@@ -212,6 +222,13 @@ class CarInterface(CarInterfaceBase):
     if ret.steerControlType == car.CarParams.SteerControlType.angle:
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
 
+      ret.lateralTuning.init('pid')
+      ret.lateralTuning.pid.kiBP = [0.0]
+      ret.lateralTuning.pid.kpBP = [0.0]
+      ret.lateralTuning.pid.kpV = [0.5]
+      ret.lateralTuning.pid.kiV = [0.1]
+      ret.lateralTuning.pid.kf = 1.0
+
     # we can't use the fingerprint to detect this reliably, since
     # the EV gas pedal signal can take a couple seconds to appear
     if candidate in EV_HYBRID_CAR:

selfdrive/controls/controlsd.py

@@ -157,9 +157,9 @@ class Controls:
     self.VM = VehicleModel(self.CP)
 
     self.LaC: LatControl
-    if self.CP.steerControlType == SteerControlType.angle:
-      self.LaC = LatControlAngle(self.CP, self.CI)
-    elif self.CP.lateralTuning.which() == 'pid':
+    # if self.CP.steerControlType == SteerControlType.angle:
+    #   self.LaC = LatControlAngle(self.CP, self.CI)
+    if self.CP.lateralTuning.which() == 'pid':
       self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
       self.LaC = LatControlINDI(self.CP, self.CI)
@@ -619,8 +619,7 @@ class Controls:
         lac_log.saturated = abs(actuators.steer) >= 0.9
 
     # Send a "steering required alert" if saturation count has reached the limit
-    if (lac_log.active and not CS.steeringPressed and self.CP.steerControlType == SteerControlType.torque and
-        self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode):
+    if (lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode):
       undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
       turning = abs(lac_log.desiredLateralAccel) > 1.0
       good_speed = CS.vEgo > 5
@@ -774,8 +773,8 @@ class Controls:
     lat_tuning = self.CP.lateralTuning.which()
     if self.joystick_mode:
       controlsState.lateralControlState.debugState = lac_log
-    elif self.CP.steerControlType == SteerControlType.angle:
-      controlsState.lateralControlState.angleState = lac_log
+    # elif self.CP.steerControlType == SteerControlType.angle:
+    #   controlsState.lateralControlState.angleState = lac_log
     elif lat_tuning == 'pid':
       controlsState.lateralControlState.pidState = lac_log
     elif lat_tuning == 'torque':

selfdrive/controls/lib/latcontrol.py

@@ -8,8 +8,9 @@ MIN_STEER_SPEED = 0.0
 
 class LatControl(ABC):
   def __init__(self, CP, CI):
+    self.CP = CP
     self.sat_count_rate = 1.0 * DT_CTRL
-    self.sat_limit = CP.steerLimitTimer
+    self.sat_limit = self.CP.steerLimitTimer
     self.sat_count = 0.
 
     # we define the steer torque scale as [-1.0...1.0]

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,16 +1,23 @@
 import math
 
-from cereal import log
+from cereal import car, log
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 from selfdrive.controls.lib.pid import PIDController
 
+SteerControlType = car.CarParams.SteerControlType
+STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+
 
 class LatControlPID(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
+    kargs = {}
+    if self.CP.steerControlType == SteerControlType.torque:
+      kargs.update({"pos_limit": self.steer_max, "neg_limit": -self.steer_max})
+
     self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                              (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
-                             k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
+                             k_f=CP.lateralTuning.pid.kf, **kargs)
     self.get_steer_feedforward = CI.get_steer_feedforward_function()
 
   def reset(self):
@@ -43,6 +50,15 @@ class LatControlPID(LatControl):
       pid_log.i = self.pid.i
       pid_log.f = self.pid.f
       pid_log.output = output_steer
-      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
+
+      if self.CP.steerControlType != SteerControlType.torque:
+        angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
+        pid_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited)
+      else:
+        pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
+
+    if self.CP.steerControlType != SteerControlType.torque:
+      angle_steers_des = output_steer
+      output_steer = 0
 
     return output_steer, angle_steers_des, pid_log