sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
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
old-commit-hash: 056b330e8b
chrysler-long2
Vivek Aithal 1 year ago committed by GitHub
parent 55f88260d1
commit 80195f1432
3 changed files with 30 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      selfdrive/car/gm/interface.py
  2. 19
      selfdrive/car/interfaces.py
  3. 23
      selfdrive/controls/lib/latcontrol_torque.py

8
selfdrive/car/gm/interface.py
Unescape 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 import create_button_events, get_safety_config
from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG 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.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 from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
ButtonType = car.CarState.ButtonEvent.Type ButtonType = car.CarState.ButtonEvent.Type
@ -44,8 +44,8 @@ class CarInterface(CarInterfaceBase):
else: else:
return CarInterfaceBase.get_steer_feedforward_default return CarInterfaceBase.get_steer_feedforward_default
def torque_from_lateral_accel_siglin(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs, torque_params: car.CarParams.LateralTorqueTuning, lateral_accel_error: float,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> 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) friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
def sig(val): def sig(val):
@ -58,7 +58,7 @@ class CarInterface(CarInterfaceBase):
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint) non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined" assert non_linear_torque_params, "The params are not defined"
a, b, c, _ = non_linear_torque_params 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 return float(steer_torque) + friction
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:

19
selfdrive/car/interfaces.py
Unescape View File

@ -4,7 +4,7 @@ import numpy as np
import tomllib import tomllib
from abc import abstractmethod, ABC from abc import abstractmethod, ABC
from enum import StrEnum 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 cereal import car
from openpilot.common.basedir import BASEDIR from openpilot.common.basedir import BASEDIR
@ -20,7 +20,6 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
ButtonType = car.CarState.ButtonEvent.Type ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter GearShifter = car.CarState.GearShifter
EventName = car.CarEvent.EventName 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 MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
ACCEL_MAX = 2.0 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') 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): def get_torque_params(candidate):
with open(TORQUE_SUBSTITUTE_PATH, 'rb') as f: with open(TORQUE_SUBSTITUTE_PATH, 'rb') as f:
sub = tomllib.load(f) sub = tomllib.load(f)
@ -130,11 +139,11 @@ class CarInterfaceBase(ABC):
def get_steer_feedforward_function(self): def get_steer_feedforward_function(self):
return self.get_steer_feedforward_default return self.get_steer_feedforward_default
def torque_from_lateral_accel_linear(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, 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) -> float: 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) # 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) 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: def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
return self.torque_from_lateral_accel_linear return self.torque_from_lateral_accel_linear

23
selfdrive/controls/lib/latcontrol_torque.py
Unescape View File

@ -2,6 +2,7 @@ import math
from cereal import log from cereal import log
from openpilot.common.numpy_fast import interp 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.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY 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): def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
pid_log = log.ControlsState.LateralTorqueState.new_message() pid_log = log.ControlsState.LateralTorqueState.new_message()
if not active: if not active:
output_torque = 0.0 output_torque = 0.0
pid_log.active = False pid_log.active = False
else: 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: 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)) curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
else: 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_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
curvature_deadzone = 0.0 curvature_deadzone = 0.0
@ -61,15 +62,15 @@ class LatControlTorque(LatControl):
low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature
gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.torque_params, setpoint, torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
lateral_accel_deadzone, friction_compensation=False) setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params, measurement, torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
lateral_accel_deadzone, friction_compensation=False) measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
pid_log.error = torque_from_setpoint - torque_from_measurement pid_log.error = torque_from_setpoint - torque_from_measurement
ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, 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, desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
lateral_accel_deadzone, friction_compensation=True) gravity_adjusted=True)
freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(pid_log.error, output_torque = self.pid.update(pid_log.error,

Write Preview
Loading…
Cancel
Save