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

LatControlTorque: Add more inputs (#31252)

Browse Source 
* add history and state to the ff inputs

* add history

* resolve comments

* remove history, simplify

* don't compute lateral accel, roll comp always
This commit is contained in:
Vivek Aithal
2024-02-01 04:12:48 -08:00
committed by GitHub
parent f0b6f48948
commit 056b330e8b
3 changed files with 30 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/gm/interface.py
+4 -4
View File
@@ -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:
selfdrive/car/interfaces.py
+14 -5
View File
@@ -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
selfdrive/controls/lib/latcontrol_torque.py
+12 -11
View File
@@ -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,