Misc PID refactors (#35844)

* Misc PID refactors

* dead

* finish rename

* unused import

* whitespace

* typo

* fix fan controller

* pid_log

* whitespace

* integral clipping in pid

* update ref

* cleaner

* rm print

* update ref

* revert fan changes

* forgot this

common/pid.py

@@ -17,7 +17,6 @@ class PIDController:
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
 
-    self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
     self.speed = 0.0
 
@@ -35,10 +34,6 @@ class PIDController:
   def k_d(self):
     return np.interp(self.speed, self._k_d[0], self._k_d[1])
 
-  @property
-  def error_integral(self):
-    return self.i/self.k_i
-
   def reset(self):
     self.p = 0.0
     self.i = 0.0
@@ -46,25 +41,21 @@ class PIDController:
     self.f = 0.0
     self.control = 0
 
-  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
+  def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
     self.speed = speed
-
     self.p = float(error) * self.k_p
     self.f = feedforward * self.k_f
     self.d = error_rate * self.k_d
 
-    if override:
-      self.i -= self.i_unwind_rate * float(np.sign(self.i))
-    else:
-      if not freeze_integrator:
-        self.i = self.i + error * self.k_i * self.i_rate
+    if not freeze_integrator:
+      i = self.i + error * self.k_i * self.i_rate
 
-        # Clip i to prevent exceeding control limits
-        control_no_i = self.p + self.d + self.f
-        control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
-        self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
+      # Don't allow windup if already clipping
+      test_control = self.p + i + self.d + self.f
+      i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit
+      i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit
+      self.i = np.clip(i, i_lowerbound, i_upperbound)
 
     control = self.p + self.i + self.d + self.f
-
     self.control = np.clip(control, self.neg_limit, self.pos_limit)
     return self.control

selfdrive/controls/controlsd.py

@@ -40,7 +40,7 @@ class Controls:
                                    'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'])
 
-    self.steer_limited_by_controls = False
+    self.steer_limited_by_safety = False
     self.curvature = 0.0
     self.desired_curvature = 0.0
 
@@ -120,7 +120,7 @@ class Controls:
 
     actuators.curvature = self.desired_curvature
     steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
-                                                       self.steer_limited_by_controls, self.desired_curvature,
+                                                       self.steer_limited_by_safety, self.desired_curvature,
                                                        curvature_limited)  # TODO what if not available
     actuators.torque = float(steer)
     actuators.steeringAngleDeg = float(steeringAngleDeg)
@@ -167,10 +167,10 @@ class Controls:
     if self.sm['selfdriveState'].active:
       CO = self.sm['carOutput']
       if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-        self.steer_limited_by_controls = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
+        self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
                                               STEER_ANGLE_SATURATION_THRESHOLD
       else:
-        self.steer_limited_by_controls = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
+        self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
 
     # TODO: both controlsState and carControl valids should be set by
     #       sm.all_checks(), but this creates a circular dependency

selfdrive/controls/lib/latcontrol.py

@@ -15,15 +15,15 @@ class LatControl(ABC):
     self.steer_max = 1.0
 
   @abstractmethod
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited):
     pass
 
   def reset(self):
     self.sat_count = 0.
 
-  def _check_saturation(self, saturated, CS, steer_limited_by_controls, curvature_limited):
+  def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
     # Saturated only if control output is not being limited by car torque/angle rate limits
-    if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed:
+    if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
       self.sat_count += self.sat_count_rate
     else:
       self.sat_count -= self.sat_count_rate

selfdrive/controls/lib/latcontrol_angle.py

@@ -3,6 +3,7 @@ import math
 from cereal import log
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 
+# TODO This is speed dependent
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
 
@@ -10,9 +11,9 @@ class LatControlAngle(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
     self.sat_check_min_speed = 5.
-    self.use_steer_limited_by_controls = CP.brand == "tesla"
+    self.use_steer_limited_by_safety = CP.brand == "tesla"
 
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if not active:
@@ -23,9 +24,9 @@ class LatControlAngle(LatControl):
       angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
       angle_steers_des += params.angleOffsetDeg
 
-    if self.use_steer_limited_by_controls:
+    if self.use_steer_limited_by_safety:
       # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
-      angle_control_saturated = steer_limited_by_controls
+      angle_control_saturated = steer_limited_by_safety
     else:
       # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
       # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque  # TODO: this can be improved

selfdrive/controls/lib/latcontrol_pid.py

@@ -13,11 +13,7 @@ class LatControlPID(LatControl):
                              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 reset(self):
-    super().reset()
-    self.pid.reset()
-
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)
@@ -29,20 +25,24 @@ class LatControlPID(LatControl):
     pid_log.steeringAngleDesiredDeg = angle_steers_des
     pid_log.angleError = error
     if not active:
-      output_steer = 0.0
+      output_torque = 0.0
       pid_log.active = False
-      self.pid.reset()
+
     else:
       # offset does not contribute to resistive torque
-      steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+      ff = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+
+      output_torque = self.pid.update(error,
+                                feedforward=ff,
+                                speed=CS.vEgo,
+                                freeze_integrator=freeze_integrator)
 
-      output_steer = self.pid.update(error, override=CS.steeringPressed,
-                                     feedforward=steer_feedforward, speed=CS.vEgo)
       pid_log.active = True
       pid_log.p = float(self.pid.p)
       pid_log.i = float(self.pid.i)
       pid_log.f = float(self.pid.f)
-      pid_log.output = float(output_steer)
-      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls, curvature_limited))
+      pid_log.output = float(output_torque)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
 
-    return output_steer, angle_steers_des, pid_log
+    return output_torque, angle_steers_des, pid_log

selfdrive/controls/lib/latcontrol_torque.py

@@ -37,7 +37,7 @@ class LatControlTorque(LatControl):
     self.torque_params.latAccelOffset = latAccelOffset
     self.torque_params.friction = friction
 
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     if not active:
       output_torque = 0.0
@@ -66,7 +66,7 @@ class LatControlTorque(LatControl):
                                           gravity_adjusted=True)
       ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
-      freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
       output_torque = self.pid.update(pid_log.error,
                                       feedforward=ff,
                                       speed=CS.vEgo,
@@ -80,7 +80,7 @@ class LatControlTorque(LatControl):
       pid_log.output = float(-output_torque)
       pid_log.actualLateralAccel = float(actual_lateral_accel)
       pid_log.desiredLateralAccel = float(desired_lateral_accel)
-      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls, curvature_limited))
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
 
     # TODO left is positive in this convention
     return -output_torque, 0.0, pid_log

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-62b3e8590fe85f87494517b3177a0ccaad1e17e5
+543bd2347fa35f8300478a3893fdd0a03a7c1fe6

tools/tuning/measure_steering_accuracy.py

@@ -49,7 +49,7 @@ class SteeringAccuracyTool:
     active = sm['controlsState'].active
     steer = sm['carOutput'].actuatorsOutput.torque
     standstill = sm['carState'].standstill
-    steer_limited_by_controls = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2
+    steer_limited_by_safety = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2
     overriding = sm['carState'].steeringPressed
     changing_lanes = sm['modelV2'].meta.laneChangeState != 0
     model_points = sm['modelV2'].position.y
@@ -77,7 +77,7 @@ class SteeringAccuracyTool:
             self.speed_group_stats[group][angle_abs]["steer"] += abs(steer)
             if len(model_points):
               self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0])
-            if steer_limited_by_controls:
+            if steer_limited_by_safety:
               self.speed_group_stats[group][angle_abs]["limited"] += 1
             if control_state.saturated:
               self.speed_group_stats[group][angle_abs]["saturated"] += 1