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

Interpolate ki/kp for steering PID loop #200

Merged
merged 3 commits into from
Mar 15, 2018
Merged

Interpolate ki/kp for steering PID loop #200

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
55b65d7
Interpolate ki/kp for steering PID loop
vntarasov Feb 22, 2018
5c4ddf7
Pass speed to steering PID loop for ki/kp interpolation
vntarasov Feb 22, 2018
3c2ddf1
Remove unused numbers import
vntarasov Mar 15, 2018
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
8 changes: 6 additions & 2 deletions cereal/car.capnp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -301,8 +301,12 @@ struct CarParams {
tireStiffnessRear @14 :Float32; # [N/rad] rear tire coeff of stiff

# Kp and Ki for the lateral control
steerKp @15 :Float32;
steerKi @16 :Float32;
steerKpBP @42 :List(Float32);
steerKpV @43 :List(Float32);
steerKiBP @44 :List(Float32);
steerKiV @45 :List(Float32);
steerKpDEPRECATED @15 :Float32;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@vntarasov , since carParams isn't logged, you can remove the DEPRECATED ones.

Copy link
Author

@vntarasov vntarasov Feb 22, 2018 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks @rbiasini! Should I then clean up existing DEPRECATED carParams fields as well?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No worries, I'll do the clean up on our internal branch, since it will generate a lot of index shifting.

steerKiDEPRECATED @16 :Float32;
steerKf @25 :Float32;

# Kp and Ki for the longitudinal control
Expand Down
13 changes: 7 additions & 6 deletions selfdrive/car/honda/interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,7 @@ def get_params(candidate, fingerprint):
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000

ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
if candidate == CAR.CIVIC:
stop_and_go = True
ret.mass = mass_civic
Expand All @@ -159,7 +160,7 @@ def get_params(candidate, fingerprint):
ret.steerRatio = 13.0
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]

ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
Expand All @@ -173,7 +174,7 @@ def get_params(candidate, fingerprint):
ret.steerRatio = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]

ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
Expand All @@ -185,7 +186,7 @@ def get_params(candidate, fingerprint):
ret.wheelbase = 2.62
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]

ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
Expand All @@ -197,7 +198,7 @@ def get_params(candidate, fingerprint):
ret.wheelbase = 2.68
ret.centerToFront = ret.wheelbase * 0.38
ret.steerRatio = 15.0
ret.steerKp, ret.steerKi = 0.8, 0.24
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]

ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
Expand All @@ -209,7 +210,7 @@ def get_params(candidate, fingerprint):
ret.wheelbase = 3.00
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 14.35
ret.steerKp, ret.steerKi = 0.6, 0.18
ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]

ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
Expand All @@ -221,7 +222,7 @@ def get_params(candidate, fingerprint):
ret.wheelbase = 2.81
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0
ret.steerKp, ret.steerKi = 0.38, 0.11
ret.steerKpV, ret.steerKiV = [[0.38], [0.11]]

ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
Expand Down
9 changes: 5 additions & 4 deletions selfdrive/car/toyota/interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,36 +72,37 @@ def get_params(candidate, fingerprint):
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000

ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
if candidate == CAR.PRIUS:
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.70
ret.steerRatio = 14.5 # TODO: find exact value for Prius
ret.mass = 3045./2.205 + std_cargo
ret.steerKp, ret.steerKi = 0.6, 0.05
ret.steerKpV, ret.steerKiV = [[0.6], [0.05]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
ret.steerRateCost = 2.
elif candidate in [CAR.RAV4, CAR.RAV4H]:
ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.65
ret.steerRatio = 14.5 # Rav4 2017
ret.mass = 3650./2.205 + std_cargo # mean between normal and hybrid
ret.steerKp, ret.steerKi = 0.6, 0.05
ret.steerKpV, ret.steerKiV = [[0.6], [0.05]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
ret.steerRateCost = 1.
elif candidate == CAR.COROLLA:
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.70
ret.steerRatio = 17.8
ret.mass = 2860./2.205 + std_cargo # mean between normal and hybrid
ret.steerKp, ret.steerKi = 0.2, 0.05
ret.steerKpV, ret.steerKiV = [[0.2], [0.05]]
ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerRateCost = 1.
elif candidate == CAR.LEXUS_RXH:
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.79
ret.steerRatio = 16. # official specs say 14.8, but it does not seem right
ret.mass = 4481./2.205 + std_cargo # mean between min and max
ret.steerKp, ret.steerKi = 0.6, 0.1
ret.steerKpV, ret.steerKiV = [[0.6], [0.1]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
ret.steerRateCost = .8

Expand Down
6 changes: 4 additions & 2 deletions selfdrive/controls/lib/latcontrol.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,9 @@ def get_steer_max(CP, v_ego):

class LatControl(object):
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
self.pid = PIController((VM.CP.steerKpBP, VM.CP.steerKpV),
(VM.CP.steerKiBP, VM.CP.steerKiV),
k_f=VM.CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc(VM.CP.steerRateCost)

Expand Down Expand Up @@ -103,7 +105,7 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward)
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego)

self.sat_flag = self.pid.saturated
return output_steer
15 changes: 2 additions & 13 deletions selfdrive/controls/lib/pid.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
import numpy as np
from common.numpy_fast import clip, interp
import numbers

def apply_deadzone(error, deadzone):
if error > deadzone:
Expand Down Expand Up @@ -30,21 +29,11 @@ def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, s

@property
def k_p(self):
if isinstance(self._k_p, numbers.Number):
kp = self._k_p
else:
kp = interp(self.speed, self._k_p[0], self._k_p[1])

return kp
return interp(self.speed, self._k_p[0], self._k_p[1])

@property
def k_i(self):
if isinstance(self._k_i, numbers.Number):
ki = self._k_i
else:
ki = interp(self.speed, self._k_i[0], self._k_i[1])

return ki
return interp(self.speed, self._k_i[0], self._k_i[1])

def _check_saturation(self, control, override, error):
saturated = (control < self.neg_limit) or (control > self.pos_limit)
Expand Down