Car controls: common angle limiting function (#26732)

* common angle limiting function

* save a line

selfdrive/car/__init__.py

@@ -1,8 +1,9 @@
 # functions common among cars
 import capnp
+from collections import namedtuple
 
 from cereal import car
-from common.numpy_fast import clip
+from common.numpy_fast import clip, interp
 from typing import Dict
 
 # kg of standard extra cargo to count for drive, gas, etc...
@@ -10,6 +11,7 @@ STD_CARGO_KG = 136.
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
+AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v'])
 
 
 def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
@@ -111,6 +113,15 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq
   return int(round(float(apply_torque)))
 
 
+def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS):
+  # pick angle rate limits based on wind up/down
+  steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last)
+  rate_limits = LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else LIMITS.ANGLE_RATE_LIMIT_DOWN
+
+  angle_rate_lim = interp(v_ego, rate_limits.speed_bp, rate_limits.angle_v)
+  return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim)
+
+
 def crc8_pedal(data):
   crc = 0xFF    # standard init value
   poly = 0xD5   # standard crc8: x8+x7+x6+x4+x2+1

selfdrive/car/nissan/carcontroller.py

@@ -1,6 +1,6 @@
 from cereal import car
-from common.numpy_fast import clip, interp
 from opendbc.can.packer import CANPacker
+from selfdrive.car import apply_std_steer_angle_limits
 from selfdrive.car.nissan import nissancan
 from selfdrive.car.nissan.values import CAR, CarControllerParams
 
@@ -14,7 +14,7 @@ class CarController:
     self.frame = 0
 
     self.lkas_max_torque = 0
-    self.last_angle = 0
+    self.apply_angle_last = 0
 
     self.packer = CANPacker(dbc_name)
 
@@ -28,18 +28,11 @@ class CarController:
     ### STEER ###
     lkas_hud_msg = CS.lkas_hud_msg
     lkas_hud_info_msg = CS.lkas_hud_info_msg
-    apply_angle = actuators.steeringAngleDeg
-
     steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
 
     if CC.latActive:
       # windup slower
-      if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
+      apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams)
-        angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
-      else:
-        angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU)
-
-      apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
 
       # Max torque from driver before EPS will give up and not apply torque
       if not bool(CS.out.steeringPressed):
@@ -57,7 +50,7 @@ class CarController:
       apply_angle = CS.out.steeringAngleDeg
       self.lkas_max_torque = 0
 
-    self.last_angle = apply_angle
+    self.apply_angle_last = apply_angle
 
     if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd:
       can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg))

selfdrive/car/nissan/values.py

@@ -4,7 +4,7 @@ from enum import Enum
 
 from cereal import car
 from panda.python import uds
-from selfdrive.car import dbc_dict
+from selfdrive.car import AngleRateLimit, dbc_dict
 from selfdrive.car.docs_definitions import CarInfo, Harness
 from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
@@ -12,9 +12,8 @@ Ecu = car.CarParams.Ecu
 
 
 class CarControllerParams:
-  ANGLE_DELTA_BP = [0., 5., 15.]
+  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
-  ANGLE_DELTA_V = [5., .8, .15]     # windup limit
+  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
-  ANGLE_DELTA_VU = [5., 3.5, 0.4]   # unwind limit
   LKAS_MAX_TORQUE = 1               # A value of 1 is easy to overpower
   STEER_THRESHOLD = 1.0
 

selfdrive/car/tesla/carcontroller.py

@@ -1,5 +1,6 @@
-from common.numpy_fast import clip, interp
+from common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
+from selfdrive.car import apply_std_steer_angle_limits
 from selfdrive.car.tesla.teslacan import TeslaCAN
 from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
 
@@ -8,7 +9,7 @@ class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.frame = 0
-    self.last_angle = 0
+    self.apply_angle_last = 0
     self.packer = CANPacker(dbc_name)
     self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
     self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
@@ -24,20 +25,15 @@ class CarController:
     lkas_enabled = CC.latActive and not hands_on_fault
 
     if lkas_enabled:
-      apply_angle = actuators.steeringAngleDeg
-
       # Angular rate limit based on speed
-      steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle)
+      apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams)
-      rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.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)
 
       # To not fault the EPS
       apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20)
     else:
       apply_angle = CS.out.steeringAngleDeg
 
-    self.last_angle = apply_angle
+    self.apply_angle_last = apply_angle
     can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame))
 
     # Longitudinal control (in sync with stock message, about 40Hz)

selfdrive/car/tesla/values.py

@@ -2,14 +2,13 @@ from collections import namedtuple
 from typing import Dict, List, Union
 
 from cereal import car
-from selfdrive.car import dbc_dict
+from selfdrive.car import AngleRateLimit, dbc_dict
 from selfdrive.car.docs_definitions import CarInfo
 from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 
 Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
-AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
 
 
 class CAR:
@@ -104,8 +103,8 @@ BUTTONS = [
 ]
 
 class CarControllerParams:
-  RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
+  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
-  RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
+  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
   JERK_LIMIT_MAX = 8
   JERK_LIMIT_MIN = -8
   ACCEL_TO_SPEED_MULTIPLIER = 3