diff --git a/selfdrive/car/data_structures.py b/selfdrive/car/data_structures.py index ef443907a7..aedd1d16ff 100644 --- a/selfdrive/car/data_structures.py +++ b/selfdrive/car/data_structures.py @@ -100,11 +100,6 @@ class CarParams: longitudinalTuning: 'CarParams.LongitudinalPIDTuning' = field(default_factory=lambda: CarParams.LongitudinalPIDTuning()) lateralParams: 'CarParams.LateralParams' = field(default_factory=lambda: CarParams.LateralParams()) - # lateralTuning: 'CarParams.LateralPIDTuning | CarParams.LateralTorqueTuning' = field(default_factory=lambda: CarParams.LateralPIDTuning()) - # lateralTuningWhich: type['CarParams.LateralPIDTuning | CarParams.LateralTorqueTuning'] = field(default_factory=lambda: CarParams.LateralPIDTuning) - # lateralPIDTuning: 'CarParams.LateralPIDTuning' = field(default_factory=lambda: CarParams.LateralPIDTuning()) - # lateralTorqueTuning: 'CarParams.LateralTorqueTuning' = field(default_factory=lambda: CarParams.LateralTorqueTuning()) - lateralTuning: 'CarParams.LateralTuning' = field(default_factory=lambda: CarParams.LateralTuning()) @dataclass @@ -232,60 +227,3 @@ class CarParams: programmedFuelInjection = auto() debug = auto() - - -import typing - -# CP = CarParams() -# -# print(CP.lateralTuningWhich is CarParams.LateralPIDTuning) -# print(CP.lateralTuningWhich is CarParams.LateralTorqueTuning) -# print() -# CP.lateralTuningWhich = CarParams.LateralTorqueTuning -# print(CP.lateralTuningWhich is CarParams.LateralPIDTuning) -# print(CP.lateralTuningWhich is CarParams.LateralTorqueTuning) - -# -# -# T = typing.TypeVar('T') -# -# @dataclass -# class UnionField(typing.Generic[T]): -# value: T -# -# def init(self, value: T): -# self.value = value -# -# def which(self): -# return type(self.value).__name__ -# -# -# lateral_tuning: UnionField[CarParams.LateralTorqueTuning | CarParams.LateralPIDTuning] = UnionField(CarParams.LateralPIDTuning) -# -# lateral_tuning.init(CarParams().LateralPIDTuning()) -# if isinstance(lateral_tuning.value, CarParams.LateralTorqueTuning): -# print(lateral_tuning.value.useSteeringAngle) # This will work without mypy errors - -T = typing.TypeVar('T') - - -# def which(value: object, typ: typing.Type[T]) -> bool: -# return isinstance(value, typ) - -which = isinstance - - -lateralTuning: CarParams.LateralPIDTuning | CarParams.LateralTorqueTuning = field(default_factory=lambda: CarParams.LateralPIDTuning()) - -if isinstance(lateralTuning, CarParams.LateralTorqueTuning): - lateralTuning.useSteeringAngle = True - - - -# @dataclass -# @apply_auto_fields -# class UnionDataclass: -# which: str = 'pid' -# -# pid: CarParams.LateralPIDTuning = auto_field() -# torque: CarParams.LateralTorqueTuning = auto_field() diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 8c99887251..5546ad453c 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -69,8 +69,8 @@ class CarInterface(CarInterfaceBase): # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]] - ret.lateralTuning.kiBP, ret.lateralTuning.kpBP = [[0.], [0.]] - ret.lateralTuning.kf = 0.00006 # conservative feed-forward + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward if candidate in HONDA_BOSCH: ret.longitudinalActuatorDelay = 0.5 # s @@ -95,30 +95,30 @@ class CarInterface(CarInterfaceBase): # modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480 # note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.3], [0.1]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[1.1], [0.33]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CIVIC_2022): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.8], [0.24]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate == CAR.HONDA_ACCORD: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end if eps_modified: - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.3], [0.09]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] else: - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.6], [0.18]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.ACURA_ILX: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.8], [0.24]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate in (CAR.HONDA_CRV, CAR.HONDA_CRV_EU): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.8], [0.24]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.wheelSpeedFactor = 1.025 elif candidate == CAR.HONDA_CRV_5G: @@ -127,43 +127,43 @@ class CarInterface(CarInterfaceBase): # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.21], [0.07]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] else: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.64], [0.192]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] ret.wheelSpeedFactor = 1.025 elif candidate == CAR.HONDA_CRV_HYBRID: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.6], [0.18]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.wheelSpeedFactor = 1.025 elif candidate == CAR.HONDA_FIT: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.2], [0.05]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] elif candidate == CAR.HONDA_FREED: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.2], [0.05]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] elif candidate in (CAR.HONDA_HRV, CAR.HONDA_HRV_3G): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] if candidate == CAR.HONDA_HRV: - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.16], [0.025]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] ret.wheelSpeedFactor = 1.025 else: - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning elif candidate == CAR.ACURA_RDX: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.8], [0.24]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate == CAR.ACURA_RDX_3G: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.2], [0.06]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] elif candidate in (CAR.HONDA_ODYSSEY, CAR.HONDA_ODYSSEY_CHN): - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.28], [0.08]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] if candidate == CAR.HONDA_ODYSSEY_CHN: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end else: @@ -171,19 +171,19 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.HONDA_PILOT: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.38], [0.11]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.HONDA_RIDGELINE: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.38], [0.11]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.HONDA_INSIGHT: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.6], [0.18]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.HONDA_E: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.lateralTuning.kpV, ret.lateralTuning.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning else: raise ValueError(f"unsupported car {candidate}") diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 53d5100a4f..5db51d2d87 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -212,20 +212,18 @@ class CarInterfaceBase(ABC): return ret @staticmethod - def configure_torque_tune(candidate: str, ret: CarParams, steering_angle_deadzone_deg: float = 0.0, use_steering_angle: bool = True): + def configure_torque_tune(candidate: str, tune: CarParams.LateralTuning, steering_angle_deadzone_deg: float = 0.0, use_steering_angle: bool = True): params = get_torque_params()[candidate] - tune = CarParams.LateralTorqueTuning() - tune.useSteeringAngle = use_steering_angle - tune.kp = 1.0 - tune.kf = 1.0 - tune.ki = 0.1 - tune.friction = params['FRICTION'] - tune.latAccelFactor = params['LAT_ACCEL_FACTOR'] - tune.latAccelOffset = 0.0 - tune.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg - - ret.lateralTuning = tune + tune.init('torque') + tune.torque.useSteeringAngle = use_steering_angle + tune.torque.kp = 1.0 + tune.torque.kf = 1.0 + tune.torque.ki = 0.1 + tune.torque.friction = params['FRICTION'] + tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] + tune.torque.latAccelOffset = 0.0 + tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg @abstractmethod def _update(self, c: car.CarControl) -> car.CarState: