base LatControl class (#21967)

* base LatControl class, move sat check out of pid.py

clean up

clean up

* fix

* global variable for min control speed

* nicer name

* unify latcontrol class init arguments

* add to release files

* saturated if close to limit

* move angle mode saturation checks into class

* check_saturation function takes in current saturated status

undo

* apply latcontrol_angle's active checking to all controllers

* clean up

* move those back

* make abstract baseclass

* add test for saturation

* keep clip

* update ref

* fix static analysis

Co-authored-by: Willem Melching <willem.melching@gmail.com>

release/files_common

@@ -238,6 +238,7 @@ selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/latcontrol_indi.py
 selfdrive/controls/lib/latcontrol_lqr.py
 selfdrive/controls/lib/latcontrol_pid.py
+selfdrive/controls/lib/latcontrol.py
 selfdrive/controls/lib/lateral_planner.py
 selfdrive/controls/lib/longcontrol.py
 selfdrive/controls/lib/longitudinal_planner.py

selfdrive/car/ford/interface.py

@@ -21,7 +21,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]]     # TODO: tune this
     ret.lateralTuning.pid.kf = 1. / MAX_ANGLE   # MAX Steer angle to normalize FF
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
-    ret.steerLimitTimer = 0.8
+    ret.steerLimitTimer = 1.0
     ret.steerRateCost = 1.0
     ret.centerToFront = ret.wheelbase * 0.44
     tire_stiffness_factor = 0.5328

selfdrive/car/nissan/interface.py

@@ -16,7 +16,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret.carName = "nissan"
     ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
 
-    ret.steerLimitAlert = False
+    ret.steerLimitTimer = 1.0
     ret.steerRateCost = 0.5
 
     ret.steerActuatorDelay = 0.1

selfdrive/car/tesla/interface.py

@@ -40,6 +40,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
       ret.openpilotLongitudinalControl = False
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)]
 
+    ret.steerLimitTimer = 1.0
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.5
 

selfdrive/controls/controlsd.py

@@ -31,8 +31,6 @@
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
-STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
-STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -128,13 +126,13 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.VM = VehicleModel(self.CP)
 
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      self.LaC = LatControlAngle(self.CP)
+      self.LaC = LatControlAngle(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'pid':
       self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
-      self.LaC = LatControlINDI(self.CP)
+      self.LaC = LatControlINDI(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'lqr':
-      self.LaC = LatControlLQR(self.CP)
+      self.LaC = LatControlLQR(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled
@@ -148,7 +146,6 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.cruise_mismatch_counter = 0
     self.can_rcv_error_counter = 0
     self.last_blinker_frame = 0
-    self.saturated_count = 0
     self.distance_traveled = 0
     self.last_functional_fan_frame = 0
     self.events_prev = []
@@ -518,19 +515,8 @@ def state_control(self, CS):
         lac_log.output = steer
         lac_log.saturated = abs(steer) >= 0.9
 
-    # Check for difference between desired angle and angle for angle based control
-    angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
-      abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
-
-    if angle_control_saturated and not CS.steeringPressed and self.active:
-      self.saturated_count += 1
-    else:
-      self.saturated_count = 0
-
     # Send a "steering required alert" if saturation count has reached the limit
-    if (lac_log.saturated and not CS.steeringPressed) or \
-       (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
-
+    if lac_log.active and lac_log.saturated and not CS.steeringPressed:
       dpath_points = lat_plan.dPathPoints
       if len(dpath_points):
         # Check if we deviated from the path

selfdrive/controls/lib/latcontrol.py

@@ -0,0 +1,28 @@
+from abc import abstractmethod, ABC
+
+from common.realtime import DT_CTRL
+from common.numpy_fast import clip
+
+MIN_STEER_SPEED = 0.3
+
+
+class LatControl(ABC):
+  def __init__(self, CP, CI):
+    self.sat_count_rate = 1.0 * DT_CTRL
+    self.sat_limit = CP.steerLimitTimer
+    self.sat_count = 0.
+
+  @abstractmethod
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+    pass
+
+  def reset(self):
+    self.sat_count = 0.
+
+  def _check_saturation(self, saturated, CS):
+    if saturated and CS.vEgo > 10. and not CS.steeringRateLimited and not CS.steeringPressed:
+      self.sat_count += self.sat_count_rate
+    else:
+      self.sat_count -= self.sat_count_rate
+    self.sat_count = clip(self.sat_count, 0.0, 1.0)
+    return self.sat_count > self.sat_limit

selfdrive/controls/lib/latcontrol_angle.py

@@ -1,28 +1,25 @@
 import math
 
 from cereal import log
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 
+STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
-class LatControlAngle():
-  def __init__(self, CP):
-    pass
-
-  def reset(self):
-    pass
 
+class LatControlAngle(LatControl):
   def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_STEER_SPEED or not active:
       angle_log.active = False
       angle_steers_des = float(CS.steeringAngleDeg)
     else:
       angle_log.active = True
       angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
       angle_steers_des += params.angleOffsetDeg
 
-    angle_log.saturated = False
+    angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
+    angle_log.saturated = self._check_saturation(angle_control_saturated, CS)
     angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     angle_log.steeringAngleDesiredDeg = angle_steers_des
-
     return 0, float(angle_steers_des), angle_log

selfdrive/controls/lib/latcontrol_indi.py

@@ -8,10 +8,12 @@
 from selfdrive.car import apply_toyota_steer_torque_limits
 from selfdrive.car.toyota.values import CarControllerParams
 from selfdrive.controls.lib.drive_helpers import get_steer_max
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 
 
-class LatControlINDI():
-  def __init__(self, CP):
+class LatControlINDI(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
     self.angle_steers_des = 0.
 
     A = np.array([[1.0, DT_CTRL, 0.0],
@@ -42,8 +44,6 @@ def __init__(self, CP):
     self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV)
     self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV)
 
-    self.sat_count_rate = 1.0 * DT_CTRL
-    self.sat_limit = CP.steerLimitTimer
     self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
 
     self.reset()
@@ -65,24 +65,12 @@ def inner_loop_gain(self):
     return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
 
   def reset(self):
+    super().reset()
     self.steer_filter.x = 0.
     self.output_steer = 0.
-    self.sat_count = 0.
     self.speed = 0.
 
-  def _check_saturation(self, control, check_saturation, limit):
-    saturated = abs(control) == limit
-
-    if saturated and check_saturation:
-      self.sat_count += self.sat_count_rate
-    else:
-      self.sat_count -= self.sat_count_rate
-
-    self.sat_count = clip(self.sat_count, 0.0, 1.0)
-
-    return self.sat_count > self.sat_limit
-
-  def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     self.speed = CS.vEgo
     # Update Kalman filter
     y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
@@ -93,14 +81,14 @@ def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvatur
     indi_log.steeringRateDeg = math.degrees(self.x[1])
     indi_log.steeringAccelDeg = math.degrees(self.x[2])
 
-    steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
+    steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)
     steers_des += math.radians(params.angleOffsetDeg)
     indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
 
-    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
+    rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0)
     indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
 
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_STEER_SPEED or not active:
       indi_log.active = False
       self.output_steer = 0.0
       self.steer_filter.x = 0.0
@@ -142,8 +130,6 @@ def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvatur
       indi_log.delayedOutput = float(self.steer_filter.x)
       indi_log.delta = float(delta_u)
       indi_log.output = float(self.output_steer)
-
-      check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
-      indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
+      indi_log.saturated = self._check_saturation(steers_max - abs(self.output_steer) < 1e-3, CS)
 
     return float(self.output_steer), float(steers_des), indi_log

selfdrive/controls/lib/latcontrol_lqr.py

@@ -5,10 +5,12 @@
 from common.realtime import DT_CTRL
 from cereal import log
 from selfdrive.controls.lib.drive_helpers import get_steer_max
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 
 
-class LatControlLQR():
-  def __init__(self, CP):
+class LatControlLQR(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
     self.scale = CP.lateralTuning.lqr.scale
     self.ki = CP.lateralTuning.lqr.ki
 
@@ -23,26 +25,11 @@ def __init__(self, CP):
     self.i_unwind_rate = 0.3 * DT_CTRL
     self.i_rate = 1.0 * DT_CTRL
 
-    self.sat_count_rate = 1.0 * DT_CTRL
-    self.sat_limit = CP.steerLimitTimer
-
     self.reset()
 
   def reset(self):
+    super().reset()
     self.i_lqr = 0.0
-    self.sat_count = 0.0
-
-  def _check_saturation(self, control, check_saturation, limit):
-    saturated = abs(control) == limit
-
-    if saturated and check_saturation:
-      self.sat_count += self.sat_count_rate
-    else:
-      self.sat_count -= self.sat_count_rate
-
-    self.sat_count = clip(self.sat_count, 0.0, 1.0)
-
-    return self.sat_count > self.sat_limit
 
   def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     lqr_log = log.ControlsState.LateralLQRState.new_message()
@@ -64,7 +51,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
     e = steering_angle_no_offset - angle_steers_k
     self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
 
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_STEER_SPEED or not active:
       lqr_log.active = False
       lqr_output = 0.
       output_steer = 0.
@@ -91,12 +78,9 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
       output_steer = lqr_output + self.i_lqr
       output_steer = clip(output_steer, -steers_max, steers_max)
 
-    check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
-    saturated = self._check_saturation(output_steer, check_saturation, steers_max)
-
     lqr_log.steeringAngleDeg = angle_steers_k
     lqr_log.i = self.i_lqr
     lqr_log.output = output_steer
     lqr_log.lqrOutput = lqr_output
-    lqr_log.saturated = saturated
+    lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
     return output_steer, desired_angle, lqr_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -2,18 +2,20 @@
 
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 from cereal import log
 
 
-class LatControlPID():
+class LatControlPID(LatControl):
   def __init__(self, CP, CI):
+    super().__init__(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)
+                            k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0)
     self.get_steer_feedforward = CI.get_steer_feedforward_function()
 
   def reset(self):
+    super().reset()
     self.pid.reset()
 
   def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
@@ -26,7 +28,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
 
     pid_log.steeringAngleDesiredDeg = angle_steers_des
     pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_STEER_SPEED or not active:
       output_steer = 0.0
       pid_log.active = False
       self.pid.reset()
@@ -40,14 +42,13 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
 
       deadzone = 0.0
 
-      check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
-      output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
+      output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
                                      feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
       pid_log.active = True
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i
       pid_log.f = self.pid.f
       pid_log.output = output_steer
-      pid_log.saturated = bool(self.pid.saturated)
+      pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
 
     return output_steer, angle_steers_des, pid_log

selfdrive/controls/lib/longcontrol.py

@@ -44,8 +44,7 @@ def __init__(self, CP):
     self.long_control_state = LongCtrlState.off  # initialized to off
     self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
-                            rate=1/DT_CTRL,
-                            sat_limit=0.8)
+                            rate=1 / DT_CTRL)
     self.v_pid = 0.0
     self.last_output_accel = 0.0