raw desired curvature as input to all controllers

Author: felsager

selfdrive/controls/controlsd.py

@@ -123,14 +123,8 @@ def state_control(self):
     curvature_limited = curvature_limited or raw_curvature_limited
 
     actuators.curvature = self.desired_curvature
-
-    if self.CP.lateralTuning.which() == 'torque':
-      steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
-                                                        self.steer_limited_by_safety, self.desired_curvature, new_raw_desired_curvature,
-                                                        curvature_limited)  # TODO what if not available
-    else:
-      steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
-                                                      self.steer_limited_by_safety, self.desired_curvature,
+    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+                                                      self.steer_limited_by_safety, self.desired_curvature, self.raw_desired_curvature,
                                                       curvature_limited)  # TODO what if not available
     actuators.torque = float(steer)
     actuators.steeringAngleDeg = float(steeringAngleDeg)

selfdrive/controls/lib/latcontrol.py

@@ -15,7 +15,7 @@ def __init__(self, CP, CI):
     self.steer_max = 1.0
 
   @abstractmethod
-  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited):
     pass
 
   def reset(self):

selfdrive/controls/lib/latcontrol_angle.py

@@ -13,7 +13,7 @@ def __init__(self, CP, CI):
     self.sat_check_min_speed = 5.
     self.use_steer_limited_by_safety = CP.brand == "tesla"
 
-  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if not active:

selfdrive/controls/lib/latcontrol_pid.py

@@ -13,7 +13,7 @@ def __init__(self, CP, CI):
                              k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
     self.get_steer_feedforward = CI.get_steer_feedforward_function()
 
-  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, raw_desired_curvature, curvature_limited):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)