Clean up PID controller ACCEL_MIN/ACCEL_MAX (#22148)

* use ISO 15622:2018 limits in PID controller

* allow more for nidec

* limit PID inputs

* CP is not needed

* add GM

* not used

* update ref

* fix honda bosch

selfdrive/car/gm/interface.py

@@ -2,7 +2,7 @@
 from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.car.gm.values import CAR, CruiseButtons, \
-                                    AccState
+                                    AccState, CarControllerParams
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -10,6 +10,10 @@ ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
 class CarInterface(CarInterfaceBase):
+  @staticmethod
+  def get_pid_accel_limits(CP, current_speed, cruise_speed):
+    params = CarControllerParams()
+    return params.ACCEL_MIN, params.ACCEL_MAX
 
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):

selfdrive/car/gm/values.py

@@ -25,10 +25,20 @@ class CarControllerParams():
     MAX_GAS = 3072              # Only a safety limit
     ZERO_GAS = 2048
     MAX_BRAKE = 350             # Should be around 3.5m/s^2, including regen
+
+    self.ACCEL_MAX = 2.0 # m/s^2
+
+    # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
+    # perform the closed loop control, and might need some
+    # to apply some more braking if we're on a downhill slope.
+    # Our controller should still keep the 2 second average above
+    # -3.5 m/s^2 as per planner limits
+    self.ACCEL_MIN = -4.0 # m/s^2
+
     self.MAX_ACC_REGEN = 1404  # ACC Regen braking is slightly less powerful than max regen paddle
-    self.GAS_LOOKUP_BP = [-1.0, 0., 2.0]
+    self.GAS_LOOKUP_BP = [-1.0, 0., self.ACCEL_MAX]
     self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
-    self.BRAKE_LOOKUP_BP = [-4., -1.0]
+    self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, -1.0]
     self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
 
 class CAR:

selfdrive/car/honda/interface.py

@@ -6,7 +6,7 @@ from common.params import Params
 from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 from selfdrive.car.honda.hondacan import disable_radar
 from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
-from selfdrive.car.interfaces import CarInterfaceBase, ACCEL_MAX, ACCEL_MIN
+from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.config import Conversions as CV
 
 
@@ -17,12 +17,15 @@ TransmissionType = car.CarParams.TransmissionType
 
 class CarInterface(CarInterfaceBase):
   @staticmethod
-  def get_pid_accel_limits(current_speed, cruise_speed):
-    # NIDECs don't allow acceleration near cruise_speed,
-    # so limit limits of pid to prevent windup
-    ACCEL_MAX_VALS = [ACCEL_MAX, 0.2]
-    ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
-    return ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
+  def get_pid_accel_limits(CP, current_speed, cruise_speed):
+    if CP.carFingerprint in HONDA_BOSCH:
+      return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
+    else:
+      # NIDECs don't allow acceleration near cruise_speed,
+      # so limit limits of pid to prevent windup
+      ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2]
+      ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
+      return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
 
   @staticmethod
   def calc_accel_override(a_ego, a_target, v_ego, v_target):
@@ -54,7 +57,7 @@ class CarInterface(CarInterfaceBase):
     # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
     # unless aTargetMax is very high and then we scale with it; this help in quicker restart
 
-    return float(max(max_accel, a_target / CarControllerParams.ACCEL_MAX)) * min(speedLimiter, accelLimiter)
+    return float(max(max_accel, a_target / CarControllerParams.NIDEC_ACCEL_MAX)) * min(speedLimiter, accelLimiter)
 
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value

selfdrive/car/honda/values.py

@@ -5,7 +5,16 @@ Ecu = car.CarParams.Ecu
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 class CarControllerParams():
-  ACCEL_MAX = 1.6
+  # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
+  # perform the closed loop control, and might need some
+  # to apply some more braking if we're on a downhill slope.
+  # Our controller should still keep the 2 second average above
+  # -3.5 m/s^2 as per planner limits
+  NIDEC_ACCEL_MIN = -4.0 # m/s^2
+  NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons
+
+  BOSCH_ACCEL_MIN = -3.5 # m/s^2
+  BOSCH_ACCEL_MAX = 2.0 # m/s^2
 
   def __init__(self, CP):
     self.BRAKE_MAX = 1024//4

selfdrive/car/interfaces.py

@@ -18,7 +18,7 @@ EventName = car.CarEvent.EventName
 #          model predictions above this speed can be unpredictable
 MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS  # 135 + 4 = 86 mph
 ACCEL_MAX = 2.0
-ACCEL_MIN = -4.0
+ACCEL_MIN = -3.5
 
 
 # generic car and radar interfaces
@@ -45,7 +45,7 @@ class CarInterfaceBase():
       self.CC = CarController(self.cp.dbc_name, CP, self.VM)
 
   @staticmethod
-  def get_pid_accel_limits(current_speed, cruise_speed):
+  def get_pid_accel_limits(CP, current_speed, cruise_speed):
     return ACCEL_MIN, ACCEL_MAX
 
   @staticmethod

selfdrive/car/toyota/interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP
+from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -9,6 +9,10 @@ EventName = car.CarEvent.EventName
 
 
 class CarInterface(CarInterfaceBase):
+  @staticmethod
+  def get_pid_accel_limits(CP, current_speed, cruise_speed):
+    return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
+
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

selfdrive/controls/controlsd.py

@@ -460,7 +460,7 @@ class Controls:
 
     if not self.joystick_mode:
       # accel PID loop
-      pid_accel_limits = self.CI.get_pid_accel_limits(CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
+      pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
       actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits)
 
       # Steering PID loop and lateral MPC

selfdrive/controls/lib/longcontrol.py

@@ -16,6 +16,10 @@ DECEL_STOPPING_TARGET = 2.0  # apply at least this amount of brake to maintain t
 RATE = 100.0
 DEFAULT_LONG_LAG = 0.15
 
+# As per ISO 15622:2018 for all speeds
+ACCEL_MIN_ISO = -3.5 # m/s^2
+ACCEL_MAX_ISO = 2.0 # m/s^2
+
 
 # TODO this logic isn't really car independent, does not belong here
 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
@@ -82,6 +86,9 @@ class LongControl():
       v_target_future = 0.0
       a_target = 0.0
 
+    # TODO: This check is not complete and needs to be enforced by MPC
+    a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO)
+
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-858ebd51d52e76edf61190014337df299a7e6374
+bc50c3f64b4677eee32bc49bdf69c3be265fd743