mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-22 22:23:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

cleanup torque tuning config (#24951)

Browse Source
This commit is contained in:
Adeeb Shihadeh
2022-06-24 13:01:49 -07:00
committed by GitHub
parent e26db5dc91
commit 062a8bcdbd
5 changed files with 44 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
selfdrive/car/hyundai/interface.py
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:

57
selfdrive/car/interfaces.py
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]
def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0):
params = get_torque_params(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'])}
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
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
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
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)