compensate for creeping when stopping

opendbc/car/ford/carcontroller.py

@@ -1,8 +1,8 @@
 from opendbc.can.packer import CANPacker
 from opendbc.car import apply_std_steer_angle_limits, structs
 from opendbc.car.ford import fordcan
-from opendbc.car.ford.values import CarControllerParams, FordFlags
-from opendbc.car.common.numpy_fast import clip
+from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
+from opendbc.car.common.numpy_fast import clip, interp
 from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
 
 LongCtrlState = structs.CarControl.Actuators.LongControlState
@@ -21,6 +21,13 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c
   return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
 
 
+def apply_creep_compensation(accel: float, v_ego: float) -> float:
+  creep_accel = interp(v_ego, [1., 3.], [0.6, 0.])
+  if accel < 0.:
+    accel -= creep_accel
+  return accel
+
+
 class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP):
     super().__init__(dbc_name, CP)
@@ -88,8 +95,15 @@ def update(self, CC, CS, now_nanos):
     ### longitudinal control ###
     # send acc msg at 50Hz
     if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
+      # Compensate for engine creep at low speed.
+      # Either the ABS does not account for engine creep, or the correction is very slow
+      # TODO: whitelist more cars
+      accel = actuators.accel
+      if CC.longActive and self.CP.carFingerprint == CAR.FORD_BRONCO_SPORT_MK1:
+        accel = apply_creep_compensation(accel, CS.out.vEgo)
+      accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+
       # Both gas and accel are in m/s^2, accel is used solely for braking
-      accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
       gas = accel
       if not CC.longActive or gas < CarControllerParams.MIN_GAS:
         gas = CarControllerParams.INACTIVE_GAS