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

Lateral PID: move steer feedforward to CarInterface #22411

Merged
merged 2 commits into from
Oct 4, 2021
Merged

Lateral PID: move steer feedforward to CarInterface #22411

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
13ecb0c
move steer feedforward to car interface, in car parameters
hewers Oct 2, 2021
630ce9e
LatControlPID.get_steer_feedforward()
hewers Oct 3, 2021
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
6 changes: 6 additions & 0 deletions selfdrive/car/interfaces.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
def init(CP, logcan, sendcan):
pass

@staticmethod
def get_steer_feedforward(desired_angle, v_ego):
# Proportional to realigning tire momentum: lateral acceleration.
# TODO: something with lateralPlan.curvatureRates
return desired_angle * (v_ego**2)

# returns a set of default params to avoid repetition in car specific params
@staticmethod
def get_std_params(candidate, fingerprint):
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/controlsd.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ def __init__(self, sm=None, pm=None, can_sock=None):
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
Expand Down
10 changes: 5 additions & 5 deletions selfdrive/controls/lib/latcontrol_pid.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,12 @@


class LatControlPID():
def __init__(self, CP):
def __init__(self, CP, CI):
self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.get_steer_feedforward = CI.get_steer_feedforward

def reset(self):
self.pid.reset()
Expand All @@ -23,7 +24,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg

pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
Expand All @@ -33,9 +34,8 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max

# TODO: feedforward something based on lat_plan.rateSteers
steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)

deadzone = 0.0

Expand Down