Merge branch 'devel' of https://github.com/commaai/openpilot into devel-lexus-ish-ave30r

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):

selfdrive/car/hyundai/carstate.py

@@ -44,6 +44,11 @@ def get_can_parser(CP):
     ("CF_Clu_AmpInfo", "CLU11", 0),
     ("CF_Clu_AliveCnt1", "CLU11", 0),
 
+    ("CF_Clu_InhibitD", "CLU15", 0),
+    ("CF_Clu_InhibitP", "CLU15", 0),
+    ("CF_Clu_InhibitN", "CLU15", 0),
+    ("CF_Clu_InhibitR", "CLU15", 0),
+
     ("CF_Lvr_Gear","LVR12",0),
 
     ("ACCEnable", "TCS13", 0),
@@ -204,7 +209,7 @@ class CarState(object):
       self.pedal_gas = cp.vl["EMS12"]['TPS']
     self.car_gas = cp.vl["EMS12"]['TPS']
 
-    # Gear Selecton - This should be compatible with all Kia/Hyundai with Auto's
+    # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
     gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
     if gear == 5:
       self.gear_shifter = "drive"
@@ -217,6 +222,18 @@ class CarState(object):
     else:
       self.gear_shifter = "unknown"
 
+    # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
+    if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
+      self.gear_shifter_cluster = "drive"
+    elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
+      self.gear_shifter_cluster = "neutral"
+    elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
+      self.gear_shifter_cluster = "park"
+    elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
+      self.gear_shifter_cluster = "reverse"
+    else:
+      self.gear_shifter_cluster = "unknown"
+
     # save the entire LKAS11 and CLU11
     self.lkas11 = cp_cam.vl["LKAS11"]
     self.clu11 = cp.vl["CLU11"]

selfdrive/car/hyundai/interface.py

@@ -95,14 +95,15 @@ class CarInterface(object):
       ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
       ret.minSteerSpeed = 0.
     elif candidate == CAR.ELANTRA:
-      ret.steerKf = 0.00004
+      ret.steerKf = 0.00006
       ret.steerRateCost = 0.5
       ret.mass = 1275 + std_cargo
       ret.wheelbase = 2.7
-      ret.steerRatio = 16.9
+      ret.steerRatio = 13.73   #Spec
+      tire_stiffness_factor = 0.385
       ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
-      ret.steerKpV, ret.steerKiV = [[0.20], [0.01]]
-      ret.minSteerSpeed = 35 * CV.MPH_TO_MS
+      ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
+      ret.minSteerSpeed = 32 * CV.MPH_TO_MS
     elif candidate == CAR.GENESIS:
       ret.steerKf = 0.00005
       ret.steerRateCost = 0.5
@@ -190,7 +191,10 @@ class CarInterface(object):
     ret.wheelSpeeds.rr = self.CS.v_wheel_rr
 
     # gear shifter
-    ret.gearShifter = self.CS.gear_shifter
+    if self.CP.carFingerprint == CAR.ELANTRA:
+      ret.gearShifter = self.CS.gear_shifter_cluster
+    else:
+      ret.gearShifter = self.CS.gear_shifter
 
     # gas pedal
     ret.gas = self.CS.car_gas