Cleanup toyota tuning (#22859)
* start cleanup * start lateral * finish lateral * add file * typo * add enu * remove whitespace * cleanup safetyparam * typopull/21968/head
HaraldSchafer
GitHub
parent
ca88a8769b
commit
06844821c5
|
|
@ -130,6 +130,7 @@ selfdrive/car/hyundai/carcontroller.py
|
|||
selfdrive/car/hyundai/hyundaican.py
|
||||
selfdrive/car/toyota/__init__.py
|
||||
selfdrive/car/toyota/carstate.py
|
||||
selfdrive/car/toyota/tunes.py
|
||||
selfdrive/car/toyota/interface.py
|
||||
selfdrive/car/toyota/radar_interface.py
|
||||
selfdrive/car/toyota/values.py
|
||||
|
|
|
|||
|
|
@ -1,7 +1,8 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
|
||||
from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune
|
||||
from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, CarControllerParams
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
|
@ -23,267 +24,208 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
|
||||
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
|
||||
|
||||
if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
# Most cars use this default safety param
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
|
||||
if candidate == CAR.PRIUS:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 15.74 # unknown end-to-end spec
|
||||
tire_stiffness_factor = 0.6371 # hand-tune
|
||||
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
|
||||
ret.lateralTuning.init('indi')
|
||||
ret.lateralTuning.indi.innerLoopGainBP = [0.]
|
||||
ret.lateralTuning.indi.innerLoopGainV = [4.0]
|
||||
ret.lateralTuning.indi.outerLoopGainBP = [0.]
|
||||
ret.lateralTuning.indi.outerLoopGainV = [3.0]
|
||||
ret.lateralTuning.indi.timeConstantBP = [0.]
|
||||
ret.lateralTuning.indi.timeConstantV = [1.0]
|
||||
ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
|
||||
ret.lateralTuning.indi.actuatorEffectivenessV = [1.0]
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
|
||||
ret.steerActuatorDelay = 0.3
|
||||
|
||||
elif candidate in [CAR.RAV4, CAR.RAV4H]:
|
||||
stop_and_go = True if (candidate in CAR.RAV4H) else False
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.65
|
||||
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
|
||||
tire_stiffness_factor = 0.5533
|
||||
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
|
||||
ret.lateralTuning.init('lqr')
|
||||
|
||||
ret.lateralTuning.lqr.scale = 1500.0
|
||||
ret.lateralTuning.lqr.ki = 0.05
|
||||
|
||||
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
|
||||
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
|
||||
ret.lateralTuning.lqr.c = [1., 0.]
|
||||
ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
|
||||
ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
|
||||
ret.lateralTuning.lqr.dcGain = 0.002237852961363602
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
|
||||
|
||||
elif candidate == CAR.COROLLA:
|
||||
stop_and_go = False
|
||||
ret.safetyConfigs[0].safetyParam = 88
|
||||
stop_and_go = False
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 18.27
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_A)
|
||||
|
||||
elif candidate == CAR.LEXUS_RX:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.79
|
||||
ret.steerRatio = 14.8
|
||||
tire_stiffness_factor = 0.5533
|
||||
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_B)
|
||||
|
||||
|
||||
elif candidate == CAR.LEXUS_RXH:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.79
|
||||
ret.steerRatio = 16. # 14.8 is spec end-to-end
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
|
||||
|
||||
elif candidate == CAR.LEXUS_RX_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.79
|
||||
ret.steerRatio = 14.8
|
||||
tire_stiffness_factor = 0.5533 # not optimized yet
|
||||
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
|
||||
|
||||
elif candidate == CAR.LEXUS_RXH_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.79
|
||||
ret.steerRatio = 16.0 # 14.8 is spec end-to-end
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 4481.0 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.15]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_E)
|
||||
|
||||
elif candidate in [CAR.CHR, CAR.CHRH]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.63906
|
||||
ret.steerRatio = 13.6
|
||||
tire_stiffness_factor = 0.7933
|
||||
ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_F)
|
||||
|
||||
elif candidate in [CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.82448
|
||||
ret.steerRatio = 13.7
|
||||
tire_stiffness_factor = 0.7933
|
||||
ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
|
||||
|
||||
elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.84988 # 112.2 in = 2.84988 m
|
||||
ret.steerRatio = 16.0
|
||||
tire_stiffness_factor = 0.8
|
||||
ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning
|
||||
ret.lateralTuning.pid.kf = 0.00012 # community tuning
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
|
||||
|
||||
elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 16.0
|
||||
tire_stiffness_factor = 0.8
|
||||
ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning
|
||||
ret.lateralTuning.pid.kf = 0.00012 # community tuning
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
|
||||
|
||||
elif candidate in [CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019]:
|
||||
stop_and_go = False
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.82
|
||||
ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
|
||||
tire_stiffness_factor = 0.7983
|
||||
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_H)
|
||||
|
||||
elif candidate in [CAR.RAV4_TSS2, CAR.RAV4H_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.68986
|
||||
ret.steerRatio = 14.3
|
||||
tire_stiffness_factor = 0.7933
|
||||
ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
|
||||
|
||||
# 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
|
||||
# See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']):
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00004
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_I)
|
||||
break
|
||||
|
||||
elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback
|
||||
ret.steerRatio = 13.9
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
|
||||
|
||||
elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.8702
|
||||
ret.steerRatio = 16.0 # not optimized
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
|
||||
|
||||
elif candidate == CAR.LEXUS_ESH:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.8190
|
||||
ret.steerRatio = 16.06
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 3682. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
|
||||
|
||||
elif candidate == CAR.SIENNA:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 3.03
|
||||
ret.steerRatio = 15.5
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
|
||||
|
||||
elif candidate == CAR.LEXUS_IS:
|
||||
stop_and_go = False
|
||||
ret.safetyConfigs[0].safetyParam = 77
|
||||
stop_and_go = False
|
||||
ret.wheelbase = 2.79908
|
||||
ret.steerRatio = 13.3
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
|
||||
|
||||
elif candidate == CAR.LEXUS_CTH:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 100
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.60
|
||||
ret.steerRatio = 18.6
|
||||
tire_stiffness_factor = 0.517
|
||||
ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00007
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_M)
|
||||
|
||||
elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2]:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.66
|
||||
ret.steerRatio = 14.7
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
|
||||
|
||||
elif candidate == CAR.PRIUS_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.70002 # from toyota online sepc.
|
||||
ret.steerRatio = 13.4 # True steerRation from older prius
|
||||
tire_stiffness_factor = 0.6371 # hand-tune
|
||||
ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.15]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_N)
|
||||
|
||||
elif candidate == CAR.MIRAI:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 2.91
|
||||
ret.steerRatio = 14.8
|
||||
tire_stiffness_factor = 0.8
|
||||
ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
|
||||
|
||||
elif candidate == CAR.ALPHARD_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyConfigs[0].safetyParam = 73
|
||||
ret.wheelbase = 3.00
|
||||
ret.steerRatio = 14.2
|
||||
tire_stiffness_factor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]]
|
||||
ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_J)
|
||||
|
||||
ret.steerRateCost = 1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
|
@ -316,28 +258,13 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
|
||||
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
|
||||
ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36]
|
||||
set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL)
|
||||
elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]:
|
||||
# Improved longitudinal tune
|
||||
ret.longitudinalTuning.deadzoneBP = [0., 8.05]
|
||||
ret.longitudinalTuning.deadzoneV = [.0, .14]
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 20.]
|
||||
ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
|
||||
ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.]
|
||||
ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
|
||||
set_long_tune(ret.longitudinalTuning, LongTunes.TSS2)
|
||||
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
|
||||
ret.startingAccelRate = 6.0 # release brakes fast
|
||||
else:
|
||||
# Default longitudinal tune
|
||||
ret.longitudinalTuning.deadzoneBP = [0., 9.]
|
||||
ret.longitudinalTuning.deadzoneV = [0., .15]
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.54, 0.36]
|
||||
set_long_tune(ret.longitudinalTuning, LongTunes.TSS)
|
||||
|
||||
return ret
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,142 @@
|
|||
#!/usr/bin/env python3
|
||||
from enum import Enum
|
||||
from selfdrive.car.toyota.values import MIN_ACC_SPEED, PEDAL_HYST_GAP
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
###### LONG ######
|
||||
def set_long_tune(tune, name):
|
||||
if name == LongTunes.PEDAL:
|
||||
tune.deadzoneBP = [0.]
|
||||
tune.deadzoneV = [0.]
|
||||
tune.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
|
||||
tune.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
|
||||
tune.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
|
||||
tune.kiV = [0.18, 0.165, 0.489, 0.36]
|
||||
# Improved longitudinal tune
|
||||
elif name == LongTunes.TSS2:
|
||||
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):
|
||||
if name == LatTunes.INDI_PRIUS:
|
||||
tune.init('indi')
|
||||
tune.indi.innerLoopGainBP = [0.]
|
||||
tune.indi.innerLoopGainV = [4.0]
|
||||
tune.indi.outerLoopGainBP = [0.]
|
||||
tune.indi.outerLoopGainV = [3.0]
|
||||
tune.indi.timeConstantBP = [0.]
|
||||
tune.indi.timeConstantV = [1.0]
|
||||
tune.indi.actuatorEffectivenessBP = [0.]
|
||||
tune.indi.actuatorEffectivenessV = [1.0]
|
||||
|
||||
elif name == LatTunes.LQR_RAV4:
|
||||
tune.init('lqr')
|
||||
tune.lqr.scale = 1500.0
|
||||
tune.lqr.ki = 0.05
|
||||
tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
|
||||
tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
|
||||
tune.lqr.c = [1., 0.]
|
||||
tune.lqr.k = [-110.73572306, 451.22718255]
|
||||
tune.lqr.l = [0.3233671, 0.3185757]
|
||||
tune.lqr.dcGain = 0.002237852961363602
|
||||
|
||||
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_B:
|
||||
tune.pid.kpV = [0.6]
|
||||
tune.pid.kiV = [0.05]
|
||||
tune.pid.kf = 0.00006
|
||||
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_E:
|
||||
tune.pid.kpV = [0.6]
|
||||
tune.pid.kiV = [0.15]
|
||||
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')
|
||||
Loading…
Reference in New Issue