Add latcontrol for curvature (#38141)

* undeprecate curvature

* use pid_log

* bump

* fix test

* bump

* bump

* fix

* tune saturation

* bump

* fix torque bar

* bump

* rm redundant

* reduce sesitivity

* pid

* bump

* reset opendbc

* preserve sorting

* bump opendbc

cereal/deprecated.capnp

@@ -760,18 +760,6 @@ struct LateralLQRState @0x9024e2d790c82ade {
   steeringAngleDesiredDeg @6 :Float32;
 }
 
-struct LateralCurvatureState @0xad9d8095c06f7c61 {
-  active @0 :Bool;
-  actualCurvature @1 :Float32;
-  desiredCurvature @2 :Float32;
-  error @3 :Float32;
-  p @4 :Float32;
-  i @5 :Float32;
-  f @6 :Float32;
-  output @7 :Float32;
-  saturated @8 :Bool;
-}
-
 struct LateralPlannerSolution @0x84caeca5a6b4acfe {
   x @0 :List(Float32);
   y @1 :List(Float32);

cereal/log.capnp

@@ -818,7 +818,7 @@ struct ControlsState @0x97ff69c53601abf1 {
     debugState @59 :LateralDebugState;
     torqueState @60 :LateralTorqueState;
 
-    curvatureStateDEPRECATED @65 :Deprecated.LateralCurvatureState;
+    curvatureState @65 :LateralCurvatureState;
     lqrStateDEPRECATED @55 :Deprecated.LateralLQRState;
     indiStateDEPRECATED @52 :Deprecated.LateralINDIState;
   }
@@ -867,6 +867,18 @@ struct ControlsState @0x97ff69c53601abf1 {
     saturated @3 :Bool;
   }
 
+  struct LateralCurvatureState @0xad9d8095c06f7c61 {
+    active @0 :Bool;
+    actualCurvature @1 :Float32;
+    desiredCurvature @2 :Float32;
+    error @3 :Float32;
+    p @4 :Float32;
+    i @5 :Float32;
+    f @6 :Float32;
+    output @7 :Float32;
+    saturated @8 :Bool;
+  }
+
   deprecated :group {
     vEgo @0 :Float32;
     vEgoRaw @32 :Float32;

selfdrive/car/tests/test_models.py

@@ -186,7 +186,7 @@ class TestCarModelBase(unittest.TestCase):
     # make sure car params are within a valid range
     self.assertGreater(self.CP.mass, 1)
 
-    if self.CP.steerControlType != SteerControlType.angle:
+    if self.CP.steerControlType not in (SteerControlType.angle, SteerControlType.curvature):
       tuning = self.CP.lateralTuning.which()
       if tuning == 'pid':
         self.assertTrue(len(self.CP.lateralTuning.pid.kpV))

selfdrive/controls/controlsd.py

@@ -15,6 +15,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
@@ -53,6 +54,8 @@ class Controls:
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL)
+    elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
+      self.LaC = LatControlCurvature(self.CP, self.CI, DT_CTRL)
     elif self.CP.lateralTuning.which() == 'pid':
       self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
     elif self.CP.lateralTuning.which() == 'torque':
@@ -124,11 +127,14 @@ class Controls:
     lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
 
     actuators.curvature = self.desired_curvature
-    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
-                                                       self.steer_limited_by_safety, self.desired_curvature,
-                                                       curvature_limited, lat_delay)
+    steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+                                                     self.steer_limited_by_safety, self.desired_curvature,
+                                                     curvature_limited, lat_delay)
     actuators.torque = float(steer)
-    actuators.steeringAngleDeg = float(steeringAngleDeg)
+    if self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
+      actuators.curvature = float(lateral_output)
+    else:
+      actuators.steeringAngleDeg = float(lateral_output)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)
@@ -199,6 +205,8 @@ class Controls:
     lat_tuning = self.CP.lateralTuning.which()
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       cs.lateralControlState.angleState = lac_log
+    elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
+      cs.lateralControlState.curvatureState = lac_log
     elif lat_tuning == 'pid':
       cs.lateralControlState.pidState = lac_log
     elif lat_tuning == 'torque':

selfdrive/controls/lib/latcontrol_curvature.py

@@ -0,0 +1,58 @@
+import math
+
+from cereal import log
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
+
+CURVATURE_SATURATION_THRESHOLD = 1e-3  # 1/m
+
+
+class LatControlCurvature(LatControl):
+  def __init__(self, CP, CI, dt):
+    super().__init__(CP, CI, dt)
+    self.sat_check_min_speed = 5.
+    if CP.lateralTuning.which() == 'pid':
+      ct = CP.lateralTuning.pid
+      self.pid = PIDController((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV),
+                               pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt)
+      self.kf = ct.kf
+    else:
+      self.pid = None
+      self.kf = 1.
+
+  def reset(self):
+    super().reset()
+    if self.pid is not None:
+      self.pid.reset()
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
+    curvature_log = log.ControlsState.LateralCurvatureState.new_message()
+    actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    error = desired_curvature - actual_curvature
+
+    if not active:
+      output_curvature = 0.0
+      curvature_log.active = False
+      if self.pid is not None:
+        self.pid.reset()
+    elif self.pid is None or CS.steeringPressed:
+      # no PID or override: feedforward only
+      if self.pid is not None:
+        self.pid.reset()
+      output_curvature = self.kf * desired_curvature
+      curvature_log.active = True
+    else:
+      output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=self.kf * desired_curvature)
+      curvature_log.p = float(self.pid.p)
+      curvature_log.i = float(self.pid.i)
+      curvature_log.f = float(self.pid.f)
+      curvature_log.active = True
+
+    curvature_log.error = float(error)
+    curvature_log.actualCurvature = float(actual_curvature)
+    curvature_log.desiredCurvature = float(desired_curvature)
+    curvature_log.output = float(output_curvature)
+    curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS,
+                                                          False, curvature_limited))
+    return 0.0, float(output_curvature), curvature_log

selfdrive/ui/mici/onroad/torque_bar.py

@@ -163,7 +163,7 @@ class TorqueBar(Widget):
       return
 
     # torque line
-    if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState':
+    if ui_state.sm['controlsState'].lateralControlState.which() in ('angleState', 'curvatureState'):
       controls_state = ui_state.sm['controlsState']
       car_state = ui_state.sm['carState']
       live_parameters = ui_state.sm['liveParameters']