GM LKAS enable & acceleration by @rdm128 and @Jamezz (commaai#176)

* Add accel override for GM and retune PID

* Droppin them mad GM tunes for 0.5.10 (commaai#148)

* Add retunes and acceleration/braking functions from jamezz unstable.

* Fix 2017 fingerprint?

* Reintroduce startAccel to compensate

* Try yanking calc_accel_override to fix slow restart

* Set bp for gasMax like honda

* GM only maps 0.5 as max gas. Update to be more clear

* Revert "Try yanking calc_accel_override to fix slow restart"

This reverts commit 2c6669d.

* Undo cruise following change

* I missed

* Go back to all gas all the time

* Adjust overall max acceleration override upwards

* Invert A_ACC_MAX usage. Should help with weak accel

* Revert "Fix 2017 fingerprint?"

This reverts commit 1065d69.

* Give GM some steroids and protein shakes (commaai#150)

* Add retunes and acceleration/braking functions from jamezz unstable.

* Fix 2017 fingerprint?

* Reintroduce startAccel to compensate

* Try yanking calc_accel_override to fix slow restart

* Set bp for gasMax like honda

* GM only maps 0.5 as max gas. Update to be more clear

* Revert "Try yanking calc_accel_override to fix slow restart"

This reverts commit 2c6669d.

* Undo cruise following change

* I missed

* Go back to all gas all the time

* Adjust overall max acceleration override upwards

* Invert A_ACC_MAX usage. Should help with weak accel

* Revert "Fix 2017 fingerprint?"

This reverts commit 1065d69.

* Halve A_ACC_MAX to try and improve acceleration

* Revert "Merge branch 'kegman-unstable-merge' of github.com:jamezz-comma/openpilot into kegman-unstable-merge"

This reverts commit a096bee, reversing
changes made to 1cbb79f.

* Rename variable

* Revert "Revert "Merge branch 'kegman-unstable-merge' of github.com:jamezz-comma/openpilot into kegman-unstable-merge""

This reverts commit b587657.

* Add GM LKAS Toggle

Toggle steering assist by pressing LKA button on steering wheel.  If you have a GM Giraffe, the LKAS LED turns on and off with steering assist.

* GM Friction Brakes Icon

Read and update brakeLights based on Chassis friction brake pressure.

* GM cleanup lkMode

selfdrive/car/gm/carstate.py

@@ -21,6 +21,7 @@ def get_powertrain_can_parser(CP, canbus):
     ("TurnSignals", "BCMTurnSignals", 0),
     ("AcceleratorPedal", "AcceleratorPedal", 0),
     ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS),
+    ("LKAButton", "ASCMSteeringButton", 0),
     ("SteeringWheelAngle", "PSCMSteeringAngle", 0),
     ("FLWheelSpd", "EBCMWheelSpdFront", 0),
     ("FRWheelSpd", "EBCMWheelSpdFront", 0),
@@ -49,6 +50,15 @@ def get_powertrain_can_parser(CP, canbus):
 
   return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
 
+def get_chassis_can_parser(CP, canbus):
+  # this function generates lists for signal, messages and initial values
+  signals = [
+      # sig_name, sig_address, default
+      ("FrictionBrakePressure", "EBCMFrictionBrakeStatus", 0),
+  ]
+
+  return CANParser(DBC[CP.carFingerprint]['chassis'], signals, [], canbus.chassis)
+
 class CarState(object):
   def __init__(self, CP, canbus):
     self.CP = CP
@@ -60,6 +70,13 @@ def __init__(self, CP, canbus):
     self.prev_left_blinker_on = False
     self.right_blinker_on = False
     self.prev_right_blinker_on = False
+    self.prev_distance_button = 0
+    self.prev_lka_button = 0
+    self.lka_button = 0
+    self.distance_button = 0
+    self.follow_level = 3
+    self.lkMode = True
+    self.frictionBrakesActive = False
 
     # vEgo kalman filter
     dt = 0.01
@@ -69,11 +86,15 @@ def __init__(self, CP, canbus):
                          K=np.matrix([[0.12287673], [0.29666309]]))
     self.v_ego = 0.
 
-  def update(self, pt_cp):
+  def update(self, pt_cp, ch_cp):
 
     self.can_valid = pt_cp.can_valid
     self.prev_cruise_buttons = self.cruise_buttons
     self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
+    self.prev_lka_button = self.lka_button
+    self.lka_button = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
+    self.prev_distance_button = self.distance_button
+    self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
 
     self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
     self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
@@ -148,3 +169,9 @@ def update(self, pt_cp):
     self.brake_pressed = self.user_brake > 10 or self.regen_pressed
 
     self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive
+
+    # Update Friction Brakes from Chassis Canbus
+    self.frictionBrakesActive = bool(ch_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakePressure"] != 0)
+
+  def get_follow_level(self):
+    return self.follow_level

selfdrive/car/gm/interface.py

@@ -1,17 +1,23 @@
 #!/usr/bin/env python
 from cereal import car, log
 from common.realtime import sec_since_boot
+from common.numpy_fast import clip, interp
 from selfdrive.config import Conversions as CV
+from common.numpy_fast import clip, interp
+from selfdrive.controls.lib.planner import A_ACC_MAX
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD
-from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
+from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
+from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser, get_chassis_can_parser
 
 try:
   from selfdrive.car.gm.carcontroller import CarController
 except ImportError:
   CarController = None
 
+FOLLOW_AGGRESSION = 0.15 # (Acceleration/Decel aggression) Lower is more aggressive
+
 class CanBus(object):
   def __init__(self):
     self.powertrain = 0
@@ -34,6 +40,7 @@ def __init__(self, CP, sendcan=None):
     self.CS = CarState(CP, canbus)
     self.VM = VehicleModel(CP)
     self.pt_cp = get_powertrain_can_parser(CP, canbus)
+    self.ch_cp = get_chassis_can_parser(CP, canbus)
     self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis']
 
     # sending if read only is False
@@ -43,11 +50,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.85, 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 / FOLLOW_AGGRESSION)) * min(speedLimiter, accelLimiter)
 
   @staticmethod
   def get_params(candidate, fingerprint):
@@ -162,16 +203,16 @@ def get_params(candidate, fingerprint):
     ret.steerMaxBP = [0.] # m/s
     ret.steerMaxV = [1.]
     ret.gasMaxBP = [0.]
-    ret.gasMaxV = [.5]
+    ret.gasMaxV = [0.5]
     ret.brakeMaxBP = [0.]
     ret.brakeMaxV = [1.]
     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
 
@@ -187,8 +228,9 @@ def get_params(candidate, fingerprint):
   # returns a car.CarState
   def update(self, c):
 
-    self.pt_cp.update(int(sec_since_boot() * 1e9), False)
-    self.CS.update(self.pt_cp)
+    self.pt_cp.update(int(sec_since_boot() * 1e9), True)
+    self.ch_cp.update(int(sec_since_boot() * 1e9), True)
+    self.CS.update(self.pt_cp, self.ch_cp)
 
     # create message
     ret = car.CarState.new_message()
@@ -211,7 +253,8 @@ def update(self, c):
     # brake pedal
     ret.brake = self.CS.user_brake / 0xd0
     ret.brakePressed = self.CS.brake_pressed
-
+    ret.brakeLights = self.CS.frictionBrakesActive
+
     # steering wheel
     ret.steeringAngle = self.CS.angle_steers
 
@@ -261,13 +304,23 @@ def update(self, c):
           be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
       elif but == CruiseButtons.DECEL_SET:
         be.type = 'decelCruise'
+        if not cruiseEnabled and not self.CS.lkMode:
+          self.lkMode = True
       elif but == CruiseButtons.CANCEL:
         be.type = 'cancel'
       elif but == CruiseButtons.MAIN:
         be.type = 'altButton3'
       buttonEvents.append(be)
 
     ret.buttonEvents = buttonEvents
+
+    if cruiseEnabled and self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button:
+      self.CS.lkMode = not self.CS.lkMode
+
+    if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button:
+       self.CS.follow_level -= 1
+       if self.CS.follow_level < 1:
+         self.CS.follow_level = 3
 
     events = []
     if not self.CS.can_valid:
@@ -276,6 +329,8 @@ def update(self, c):
         events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     else:
       self.can_invalid_count = 0
+    if cruiseEnabled and (self.CS.left_blinker_on or self.CS.right_blinker_on):
+       events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
     if self.CS.steer_error:
       events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
     if self.CS.steer_not_allowed: