Ford: use curvature signal for lateral control (#27091)

* do carcontroller

Co-authored-by: Cameron Clough <cameronjclough@gmail.com>

* values done

* just send latActive in, cleaner

* Update selfdrive/car/ford/values.py

* fix a crash

* adjust ramp as well

Co-authored-by: Cameron Clough <cameronjclough@gmail.com>

selfdrive/car/ford/carcontroller.py

@@ -1,28 +1,14 @@
 import math
 from cereal import car
-from common.numpy_fast import clip, interp
+from common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
+from selfdrive.car import apply_std_steer_angle_limits
 from selfdrive.car.ford.fordcan import create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg
 from selfdrive.car.ford.values import CANBUS, CarControllerParams
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 
-def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo):
-  # rate limit
-  steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last)
-  rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN
-  max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
-  apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))
-
-  # absolute limit (LatCtlPath_An_Actl)
-  apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
-  apply_path_angle = clip(apply_path_angle, -0.5, 0.5235)
-  apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO
-
-  return apply_angle
-
-
 class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
@@ -30,7 +16,7 @@ class CarController:
     self.packer = CANPacker(dbc_name)
     self.frame = 0
 
-    self.apply_angle_last = 0
+    self.apply_curvature_last = 0
     self.main_on_last = False
     self.lkas_enabled_last = False
     self.steer_alert_last = False
@@ -57,34 +43,33 @@ class CarController:
       can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True))
 
     ### lateral control ###
-    if CC.latActive:
-      lca_rq = 1
-      apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo)
-    else:
-      lca_rq = 0
-      apply_angle = 0.
-
     # send steering commands at 20Hz
     if (self.frame % CarControllerParams.STEER_STEP) == 0:
-      # use LatCtlPath_An_Actl to actuate steering
-      path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
+      if CC.latActive:
+        # apply limits to curvature
+        apply_curvature = -self.VM.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
+        apply_curvature = apply_std_steer_angle_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgo, CarControllerParams)
+        # clip to signal range
+        apply_curvature = clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
+      else:
+        apply_curvature = 0.
 
       # set slower ramp type when small steering angle change
       # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
       steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg)
-      if steer_change < 2.0:
+      if steer_change < 2.5:
         ramp_type = 0
-      elif steer_change < 4.0:
+      elif steer_change < 5.0:
         ramp_type = 1
-      elif steer_change < 6.0:
+      elif steer_change < 7.5:
         ramp_type = 2
       else:
         ramp_type = 3
       precision = 1  # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
 
-      self.apply_angle_last = apply_angle
+      self.apply_curvature_last = apply_curvature
       can_sends.append(create_lka_msg(self.packer))
-      can_sends.append(create_lat_ctl_msg(self.packer, lca_rq, ramp_type, precision, 0, path_angle, 0, 0))
+      can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, ramp_type, precision, 0., 0., -apply_curvature, 0.))
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
@@ -102,7 +87,7 @@ class CarController:
     self.steer_alert_last = steer_alert
 
     new_actuators = actuators.copy()
-    new_actuators.steeringAngleDeg = self.apply_angle_last
+    new_actuators.curvature = self.apply_curvature_last
 
     self.frame += 1
     return new_actuators, can_sends

selfdrive/car/ford/fordcan.py

@@ -16,7 +16,7 @@ def create_lka_msg(packer):
   return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {})
 
 
-def create_lat_ctl_msg(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float,
+def create_lat_ctl_msg(packer, lat_active: bool, ramp_type: int, precision: int, path_offset: float, path_angle: float,
                        curvature: float, curvature_rate: float):
   """
   Creates a CAN message for the Ford TJA/LCA Command.
@@ -42,7 +42,7 @@ def create_lat_ctl_msg(packer, lca_rq: int, ramp_type: int, precision: int, path
   values = {
     "LatCtlRng_L_Max": 0,                       # Unknown [0|126] meter
     "HandsOffCnfm_B_Rq": 0,                     # Unknown: 0=Inactive, 1=Active [0|1]
-    "LatCtl_D_Rq": lca_rq,                      # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
+    "LatCtl_D_Rq": 1 if lat_active else 0,      # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
                                                 #       3=InterventionRight, 4-7=NotUsed [0|7]
     "LatCtlRampType_D_Rq": ramp_type,           # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
     "LatCtlPrecision_D_Rq": precision,          # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]

selfdrive/car/ford/values.py

@@ -9,7 +9,7 @@ from selfdrive.car.docs_definitions import CarInfo, Harness
 from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
-AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
+CurvatureLimit = namedtuple('CurvatureLimit', ['speed_points', 'max_angle_diff_points'])
 
 
 class CarControllerParams:
@@ -22,12 +22,13 @@ class CarControllerParams:
   # Message: Steering_Data_FD1, but send twice as fast
   BUTTONS_STEP = 10 / 2
 
-  LKAS_STEER_RATIO = 2.75       # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians
-                                # TODO: remove this once we understand how the EPS calculates the steering angle better
-  STEER_DRIVER_ALLOWANCE = 0.8  # Driver intervention threshold in Nm
+  CURVATURE_MAX = 0.02  # Max curvature for steering command, m^-1
+  STEER_DRIVER_ALLOWANCE = 0.8  # Driver intervention threshold, Nm
 
-  RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
-  RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
+  # Curvature rate limits
+  # TODO: unify field names used by curvature and angle control cars
+  ANGLE_RATE_LIMIT_UP = CurvatureLimit(speed_points=[5, 15, 25], max_angle_diff_points=[0.005, 0.00056, 0.0002])
+  ANGLE_RATE_LIMIT_DOWN = CurvatureLimit(speed_points=[5, 15, 25], max_angle_diff_points=[0.008, 0.00089, 0.00032])
 
   def __init__(self, CP):
     pass