refactor rate limits

Define rate limits as "angle per second" and apply them according to the LKAS_STEER_STEP (command frequency).
commaai · Sep 9, 2022 · 348d33b · 348d33b
1 parent 7498f86
commit 348d33b
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 4 deletions.
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
@@ -12,7 +12,8 @@ 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)
+  rate_limit /= CarControllerParams.LKAS_STEER_STEP
+  max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.angle_rate_points)
   apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))
 
   # absolute limit (LatCtlPath_An_Actl)

diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
@@ -11,7 +11,7 @@
 TransmissionType = car.CarParams.TransmissionType
 GearShifter = car.CarState.GearShifter
 
-AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
+AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'angle_rate_points'])
 
 
 class CarControllerParams:
@@ -26,8 +26,8 @@ class CarControllerParams:
   STEER_RATIO = 2.75  # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians
   STEER_DRIVER_ALLOWANCE = 0.8  # Driver intervention threshold in Newton-meters
 
-  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])
+  RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], angle_rate_points=[500., 80., 15.])
+  RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], angle_rate_points=[500., 350., 40.])
 
 
 class RADAR: