sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 64 Packages Projects Releases Wiki Activity

Interpolate ki/kp for steering PID loop (#200)

Browse Source 
* Interpolate ki/kp for steering PID loop

Very much needed for the Volt port: car ping-pongs with low kp
on high speeeds, and the loop is unstable with high kp on
low speeds.

Also, removes "number or array?" logic from PIController,
now that all the callers use interpolation ofr ki/kp.

* Pass speed to steering PID loop for ki/kp interpolation

* Remove unused numbers import
pull/6/head
Vasily Tarasov 8 years ago committed by rbiasini
parent 569db3c1d2
commit 93f55f3ccf
5 changed files with 24 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      cereal/car.capnp
  2. 13
      selfdrive/car/honda/interface.py
  3. 9
      selfdrive/car/toyota/interface.py
  4. 6
      selfdrive/controls/lib/latcontrol.py
  5. 15
      selfdrive/controls/lib/pid.py

8
cereal/car.capnp
Unescape View File

@ -303,8 +303,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;
steerKiDEPRECATED @16 :Float32;
steerKf @25 :Float32;
# Kp and Ki for the longitudinal control

13
selfdrive/car/honda/interface.py
Unescape View File

@ -151,6 +151,7 @@ class CarInterface(object):
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
if candidate == CAR.CIVIC:
stop_and_go = True
ret.mass = mass_civic
@ -159,7 +160,7 @@ class CarInterface(object):
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]
@ -173,7 +174,7 @@ class CarInterface(object):
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]
@ -185,7 +186,7 @@ class CarInterface(object):
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]
@ -197,7 +198,7 @@ class CarInterface(object):
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]
@ -209,7 +210,7 @@ class CarInterface(object):
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]
@ -221,7 +222,7 @@ class CarInterface(object):
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]

9
selfdrive/car/toyota/interface.py
Unescape View File

@ -72,12 +72,13 @@ class CarInterface(object):
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]:
@ -85,7 +86,7 @@ class CarInterface(object):
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:
@ -93,7 +94,7 @@ class CarInterface(object):
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:
@ -101,7 +102,7 @@ class CarInterface(object):
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

6
selfdrive/controls/lib/latcontrol.py
Unescape View File

@ -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)
@ -103,7 +105,7 @@ class LatControl(object):
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
selfdrive/controls/lib/pid.py
Unescape View File

@ -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:
@ -30,21 +29,11 @@ class PIController(object):
@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)

Write Preview
Loading…
Cancel
Save