diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index af34100d74..e0f616dbf9 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -7,7 +7,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus -from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD +from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs from openpilot.selfdrive.controls.lib.drive_helpers import get_friction ButtonType = car.CarState.ButtonEvent.Type @@ -44,8 +44,8 @@ class CarInterface(CarInterfaceBase): else: return CarInterfaceBase.get_steer_feedforward_default - def torque_from_lateral_accel_siglin(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, - lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float: + def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs, torque_params: car.CarParams.LateralTorqueTuning, lateral_accel_error: float, + lateral_accel_deadzone: float, friction_compensation: bool, gravity_adjusted: bool) -> float: friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) def sig(val): @@ -58,7 +58,7 @@ class CarInterface(CarInterfaceBase): non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint) assert non_linear_torque_params, "The params are not defined" a, b, c, _ = non_linear_torque_params - steer_torque = (sig(lateral_accel_value * a) * b) + (lateral_accel_value * c) + steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c) return float(steer_torque) + friction def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 59657c5437..7ecc47f2a4 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -4,7 +4,7 @@ import numpy as np import tomllib from abc import abstractmethod, ABC from enum import StrEnum -from typing import Any, Dict, Optional, Tuple, List, Callable +from typing import Any, Dict, Optional, Tuple, List, Callable, NamedTuple from cereal import car from openpilot.common.basedir import BASEDIR @@ -20,7 +20,6 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName -TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, bool], float] MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 @@ -32,6 +31,16 @@ TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.toml') +class LatControlInputs(NamedTuple): + lateral_acceleration: float + roll_compensation: float + vego: float + aego: float + + +TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, car.CarParams.LateralTorqueTuning, float, float, bool, bool], float] + + def get_torque_params(candidate): with open(TORQUE_SUBSTITUTE_PATH, 'rb') as f: sub = tomllib.load(f) @@ -130,11 +139,11 @@ class CarInterfaceBase(ABC): def get_steer_feedforward_function(self): return self.get_steer_feedforward_default - def torque_from_lateral_accel_linear(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, - lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float: + def torque_from_lateral_accel_linear(self, latcontrol_inputs: LatControlInputs, torque_params: car.CarParams.LateralTorqueTuning, + lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool, gravity_adjusted: bool) -> float: # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) - return (lateral_accel_value / float(torque_params.latAccelFactor)) + friction + return (latcontrol_inputs.lateral_acceleration / float(torque_params.latAccelFactor)) + friction def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: return self.torque_from_lateral_accel_linear diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 65fd1b51c5..34b0d47124 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -2,6 +2,7 @@ import math from cereal import log from openpilot.common.numpy_fast import interp +from openpilot.selfdrive.car.interfaces import LatControlInputs from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY @@ -38,16 +39,16 @@ class LatControlTorque(LatControl): def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() - if not active: output_torque = 0.0 pid_log.active = False else: + actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY if self.use_steering_angle: - actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + actual_curvature = actual_curvature_vm curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) else: - actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) curvature_deadzone = 0.0 @@ -61,15 +62,15 @@ class LatControlTorque(LatControl): low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY - torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.torque_params, setpoint, - lateral_accel_deadzone, friction_compensation=False) - torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params, measurement, - lateral_accel_deadzone, friction_compensation=False) + gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation + torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, + setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False) + torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, + measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False) pid_log.error = torque_from_setpoint - torque_from_measurement - ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, - desired_lateral_accel - actual_lateral_accel, - lateral_accel_deadzone, friction_compensation=True) + ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, + desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True, + gravity_adjusted=True) freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error,