From 7ff0554856769e458a4af422cd7a64a1d1620baf Mon Sep 17 00:00:00 2001
From: Thomas Helms <di.jamezz@gmail.com>
Date: Tue, 2 Apr 2019 14:17:31 -0700
Subject: [PATCH] Add retunes and acceleration/braking functions from jamezz
 unstable.

---
 selfdrive/car/gm/interface.py | 52 ++++++++++++++++++++++++++++++-----
 1 file changed, 45 insertions(+), 7 deletions(-)

diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 379819abf9cd7d..748caa4e6b92db 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -1,9 +1,11 @@
 #!/usr/bin/env python
 from cereal import car
 from common.realtime import sec_since_boot
+from common.numpy_fast import clip, interp
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
 from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, SUPERCRUISE_CARS
 from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser, get_chassis_can_parser
 
@@ -12,6 +14,8 @@
 except ImportError:
   CarController = None
 
+A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
+
 class CanBus(object):
   def __init__(self):
     self.powertrain = 0
@@ -44,11 +48,45 @@ def __init__(self, CP, sendcan=None):
 
   @staticmethod
   def compute_gb(accel, speed):
-    return float(accel) / 4.0
+  	# Ripped from compute_gb_honda in Honda's interface.py. Works well off shelf but may need more tuning
+    creep_brake = 0.0
+    creep_speed = 2.68
+    creep_brake_value = 0.10
+    if speed < creep_speed:
+      creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
+    return float(accel) / 4.8 - creep_brake
 
   @staticmethod
   def calc_accel_override(a_ego, a_target, v_ego, v_target):
-    return 1.0
+
+    # normalized max accel. Allowing max accel at low speed causes speed overshoots
+    max_accel_bp = [10, 20]    # m/s
+    max_accel_v = [0.714, 1.0] # unit of max accel
+    max_accel = interp(v_ego, max_accel_bp, max_accel_v)
+
+    # limit the pcm accel cmd if:
+    # - v_ego exceeds v_target, or
+    # - a_ego exceeds a_target and v_ego is close to v_target
+
+    eA = a_ego - a_target
+    valuesA = [1.0, 0.1]
+    bpA = [0.3, 1.1]
+
+    eV = v_ego - v_target
+    valuesV = [1.0, 0.1]
+    bpV = [0.0, 0.5]
+
+    valuesRangeV = [1., 0.]
+    bpRangeV = [-1., 0.]
+
+    # only limit if v_ego is close to v_target
+    speedLimiter = interp(eV, bpV, valuesV)
+    accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV))
+
+    # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
+    # unless aTargetMax is very high and then we scale with it; this help in quicker restart
+
+    return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
 
   @staticmethod
   def get_params(candidate, fingerprint):
@@ -178,15 +216,15 @@ def get_params(candidate, fingerprint):
     ret.longPidDeadzoneBP = [0.]
     ret.longPidDeadzoneV = [0.]
 
-    ret.longitudinalKpBP = [5., 35.]
-    ret.longitudinalKpV = [2.4, 1.5]
-    ret.longitudinalKiBP = [0.]
-    ret.longitudinalKiV = [0.36]
+    ret.longitudinalKpBP = [0., 5., 35.]
+    ret.longitudinalKpV = [3.3, 2.425, 2.2]
+    ret.longitudinalKiBP = [0., 35.]
+    ret.longitudinalKiV = [0.18, 0.36]
 
     ret.steerLimitAlert = True
 
     ret.stoppingControl = True
-    ret.startAccel = 0.8
+    ret.startAccel = 0.0
 
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
     ret.steerRateCost = 1.0