toyota angle rate limits

panda

@@ -1 +1 @@
-Subproject commit 8f946ce3576a23e57bf3e8933e92d8f32f6acd8e
+Subproject commit 5792caca07df2819b82fdadf043afda7b98e6c60

selfdrive/car/toyota/carcontroller.py

@@ -77,7 +77,12 @@ class CarController:
     percentage = interp(abs(CS.out.steeringTorque), [40, 100], [100, 0])
     apply_angle = interp(percentage, [-10, 100], [torque_sensor_angle, apply_angle])
 
-    apply_angle = clip(apply_angle, self.last_angle - self.params.ANGLE_RATE_MAX, self.last_angle + self.params.ANGLE_RATE_MAX)
+    # Angular rate limit based on speed (from TESLA)
+    steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle)
+    rate_limit = self.params.ANGLE_RATE_LIMIT_UP if steer_up else self.params.ANGLE_RATE_LIMIT_DOWN
+    max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
+    apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff)
+
     # apply_steer = clip(apply_steer,
     #                    torque_sensor_angle - self.params.ANGLE_DELTA_MAX,
     #                    torque_sensor_angle + self.params.ANGLE_DELTA_MAX)

selfdrive/car/toyota/values.py

@@ -1,4 +1,4 @@
-from collections import defaultdict
+from collections import defaultdict, namedtuple
 from dataclasses import dataclass
 from enum import Enum, IntFlag
 from typing import Dict, List, Union
@@ -12,6 +12,7 @@ from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQuerie
 Ecu = car.CarParams.Ecu
 MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
 PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
+AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
 
 
 class CarControllerParams:
@@ -23,7 +24,11 @@ class CarControllerParams:
 
   # stock LTA is 0 to 0.05 going straight
   # and 0.1 to 0.4 when turning (max seen is 0.6303)
-  ANGLE_RATE_MAX = 2.0
+
+  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
+  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
+
+  # ANGLE_RATE_MAX = 2.0
   # needs to be within +-3 degrees of current angle to avoid windup
   ANGLE_DELTA_MAX = 3.0