Ford: calculate wheel angle

Author: incognitojam

selfdrive/car/ford/carcontroller.py

@@ -1,3 +1,4 @@
+import math
 from cereal import car
 from common.numpy_fast import clip, interp
 from opendbc.can.packer import CANPacker
@@ -7,14 +8,22 @@
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 
-def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo):
+def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo, VM):
+  apply_angle = apply_steer / VM.sR
+  apply_angle_last = apply_steer_last / VM.sR
+
   # rate limit
-  steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last)
-  rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN
+  steer_up = apply_angle * apply_angle_last > 0. and abs(apply_angle) > abs(apply_angle_last)
+  rate_limit = CarControllerParams.ANGLE_RATE_LIMIT_UP if steer_up else CarControllerParams.ANGLE_RATE_LIMIT_DOWN
   max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
-  apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff))
+  apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))
+
+  # absolute limit (0.5 rad after steer ratio)
+  apply_angle = math.radians(apply_angle) * 4.1
+  apply_angle = clip(apply_angle, -0.5, 0.5235)
+  apply_angle = math.degrees(apply_angle) / 4.1
 
-  return apply_steer
+  return apply_angle * VM.sR
 
 
 class CarController:
@@ -42,32 +51,31 @@ def update(self, CC, CS):
       # cancel stock ACC
       can_sends.append(fordcan.spam_cancel_button(self.packer))
 
-    # apply rate limits
+
+    ### lateral control ###
     new_steer = actuators.steeringAngleDeg
-    apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
+    apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo, self.VM)
 
     # send steering commands at 20Hz
     if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
       lca_rq = 1 if CC.latActive else 0
 
-      # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
-      path_angle = apply_steer
+      # use LatCtlPath_An_Actl to actuate steering
+      # path angle is the car wheel angle, not the steering wheel angle
+      path_angle = math.radians(apply_steer) * 4.1 / self.VM.sR
 
-      # convert steer angle to curvature
-      curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0)
-
-      # TODO: get other actuators
-      curvature_rate = 0
-      path_offset = 0
-
-      ramp_type = 3  # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
-      precision = 0  # 0=Comfortable, 1=Precise
+      # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
+      # TODO: slower ramp speed when driver torque detected
+      ramp_type = 2
+      precision = 1  # 0=Comfortable, 1=Precise
 
       self.apply_steer_last = apply_steer
-      can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature))
+      can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, 0))
       can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
-                                                  path_offset, path_angle, curvature_rate, curvature))
+                                                  0, path_angle, 0, 0))
+
 
+    ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
 
     # send lkas ui command at 1Hz or if ui state changes

selfdrive/car/ford/fordcan.py

@@ -1,6 +1,3 @@
-from common.numpy_fast import clip
-
-
 def create_lkas_command(packer, angle_deg: float, curvature: float):
   """
   Creates a CAN message for the Ford LKAS Command.
@@ -43,10 +40,10 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
     "LatCtl_D_Rq": lca_rq,                                                  # 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]
-    "LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11),               # Path offset [-5.12|5.11] meter
-    "LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235),                   # Path angle [-0.5|0.5235] radians
-    "LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375),  # Curvature rate [-0.001024|0.00102375] 1/meter^2
-    "LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094),                  # Curvature [-0.02|0.02094] 1/meter
+    "LatCtlPathOffst_L_Actl": path_offset,                                  # Path offset [-5.12|5.11] meter
+    "LatCtlPath_An_Actl": path_angle,                                       # Path angle [-0.5|0.5235] radians
+    "LatCtlCurv_NoRate_Actl": curvature_rate,                               # Curvature rate [-0.001024|0.00102375] 1/meter^2
+    "LatCtlCurv_No_Actl": curvature,                                        # Curvature [-0.02|0.02094] 1/meter
   }
   return packer.make_can_msg("LateralMotionControl", 0, values)
 

selfdrive/car/ford/interface.py

@@ -19,9 +19,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
     ret.dashcamOnly = True
 
     # Angle-based steering
-    # TODO: use curvature control when ready
     ret.steerControlType = car.CarParams.SteerControlType.angle
-    ret.steerActuatorDelay = 0.1
+    ret.steerActuatorDelay = 0.15
     ret.steerLimitTimer = 1.0
 
     # TODO: detect stop-and-go vehicles

selfdrive/car/ford/values.py

@@ -20,8 +20,8 @@ class CarControllerParams:
   # Message: ACCDATA_3
   ACC_UI_STEP = 5
 
-  STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
-  STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
+  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[1.66, 0.26, 0.05])
+  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[1.66, 1.16, 0.133])
 
 
 class CANBUS: