Prevent PID windup on Honda NIDEC (#22112)

* add higher set speed

* freeze when close to cruise

* dont bias this

* higher set speed

* start high

* 2mph might be needed

* better condition

* limit accel windup like this

* wrong name
old-commit-hash: ebf2a2279d

selfdrive/car/honda/carcontroller.py

@@ -183,17 +183,19 @@ class CarController():
 
     # wind brake from air resistance decel at high speed
     wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
-
+    # all of this is only relevant for HONDA NIDEC
-    # All of this is only relevant for HONDA NIDEC
     max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V)
     # TODO this 1.44 is just to maintain previous behavior
     pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6)
     pcm_speed_BP = [-wind_brake,
                     -wind_brake*(3/4),
-                      0.0]
+                      0.0,
+                      0.1]
+
     pcm_speed_V = [0.0,
                    clip(CS.out.vEgo + accel/2.0 - 2.0, 0.0, 100.0),
-                   clip(CS.out.vEgo + accel/2.0 + 2.0, 0.0, 100.0)]
+                   clip(CS.out.vEgo + accel/2.0 + 2.0, 0.0, 100.0),
+                   clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
     pcm_speed = interp(-brake, pcm_speed_BP, pcm_speed_V)
 
     if not CS.CP.openpilotLongitudinalControl:

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
+from selfdrive.car.interfaces import CarInterfaceBase, ACCEL_MAX, ACCEL_MIN
 from selfdrive.config import Conversions as CV
 
 
@@ -16,6 +16,14 @@ 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)
+
   @staticmethod
   def calc_accel_override(a_ego, a_target, v_ego, v_target):
 

selfdrive/car/interfaces.py

@@ -17,6 +17,9 @@ EventName = car.CarEvent.EventName
 # WARNING: this value was determined based on the model's training distribution,
 #          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
+
 
 # generic car and radar interfaces
 
@@ -41,6 +44,10 @@ class CarInterfaceBase():
     if CarController is not None:
       self.CC = CarController(self.cp.dbc_name, CP, self.VM)
 
+  @staticmethod
+  def get_pid_accel_limits(current_speed, cruise_speed):
+    return ACCEL_MIN, ACCEL_MAX
+
   @staticmethod
   def calc_accel_override(a_ego, a_target, v_ego, v_target):
     return 1.

selfdrive/controls/controlsd.py

@@ -460,7 +460,8 @@ class Controls:
 
     if not self.joystick_mode:
       # accel PID loop
-      actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan)
+      pid_accel_limits = self.CI.get_pid_accel_limits(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
       desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,

selfdrive/controls/lib/longcontrol.py

@@ -6,8 +6,6 @@ from selfdrive.modeld.constants import T_IDXS
 
 LongCtrlState = log.ControlsState.LongControlState
 
-ACCEL_MAX = 2.0
-ACCEL_MIN = -4.0
 STOPPING_EGO_SPEED = 0.5
 STOPPING_TARGET_SPEED_OFFSET = 0.01
 STARTING_TARGET_SPEED = 0.5
@@ -63,8 +61,6 @@ class LongControl():
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
                             rate=RATE,
                             sat_limit=0.8)
-    self.pid.pos_limit = ACCEL_MAX
-    self.pid.neg_limit = ACCEL_MIN
     self.v_pid = 0.0
     self.last_output_accel = 0.0
 
@@ -73,7 +69,7 @@ class LongControl():
     self.pid.reset()
     self.v_pid = v_pid
 
-  def update(self, active, CS, CP, long_plan):
+  def update(self, active, CS, CP, long_plan, accel_limits):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     # Interp control trajectory
     # TODO estimate car specific lag, use .15s for now
@@ -86,6 +82,8 @@ class LongControl():
       v_target_future = 0.0
       a_target = 0.0
 
+    self.pid.neg_limit = accel_limits[0]
+    self.pid.pos_limit = accel_limits[1]
 
     # Update state machine
     output_accel = self.last_output_accel
@@ -107,8 +105,9 @@ class LongControl():
       # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
       prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
       deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
+      freeze_integrator = prevent_overshoot
 
-      output_accel = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
+      output_accel = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=freeze_integrator)
 
       if prevent_overshoot:
         output_accel = min(output_accel, 0.0)
@@ -118,7 +117,7 @@ class LongControl():
       # Keep applying brakes until the car is stopped
       if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET:
         output_accel -= CP.stoppingDecelRate / RATE
-      output_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX)
+      output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
 
       self.reset(CS.vEgo)
 
@@ -129,6 +128,6 @@ class LongControl():
       self.reset(CS.vEgo)
 
     self.last_output_accel = output_accel
-    final_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX)
+    final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
 
     return final_accel, v_target, a_target