diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index bc6c31e12c..87720f8754 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -2,24 +2,27 @@ import yaml import os import time from abc import abstractmethod, ABC -from typing import Any, Dict, Optional, Tuple, List +from typing import Any, Dict, Optional, Tuple, List, Callable from cereal import car from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D +from common.numpy_fast import interp from common.realtime import DT_CTRL from selfdrive.car import apply_hysteresis, create_button_enable_events, gen_empty_fingerprint -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_deadzone from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel 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 ACCEL_MIN = -3.5 +FRICTION_THRESHOLD = 0.2 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') @@ -101,6 +104,20 @@ class CarInterfaceBase(ABC): def get_steer_feedforward_function(self): return self.get_steer_feedforward_default + @staticmethod + def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, friction_compensation): + # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) + friction_interp = interp( + apply_deadzone(lateral_accel_error, lateral_accel_deadzone), + [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], + [-torque_params.friction, torque_params.friction] + ) + friction = friction_interp if friction_compensation else 0.0 + return (lateral_accel_value / torque_params.latAccelFactor) + friction + + def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: + return self.torque_from_lateral_accel_linear + # returns a set of default params to avoid repetition in car specific params @staticmethod def get_std_params(candidate, fingerprint): @@ -144,11 +161,12 @@ class CarInterfaceBase(ABC): tune.init('torque') tune.torque.useSteeringAngle = use_steering_angle - 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.kp = 1.0 + tune.torque.kf = 1.0 + tune.torque.ki = 0.1 tune.torque.friction = params['FRICTION'] - tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg + tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] + tune.torque.latAccelOffset = 0.0 @abstractmethod def _update(self, c: car.CarControl) -> car.CarState: diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c4604d90e1..f65a58275b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,7 +4,6 @@ from cereal import log from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from selfdrive.controls.lib.pid import PIDController -from selfdrive.controls.lib.drive_helpers import apply_deadzone from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # At higher speeds (25+mph) we can assume: @@ -19,19 +18,20 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # move it at all, this is compensated for too. -FRICTION_THRESHOLD = 0.2 - - class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) - self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, - k_f=CP.lateralTuning.torque.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) - self.get_steer_feedforward = CI.get_steer_feedforward_function() - self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle - self.friction = CP.lateralTuning.torque.friction - self.kf = CP.lateralTuning.torque.kf - self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg + self.torque_params = CP.lateralTuning.torque + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, + k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.torque_from_lateral_accel = CI.torque_from_lateral_accel() + self.use_steering_angle = self.torque_params.useSteeringAngle + self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg + + def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): + self.torque_params.latAccelFactor = latAccelFactor + self.torque_params.latAccelOffset = latAccelOffset + self.torque_params.friction = friction def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() @@ -55,19 +55,16 @@ class LatControlTorque(LatControl): actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200]) setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature error = setpoint - measurement - pid_log.error = error + gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY + pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error, lateral_accel_deadzone, friction_compensation=False) + ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, error, lateral_accel_deadzone, friction_compensation=True) - ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY - # convert friction into lateral accel units for feedforward - friction_compensation = interp(apply_deadzone(error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction]) - ff += friction_compensation / self.kf freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 - output_torque = self.pid.update(error, + output_torque = self.pid.update(pid_log.error, feedforward=ff, speed=CS.vEgo, freeze_integrator=freeze_integrator) @@ -78,9 +75,9 @@ class LatControlTorque(LatControl): pid_log.d = self.pid.d pid_log.f = self.pid.f pid_log.output = -output_torque - pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited) pid_log.actualLateralAccel = actual_lateral_accel pid_log.desiredLateralAccel = desired_lateral_accel + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited) # TODO left is positive in this convention return -output_torque, 0.0, pid_log diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index bca5d0c3eb..afde6ec422 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -a9a25795f5d8202f7f4c137f80ae030e790ff974 \ No newline at end of file +d14f1a61a4bfde810128a6bb703aa543268fa4a9 \ No newline at end of file