diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 8bfc067b48..9a165cf067 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,12 +1,15 @@ #!/usr/bin/env python3 +import numpy as np from cereal import car from math import fabs from panda import Panda +from common.numpy_fast import interp from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR -from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD +from selfdrive.controls.lib.drive_helpers import apply_center_deadzone ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -43,6 +46,43 @@ class CarInterface(CarInterfaceBase): else: return CarInterfaceBase.get_steer_feedforward_default + @staticmethod + def torque_from_lateral_accel_bolt(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, vego, friction_compensation): + friction_interp = interp( + apply_center_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 + steer_torque = lateral_accel_value / torque_params.latAccelFactor + + # TODO: + # 1. Learn the correction factors from data + # 2. Generalize the logic to other GM torque control platforms + steer_break_pts = np.array([-1.0, -0.9, -0.75, -0.5, 0.0, 0.5, 0.75, 0.9, 1.0]) + steer_lataccel_factors = np.array([1.5, 1.15, 1.02, 1.0, 1.0, 1.0, 1.02, 1.15, 1.5]) + steer_correction_factor = np.interp( + steer_torque, + steer_break_pts, + steer_lataccel_factors + ) + + vego_break_pts = np.array([0.0, 10.0, 15.0, 20.0, 100.0]) + vego_lataccel_factors = np.array([1.5, 1.5, 1.25, 1.0, 1.0]) + vego_correction_factor = np.interp( + vego, + vego_break_pts, + vego_lataccel_factors, + ) + + return float((steer_torque + friction) / (steer_correction_factor * vego_correction_factor)) + + def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: + if self.CP.carFingerprint == CAR.BOLT_EUV: + return self.torque_from_lateral_accel_bolt + else: + return self.torque_from_lateral_accel_linear + @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "gm" @@ -176,7 +216,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.8 ret.centerToFront = 2.15 # measured tire_stiffness_factor = 1.0 - ret.steerActuatorDelay = 0.2 + ret.steerActuatorDelay = 0.12 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.SILVERADO: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 249818369c..f1e2081d05 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -18,7 +18,7 @@ from 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] +TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, float, bool], float] MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 @@ -131,7 +131,7 @@ class CarInterfaceBase(ABC): 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): + def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, vego, friction_compensation): # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) friction_interp = interp( apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2f56094379..9129693e5a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -64,10 +64,10 @@ class LatControlTorque(LatControl): error = setpoint - measurement 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) + lateral_accel_deadzone, CS.vEgo, friction_compensation=False) 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) + lateral_accel_deadzone, CS.vEgo, friction_compensation=True) freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 85c7d8e80b..ed1875dcbe 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f4efbb65a7eb9a8f4e23492372e707674f80114e \ No newline at end of file +3e53ce81f1ce26409fdc4479e650ef5626130876 \ No newline at end of file