From b88eea00b5cd68abc85ef4610d08cb4d3cd7fd61 Mon Sep 17 00:00:00 2001
From: qadmus <42746943+qadmus@users.noreply.github.com>
Date: Thu, 21 Oct 2021 14:10:41 -0700
Subject: [PATCH] GM: improved Volt lateral tuning (#22621)

* GM Volt lateral tuning and feedforward function

* improve comments, lower P 5% to 0.17, which matches all my notes over time, 0.18 was a last test change.

* don't justify actuator delay for now

* update refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 2e0bc9d3653defb51e229d45e0c68447c51785a4
---
 selfdrive/car/gm/interface.py            | 28 ++++++++++++++++++++++--
 selfdrive/test/process_replay/ref_commit |  2 +-
 2 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index a689a78213..53692c25f5 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
+from math import fabs
 from selfdrive.config import Conversions as CV
 from selfdrive.car.gm.values import CAR, CruiseButtons, \
                                     AccState, CarControllerParams
@@ -15,6 +16,21 @@ class CarInterface(CarInterfaceBase):
     params = CarControllerParams()
     return params.ACCEL_MIN, params.ACCEL_MAX
 
+  # Volt determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
+  @staticmethod
+  def get_steer_feedforward_volt(desired_angle, v_ego):
+    # maps [-inf,inf] to [-1,1]: sigmoid(34.4 deg) = sigmoid(1) = 0.5
+    # 1 / 0.02904609 = 34.4 deg ~= 36 deg ~= 1/10 circle? Arbitrary?
+    desired_angle *= 0.02904609
+    sigmoid = desired_angle / (1 + fabs(desired_angle))
+    return 0.10006696 * sigmoid * (v_ego + 3.12485927)
+
+  def get_steer_feedforward_function(self):
+    if self.CP.carFingerprint in [CAR.VOLT]:
+      return self.get_steer_feedforward_volt
+    else:
+      return CarInterfaceBase.get_steer_feedforward_default
+
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
@@ -41,9 +57,17 @@ class CarInterface(CarInterfaceBase):
       ret.minEnableSpeed = 18 * CV.MPH_TO_MS
       ret.mass = 1607. + STD_CARGO_KG
       ret.wheelbase = 2.69
-      ret.steerRatio = 15.7
+      ret.steerRatio = 17.7  # Stock 15.7, LiveParameters
+      tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters
       ret.steerRatioRear = 0.
-      ret.centerToFront = ret.wheelbase * 0.4  # wild guess
+      ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh
+
+      ret.lateralTuning.pid.kpBP = [0., 40.]
+      ret.lateralTuning.pid.kpV = [0., 0.17]
+      ret.lateralTuning.pid.kiBP = [0.]
+      ret.lateralTuning.pid.kiV = [0.]
+      ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt()
+      ret.steerActuatorDelay = 0.2
 
     elif candidate == CAR.MALIBU:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 51dba73ceb..45e6b7d9f9 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-5522675909c1af529323735a02ae2186dbed898d
\ No newline at end of file
+d616c135e470264dbc40e77b25a4dab1fc449d75
\ No newline at end of file