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
Split 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.interfaces import CarInterfaceBase
from selfdrive.car.disable_ecu import disable_ecu
from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
@ -51,7 +50,6 @@ class CarInterface(CarInterfaceBase):
ret.stopAccel = 0.0
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):
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
@ -66,7 +64,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.84
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
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:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 4497. * CV.LB_TO_KG
@ -96,13 +94,13 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.72
ret.steerRatio = 12.9
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:
ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
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:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
@ -204,7 +202,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.80
ret.steerRatio = 13.75
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:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825. + STD_CARGO_KG
@ -244,7 +242,7 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),
get_safety_config(car.CarParams.SafetyModel.hyundaiHDA2)]
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
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
ACCEL_MAX = 2.0
ACCEL_MIN = -3.5
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_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
class CarInterfaceBase(ABC):
@ -85,7 +110,7 @@ class CarInterfaceBase(ABC):
ret.steerControlType = car.CarParams.SteerControlType.torque
ret.minSteerSpeed = 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.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
@ -110,28 +135,16 @@ class CarInterfaceBase(ABC):
return ret
@staticmethod
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'])}
def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0):
params = get_torque_params(candidate)
tune.init('torque')
tune.torque.useSteeringAngle = True
tune.torque.kp = 1.0 / params['LAT_ACCEL_FACTOR']
tune.torque.kf = 1.0 / params['LAT_ACCEL_FACTOR']
tune.torque.ki = 0.1 / params['LAT_ACCEL_FACTOR']
tune.torque.friction = params['FRICTION']
tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
@abstractmethod
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 common.conversions import Conversions as CV
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.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
@ -32,9 +31,8 @@ class CarInterface(CarInterfaceBase):
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
stop_and_go = False
torque_params = CarInterfaceBase.get_torque_params(candidate)
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:
stop_and_go = True
@ -46,7 +44,7 @@ class CarInterface(CarInterfaceBase):
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
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:
stop_and_go = True
@ -54,7 +52,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 17.4
tire_stiffness_factor = 0.5533
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):
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
from enum import Enum
from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
class LongTunes(Enum):
PEDAL = 0
@ -24,7 +23,6 @@ class LatTunes(Enum):
PID_L = 13
PID_M = 14
PID_N = 15
TORQUE = 16
###### LONG ######
@ -51,9 +49,7 @@ def set_long_tune(tune, name):
###### 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):
if name == LatTunes.TORQUE:
set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION, steering_angle_deadzone_deg)
elif 'PID' in str(name):
if 'PID' in str(name):
tune.init('pid')
tune.pid.kiBP = [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
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):
def __init__(self, CP, CI):
super().__init__(CP, CI)

Write Preview
Loading…
Cancel
Save