diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 3f5938149a0dc0..ddad96a94edd4f 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -7,7 +7,6 @@ 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 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 @@ -47,7 +46,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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 @@ -62,7 +60,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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 @@ -92,13 +90,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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 @@ -200,7 +198,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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 @@ -240,7 +238,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 3bd75080401bb1..7786df453ae42a 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -19,11 +19,36 @@ 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 @@ -85,7 +110,7 @@ def get_std_params(candidate, fingerprint): 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 @@ def get_std_params(candidate, fingerprint): 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: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 693a84cb959426..0eb4865483fbe3 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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, 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 @@ -29,9 +28,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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 @@ -43,7 +41,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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 @@ -51,7 +49,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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 diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index 3de6daae21b468..a8b8758d893344 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -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] diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f72ffc4b88ea7b..014c3480b80254 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -22,16 +22,6 @@ 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)