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

cleanup torque tuning config (#24951)

Browse Source
pull/214/head
Adeeb Shihadeh 3 years ago committed by GitHub
parent e26db5dc91
commit 062a8bcdbd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 45 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      selfdrive/car/hyundai/interface.py
  2. 59
      selfdrive/car/interfaces.py
  3. 8
      selfdrive/car/toyota/interface.py
  4. 6
      selfdrive/car/toyota/tunes.py
  5. 10
      selfdrive/controls/lib/latcontrol_torque.py

12
selfdrive/car/hyundai/interface.py
Unescape View File

@ -7,7 +7,6 @@ from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.disable_ecu import disable_ecu from selfdrive.car.disable_ecu import disable_ecu
from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
ButtonType = car.CarState.ButtonEvent.Type ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName EventName = car.CarEvent.EventName
@ -51,7 +50,6 @@ class CarInterface(CarInterfaceBase):
ret.stopAccel = 0.0 ret.stopAccel = 0.0
ret.longitudinalActuatorDelayUpperBound = 1.0 # s ret.longitudinalActuatorDelayUpperBound = 1.0 # s
torque_params = CarInterfaceBase.get_torque_params(candidate)
if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
@ -66,7 +64,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.84 ret.wheelbase = 2.84
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
tire_stiffness_factor = 0.65 tire_stiffness_factor = 0.65
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.SONATA_LF: elif candidate == CAR.SONATA_LF:
ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kf = 0.00005
ret.mass = 4497. * CV.LB_TO_KG ret.mass = 4497. * CV.LB_TO_KG
@ -96,13 +94,13 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.72 ret.wheelbase = 2.72
ret.steerRatio = 12.9 ret.steerRatio = 12.9
tire_stiffness_factor = 0.65 tire_stiffness_factor = 0.65
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.ELANTRA_HEV_2021: elif candidate == CAR.ELANTRA_HEV_2021:
ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG
ret.wheelbase = 2.72 ret.wheelbase = 2.72
ret.steerRatio = 12.9 ret.steerRatio = 12.9
tire_stiffness_factor = 0.65 tire_stiffness_factor = 0.65
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.HYUNDAI_GENESIS: elif candidate == CAR.HYUNDAI_GENESIS:
ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG ret.mass = 2060. + STD_CARGO_KG
@ -204,7 +202,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.80 ret.wheelbase = 2.80
ret.steerRatio = 13.75 ret.steerRatio = 13.75
tire_stiffness_factor = 0.5 tire_stiffness_factor = 0.5
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.KIA_STINGER: elif candidate == CAR.KIA_STINGER:
ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825. + STD_CARGO_KG ret.mass = 1825. + STD_CARGO_KG
@ -244,7 +242,7 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput), ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),
get_safety_config(car.CarParams.SafetyModel.hyundaiHDA2)] get_safety_config(car.CarParams.SafetyModel.hyundaiHDA2)]
tire_stiffness_factor = 0.65 tire_stiffness_factor = 0.65
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION']) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
# Genesis # Genesis
elif candidate == CAR.GENESIS_G70: elif candidate == CAR.GENESIS_G70:

59
selfdrive/car/interfaces.py
Unescape View File

@ -20,11 +20,36 @@ EventName = car.CarEvent.EventName
MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
ACCEL_MAX = 2.0 ACCEL_MAX = 2.0
ACCEL_MIN = -3.5 ACCEL_MIN = -3.5
TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yaml') TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yaml')
TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml') TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml')
TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.yaml') TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.yaml')
def get_torque_params(candidate, default=float('NaN')):
with open(TORQUE_SUBSTITUTE_PATH) as f:
sub = yaml.load(f, Loader=yaml.FullLoader)
if candidate in sub:
candidate = sub[candidate]
with open(TORQUE_PARAMS_PATH) as f:
params = yaml.load(f, Loader=yaml.FullLoader)
with open(TORQUE_OVERRIDE_PATH) as f:
override = yaml.load(f, Loader=yaml.FullLoader)
# Ensure no overlap
if sum([candidate in x for x in [sub, params, override]]) > 1:
raise RuntimeError(f'{candidate} is defined twice in torque config')
if candidate in override:
out = override[candidate]
elif candidate in params:
out = params[candidate]
else:
raise NotImplementedError(f"Did not find torque params for {candidate}")
return {key:out[i] for i, key in enumerate(params['legend'])}
# generic car and radar interfaces # generic car and radar interfaces
class CarInterfaceBase(ABC): class CarInterfaceBase(ABC):
@ -85,7 +110,7 @@ class CarInterfaceBase(ABC):
ret.steerControlType = car.CarParams.SteerControlType.torque ret.steerControlType = car.CarParams.SteerControlType.torque
ret.minSteerSpeed = 0. ret.minSteerSpeed = 0.
ret.wheelSpeedFactor = 1.0 ret.wheelSpeedFactor = 1.0
ret.maxLateralAccel = CarInterfaceBase.get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED'] ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED']
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
@ -110,28 +135,16 @@ class CarInterfaceBase(ABC):
return ret return ret
@staticmethod @staticmethod
def get_torque_params(candidate, default=float('NaN')): def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0):
with open(TORQUE_SUBSTITUTE_PATH) as f: params = get_torque_params(candidate)
sub = yaml.load(f, Loader=yaml.FullLoader)
if candidate in sub: tune.init('torque')
candidate = sub[candidate] tune.torque.useSteeringAngle = True
tune.torque.kp = 1.0 / params['LAT_ACCEL_FACTOR']
with open(TORQUE_PARAMS_PATH) as f: tune.torque.kf = 1.0 / params['LAT_ACCEL_FACTOR']
params = yaml.load(f, Loader=yaml.FullLoader) tune.torque.ki = 0.1 / params['LAT_ACCEL_FACTOR']
with open(TORQUE_OVERRIDE_PATH) as f: tune.torque.friction = params['FRICTION']
override = yaml.load(f, Loader=yaml.FullLoader) tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
# Ensure no overlap
if sum([candidate in x for x in [sub, params, override]]) > 1:
raise RuntimeError(f'{candidate} is defined twice in torque config')
if candidate in override:
out = override[candidate]
elif candidate in params:
out = params[candidate]
else:
raise NotImplementedError(f"Did not find torque params for {candidate}")
return {key:out[i] for i, key in enumerate(params['legend'])}
@abstractmethod @abstractmethod
def _update(self, c: car.CarControl) -> car.CarState: def _update(self, c: car.CarControl) -> car.CarState:

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

@ -2,7 +2,6 @@
from cereal import car from cereal import car
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from panda import Panda from panda import Panda
from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
@ -32,9 +31,8 @@ class CarInterface(CarInterfaceBase):
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
stop_and_go = False stop_and_go = False
torque_params = CarInterfaceBase.get_torque_params(candidate)
steering_angle_deadzone_deg = 0.0 steering_angle_deadzone_deg = 0.0
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
if candidate == CAR.PRIUS: if candidate == CAR.PRIUS:
stop_and_go = True stop_and_go = True
@ -46,7 +44,7 @@ class CarInterface(CarInterfaceBase):
for fw in car_fw: for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
steering_angle_deadzone_deg = 1.0 steering_angle_deadzone_deg = 1.0
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
elif candidate == CAR.PRIUS_V: elif candidate == CAR.PRIUS_V:
stop_and_go = True stop_and_go = True
@ -54,7 +52,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 17.4 ret.steerRatio = 17.4
tire_stiffness_factor = 0.5533 tire_stiffness_factor = 0.5533
ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'], steering_angle_deadzone_deg) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
elif candidate in (CAR.RAV4, CAR.RAV4H): elif candidate in (CAR.RAV4, CAR.RAV4H):
stop_and_go = True if (candidate in CAR.RAV4H) else False stop_and_go = True if (candidate in CAR.RAV4H) else False

6
selfdrive/car/toyota/tunes.py
Unescape View File

@ -1,6 +1,5 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from enum import Enum from enum import Enum
from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
class LongTunes(Enum): class LongTunes(Enum):
PEDAL = 0 PEDAL = 0
@ -24,7 +23,6 @@ class LatTunes(Enum):
PID_L = 13 PID_L = 13
PID_M = 14 PID_M = 14
PID_N = 15 PID_N = 15
TORQUE = 16
###### LONG ###### ###### LONG ######
@ -51,9 +49,7 @@ def set_long_tune(tune, name):
###### LAT ###### ###### LAT ######
def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True): def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
if name == LatTunes.TORQUE: if 'PID' in str(name):
set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION, steering_angle_deadzone_deg)
elif 'PID' in str(name):
tune.init('pid') tune.init('pid')
tune.pid.kiBP = [0.0] tune.pid.kiBP = [0.0]
tune.pid.kpBP = [0.0] tune.pid.kpBP = [0.0]

10
selfdrive/controls/lib/latcontrol_torque.py
Unescape View File

@ -22,16 +22,6 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
FRICTION_THRESHOLD = 0.2 FRICTION_THRESHOLD = 0.2
def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0):
tune.init('torque')
tune.torque.useSteeringAngle = True
tune.torque.kp = 1.0 / MAX_LAT_ACCEL
tune.torque.kf = 1.0 / MAX_LAT_ACCEL
tune.torque.ki = 0.1 / MAX_LAT_ACCEL
tune.torque.friction = FRICTION
tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
class LatControlTorque(LatControl): class LatControlTorque(LatControl):
def __init__(self, CP, CI): def __init__(self, CP, CI):
super().__init__(CP, CI) super().__init__(CP, CI)

Write Preview
Loading…
Cancel
Save