Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Ford Bronco: compensate for creeping when stopping #1411

Merged
merged 2 commits into from
Oct 24, 2024
Merged

Ford Bronco: compensate for creeping when stopping #1411

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9fbda5f
compensate for creeping when stopping
sshane Oct 24, 2024
ae8ab06
log new accel
sshane Oct 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 21 additions & 5 deletions opendbc/car/ford/carcontroller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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
Loading