diff --git a/opendbc/car/ford/carcontroller.py b/opendbc/car/ford/carcontroller.py index c27e4d2711..d7444160b3 100644 --- a/opendbc/car/ford/carcontroller.py +++ b/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) @@ -28,6 +35,7 @@ def __init__(self, dbc_name, CP): 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