Chevrolet Bolt: Non-linear torque tune

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 @@ def get_steer_feedforward_function(self):
     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 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long):
       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:

selfdrive/car/interfaces.py

@@ -18,7 +18,7 @@
 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 @@ 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):
+  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),

selfdrive/controls/lib/latcontrol_torque.py

@@ -64,10 +64,10 @@ def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_
       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,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-f4efbb65a7eb9a8f4e23492372e707674f80114e
+3e53ce81f1ce26409fdc4479e650ef5626130876