rate limit in path angle not steering wheel angle

commaai · Jul 19, 2022 · 42e9333 · 42e9333
1 parent 5af4eff
commit 42e9333
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 9 deletions.
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
@@ -9,18 +9,21 @@
 
 
 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 = 4.52182 * math.radians(apply_steer) / VM.sR
+  apply_angle = math.radians(apply_angle) * 4.52182
   apply_angle = clip(apply_angle, -0.5, 0.5235)
-  apply_steer = math.degrees(apply_angle) * VM.sR / 4.52182
+  apply_angle = math.degrees(apply_angle) / 4.52182
 
-  return apply_steer
+  return apply_angle * VM.sR
 
 
 class CarController:
@@ -61,7 +64,7 @@ def update(self, CC, CS):
 
       # use LatCtlPath_An_Actl to actuate steering
       # path angle is the car wheel angle, not the steering wheel angle
-      path_angle = 4.52182 * math.radians(apply_steer) / self.VM.sR
+      path_angle = math.radians(apply_steer) * 4.52182 / self.VM.sR
 
       # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
       # TODO: slower ramp speed when driver torque detected

diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
@@ -19,8 +19,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: