You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
107 lines
2.7 KiB
107 lines
2.7 KiB
#!/usr/bin/env python3
|
|
from enum import Enum
|
|
from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
|
|
|
|
class LongTunes(Enum):
|
|
PEDAL = 0
|
|
TSS2 = 1
|
|
TSS = 2
|
|
|
|
class LatTunes(Enum):
|
|
INDI_PRIUS = 0
|
|
LQR_RAV4 = 1
|
|
PID_A = 2
|
|
PID_B = 3
|
|
PID_C = 4
|
|
PID_D = 5
|
|
PID_E = 6
|
|
PID_F = 7
|
|
PID_G = 8
|
|
PID_I = 9
|
|
PID_H = 10
|
|
PID_J = 11
|
|
PID_K = 12
|
|
PID_L = 13
|
|
PID_M = 14
|
|
PID_N = 15
|
|
TORQUE = 16
|
|
|
|
|
|
###### LONG ######
|
|
def set_long_tune(tune, name):
|
|
# Improved longitudinal tune
|
|
if name == LongTunes.TSS2 or name == LongTunes.PEDAL:
|
|
tune.deadzoneBP = [0., 8.05]
|
|
tune.deadzoneV = [.0, .14]
|
|
tune.kpBP = [0., 5., 20.]
|
|
tune.kpV = [1.3, 1.0, 0.7]
|
|
tune.kiBP = [0., 5., 12., 20., 27.]
|
|
tune.kiV = [.35, .23, .20, .17, .1]
|
|
# Default longitudinal tune
|
|
elif name == LongTunes.TSS:
|
|
tune.deadzoneBP = [0., 9.]
|
|
tune.deadzoneV = [0., .15]
|
|
tune.kpBP = [0., 5., 35.]
|
|
tune.kiBP = [0., 35.]
|
|
tune.kpV = [3.6, 2.4, 1.5]
|
|
tune.kiV = [0.54, 0.36]
|
|
else:
|
|
raise NotImplementedError('This longitudinal tune does not exist')
|
|
|
|
|
|
###### LAT ######
|
|
def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, use_steering_angle=True):
|
|
if name == LatTunes.TORQUE:
|
|
set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION)
|
|
elif 'PID' in str(name):
|
|
tune.init('pid')
|
|
tune.pid.kiBP = [0.0]
|
|
tune.pid.kpBP = [0.0]
|
|
if name == LatTunes.PID_A:
|
|
tune.pid.kpV = [0.2]
|
|
tune.pid.kiV = [0.05]
|
|
tune.pid.kf = 0.00003
|
|
elif name == LatTunes.PID_C:
|
|
tune.pid.kpV = [0.6]
|
|
tune.pid.kiV = [0.1]
|
|
tune.pid.kf = 0.00006
|
|
elif name == LatTunes.PID_D:
|
|
tune.pid.kpV = [0.6]
|
|
tune.pid.kiV = [0.1]
|
|
tune.pid.kf = 0.00007818594
|
|
elif name == LatTunes.PID_F:
|
|
tune.pid.kpV = [0.723]
|
|
tune.pid.kiV = [0.0428]
|
|
tune.pid.kf = 0.00006
|
|
elif name == LatTunes.PID_G:
|
|
tune.pid.kpV = [0.18]
|
|
tune.pid.kiV = [0.015]
|
|
tune.pid.kf = 0.00012
|
|
elif name == LatTunes.PID_H:
|
|
tune.pid.kpV = [0.17]
|
|
tune.pid.kiV = [0.03]
|
|
tune.pid.kf = 0.00006
|
|
elif name == LatTunes.PID_I:
|
|
tune.pid.kpV = [0.15]
|
|
tune.pid.kiV = [0.05]
|
|
tune.pid.kf = 0.00004
|
|
elif name == LatTunes.PID_J:
|
|
tune.pid.kpV = [0.19]
|
|
tune.pid.kiV = [0.02]
|
|
tune.pid.kf = 0.00007818594
|
|
elif name == LatTunes.PID_L:
|
|
tune.pid.kpV = [0.3]
|
|
tune.pid.kiV = [0.05]
|
|
tune.pid.kf = 0.00006
|
|
elif name == LatTunes.PID_M:
|
|
tune.pid.kpV = [0.3]
|
|
tune.pid.kiV = [0.05]
|
|
tune.pid.kf = 0.00007
|
|
elif name == LatTunes.PID_N:
|
|
tune.pid.kpV = [0.35]
|
|
tune.pid.kiV = [0.15]
|
|
tune.pid.kf = 0.00007818594
|
|
else:
|
|
raise NotImplementedError('This PID tune does not exist')
|
|
else:
|
|
raise NotImplementedError('This lateral tune does not exist')
|
|
|