From 9fbda5fbea184127d31ad7f1730a9e4f10685c7b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 23 Oct 2024 17:59:37 -0700 Subject: [PATCH 1/2] compensate for creeping when stopping --- opendbc/car/ford/carcontroller.py | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/opendbc/car/ford/carcontroller.py b/opendbc/car/ford/carcontroller.py index c27e4d2711..0567da9801 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) @@ -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 From ae8ab06c9d392db5d070d61519f210bae3998b39 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 23 Oct 2024 18:58:23 -0700 Subject: [PATCH 2/2] log new accel --- opendbc/car/ford/carcontroller.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/opendbc/car/ford/carcontroller.py b/opendbc/car/ford/carcontroller.py index 0567da9801..d7444160b3 100644 --- a/opendbc/car/ford/carcontroller.py +++ b/opendbc/car/ford/carcontroller.py @@ -35,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 @@ -98,18 +99,18 @@ def update(self, CC, CS, now_nanos): # 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 + self.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) + 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 - 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) @@ -135,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