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 · c11fcb9 · c11fcb9
1 parent 9eba294
commit c11fcb9
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
@@ -13,6 +13,7 @@ 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
+  rate_limit /= CarControllerParams.LKAS_STEER_STEP
   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))
 

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:
@@ -25,8 +25,8 @@ class CarControllerParams:
   STEER_RATIO = 2.75
   STEER_DRIVER_ALLOWANCE = 0.8  # Driver intervention threshold 0.8 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])
+  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: