Ford Bronco: compensate for creeping when stopping (#1411)

* compensate for creeping when stopping

* log new accel

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,13 +21,21 @@ 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)
     self.packer = CANPacker(dbc_name)
     self.CAN = fordcan.CanBus(CP)
 
     self.apply_curvature_last = 0
+    self.accel = 0.0
     self.main_on_last = False
     self.lkas_enabled_last = False
     self.steer_alert_last = False
@@ -88,14 +96,21 @@ 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
+      self.accel = actuators.accel
+      if CC.longActive and self.CP.carFingerprint == CAR.FORD_BRONCO_SPORT_MK1:
+        self.accel = apply_creep_compensation(self.accel, CS.out.vEgo)
+      self.accel = clip(self.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
+      gas = self.accel
       if not CC.longActive or gas < CarControllerParams.MIN_GAS:
         gas = CarControllerParams.INACTIVE_GAS
       stopping = CC.actuators.longControlState == LongCtrlState.stopping
       # TODO: look into using the actuators packet to send the desired speed
-      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=V_CRUISE_MAX))
+      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, self.accel, stopping, v_ego_kph=V_CRUISE_MAX))
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
@@ -121,6 +136,7 @@ def update(self, CC, CS, now_nanos):
 
     new_actuators = actuators.as_builder()
     new_actuators.curvature = self.apply_curvature_last
+    new_actuators.accel = self.accel
 
     self.frame += 1
     return new_actuators, can_sends