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

Ford: improved steering control #25713

Closed
wants to merge 3 commits into from
Closed

Ford: improved steering control #25713

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b153a6c
cleanup
incognitojam Sep 9, 2022
efec34e
use PID lateral controller to correct for angle error
incognitojam Sep 9, 2022
e7b7f39
refactor rate limits
incognitojam Sep 9, 2022
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
48 changes: 32 additions & 16 deletions selfdrive/car/ford/carcontroller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,8 @@ def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo):
# rate limit
steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last)
rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN
max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.angle_rate_points)
max_angle_diff /= CarControllerParams.LKAS_STEER_STEP
apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))

# absolute limit (LatCtlPath_An_Actl)
Expand Down Expand Up @@ -57,30 +58,45 @@ def update(self, CC, CS):


### lateral control ###
if CC.latActive:
apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo)
else:
apply_angle = CS.out.steeringAngleDeg
# Ford lane centering command uses a third order polynomial to describe the road centerline.
# The polynomial is defined by the following coefficients:
# c0: lateral offset between the vehicle and the centerline
# c1: heading angle between the vehicle and the centerline
# c2: curvature of the centerline
# c3: rate of change of curvature of the centerline
# As the EPS controller combines this information with other sensor data, the steering angle
# cannot be easily controlled. A closed loop controller is used to achieve the desired steering
# angle.

# send steering commands at 20Hz
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
lca_rq = 1 if CC.latActive else 0
if CC.latActive:
new_steer = actuators.steeringAngleDeg + actuators.steer * CarControllerParams.STEER_MAX

lca_rq = 1
apply_angle = apply_ford_steer_angle_limits(new_steer, self.apply_angle_last, CS.out.vEgo)
else:
lca_rq = 0
apply_angle = CS.out.steeringAngleDeg

# use LatCtlPath_An_Actl to actuate steering
# path angle is the car wheel angle, not the steering wheel angle
path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO

# ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
# TODO: try slower ramp speed when driver torque detected
ramp_type = 3
precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable)

offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094)
# set slower ramp type when small steering angle change
# 0=Slow, 1=Medium, 2=Fast, 3=Immediately
angle_error = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg)
if angle_error < 3.0:
ramp_type = 0
elif angle_error < 6.0:
ramp_type = 1
else:
ramp_type = 2
precision = 1 # 0=Comfortable, 1=Precise

self.apply_angle_last = apply_angle
can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0))
can_sends.append(fordcan.create_lka_command(self.packer, 0, 0))
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
0, path_angle, 0, offset_roll_compensation_curvature))
0, path_angle, 0, 0))


### ui ###
Expand All @@ -99,7 +115,7 @@ def update(self, CC, CS):
self.steer_alert_last = steer_alert

new_actuators = actuators.copy()
new_actuators.steeringAngleDeg = apply_angle
new_actuators.steeringAngleDeg = self.apply_angle_last

self.frame += 1
return new_actuators, can_sends
11 changes: 6 additions & 5 deletions selfdrive/car/ford/interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,6 @@
from selfdrive.car.interfaces import CarInterfaceBase


EventName = car.CarEvent.EventName


class CarInterface(CarInterfaceBase):
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
Expand All @@ -21,11 +18,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, expe
ret.dashcamOnly = True
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]

# Angle-based steering
ret.steerControlType = car.CarParams.SteerControlType.angle
# Angle-based steering, but we use PID to correct for angle error
ret.steerActuatorDelay = 0.4
ret.steerLimitTimer = 1.0
tire_stiffness_factor = 1.0
ret.lateralTuning.pid.kpBP = [0.]
ret.lateralTuning.pid.kiBP = [0.]
ret.lateralTuning.pid.kf = 0.
ret.lateralTuning.pid.kpV = [0.1]
ret.lateralTuning.pid.kiV = [0.]

if candidate == CAR.ESCAPE_MK4:
ret.wheelbase = 2.71
Expand Down
11 changes: 6 additions & 5 deletions selfdrive/car/ford/values.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter

AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'angle_rate_points'])


class CarControllerParams:
Expand All @@ -22,11 +22,12 @@ class CarControllerParams:
# Message: ACCDATA_3
ACC_UI_STEP = 5

STEER_RATIO = 2.75
STEER_DRIVER_ALLOWANCE = 0.8
STEER_MAX = 5 # Max correction angle in degrees
STEER_RATIO = 2.75 # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians
STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold in Newton-meters

RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], angle_rate_points=[500., 80., 15.])
RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], angle_rate_points=[500., 350., 40.])


class RADAR:
Expand Down