GM: LKA dashboard icon support (#389)

* GM: LKA dashboard icon support

* Decrease camera keepalive interval

* Use tuple for LKA icon status

selfdrive/car/gm/carcontroller.py

@@ -26,7 +26,11 @@ class CarControllerParams():
     self.STEER_DRIVER_FACTOR = 100     # from dbc
     self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop
 
-    self.ADAS_KEEPALIVE_STEP = 10
+    # Takes case of "Service Adaptive Cruise" and "Service Front Camera"
+    # dashboard messages.
+    self.ADAS_KEEPALIVE_STEP = 100
+    self.CAMERA_KEEPALIVE_STEP = 100
+
     # pedal lookups, only for Volt
     MAX_GAS = 3072              # Only a safety limit
     ZERO_GAS = 2048
@@ -59,12 +63,11 @@ class CarController(object):
     self.pedal_steady = 0.
     self.start_time = sec_since_boot()
     self.chime = 0
-    self.lkas_active = False
-    self.inhibit_steer_for = 0
     self.steer_idx = 0
     self.apply_steer_last = 0
     self.car_fingerprint = car_fingerprint
     self.allow_controls = allow_controls
+    self.lka_icon_status_last = (False, False)
 
     # Setup detection helper. Routes commands to
     # an appropriate CAN bus number.
@@ -158,10 +161,21 @@ class CarController(object):
         can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx))
         can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx))
 
-      # Send ADAS keepalive, 10hz
       if frame % P.ADAS_KEEPALIVE_STEP == 0:
         can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
 
+    # Show green icon when LKA torque is applied, and
+    # alarming orange icon when approaching torque limit.
+    # If not sent again, LKA icon disappears in about 5 seconds.
+    # Conveniently, sending camera message periodically also works as a keepalive.
+    lka_active = CS.lkas_status == 1
+    lka_critical = lka_active and abs(actuators.steer) > 0.9
+    lka_icon_status = (lka_active, lka_critical)
+    if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
+        or lka_icon_status != self.lka_icon_status_last:
+      can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
+      self.lka_icon_status_last = lka_icon_status
+
     # Send chimes
     if self.chime != chime:
       duration = 0x3c

selfdrive/car/gm/gmcan.py

@@ -134,6 +134,16 @@ def create_chime_command(bus, chime_type, duration, repeat_cnt):
   dat = [chime_type, duration, repeat_cnt, 0xff, 0]
   return [0x10400060, 0, "".join(map(chr, dat)), bus]
 
+def create_lka_icon_command(bus, active, critical):
+  if active:
+    if critical:
+      dat = "\x40\xc0\x14"
+    else:
+      dat = "\x40\x40\x18"
+  else:
+    dat = "\x00\x00\x00"
+  return [0x104c006c, 0, dat, bus]
+
 # TODO: WIP
 '''
 def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, at_full_stop):