Better Hyundai and Genesis Support, Better future model scaling

selfdrive/car/hyundai/carcontroller.py

@@ -3,7 +3,7 @@
 from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \
                                              create_1191, create_1156, \
                                              create_clu11
-from selfdrive.car.hyundai.values import Buttons
+from selfdrive.car.hyundai.values import Buttons, CAR, LKAS_FEATURES
 from selfdrive.can.packer import CANPacker
 
 

selfdrive/car/hyundai/carstate.py

@@ -16,8 +16,6 @@ def get_can_parser(CP):
 
     ("YAW_RATE", "ESP12", 0),
 
-    ("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
-
     ("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
     ("CF_Gway_TSigLHSw", "CGW1", 0),
     ("CF_Gway_TurnSigLh", "CGW1", 0),
@@ -60,15 +58,20 @@ def get_can_parser(CP):
 
     ("CF_Lvr_GearInf", "LVR11", 0),        #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev)
 
-    ("CR_Mdps_DrvTq", "MDPS11", 0),
-
     ("CR_Mdps_StrColTq", "MDPS12", 0),
+    ("CF_Mdps_Def", "MDPS12", 0),
     ("CF_Mdps_ToiActive", "MDPS12", 0),
     ("CF_Mdps_ToiUnavail", "MDPS12", 0),
+    ("CF_Mdps_MsgCount2", "MDPS12", 0),
+    ("CF_Mdps_Chksum2", "MDPS12", 0),
+    ("CF_Mdps_ToiFlt", "MDPS12", 0),
+    ("CF_Mdps_SErr", "MDPS12", 0),
+    ("CR_Mdps_StrTq", "MDPS12", 0),
     ("CF_Mdps_FailStat", "MDPS12", 0),
     ("CR_Mdps_OutTq", "MDPS12", 0),
 
     ("VSetDis", "SCC11", 0),
+    ("MainMode_ACC", "SCC11", 0),
     ("SCCInfoDisplay", "SCC11", 0),
     ("ACCMode", "SCC12", 1),
 
@@ -79,14 +82,12 @@ def get_can_parser(CP):
   checks = [
     # address, frequency
     ("MDPS12", 50),
-    ("MDPS11", 100),
     ("TCS15", 10),
     ("TCS13", 50),
     ("CLU11", 50),
     ("ESP12", 100),
     ("EMS12", 100),
     ("CGW1", 10),
-    ("CGW4", 5),
     ("WHL_SPD11", 50),
     ("SCC11", 50),
     ("SCC12", 50),
@@ -99,6 +100,7 @@ def get_camera_parser(CP):
 
   signals = [
     # sig_name, sig_address, default
+    ("CF_Lkas_Icon", "LKAS11", 0),
     ("CF_Lkas_LdwsSysState", "LKAS11", 0),
     ("CF_Lkas_SysWarning", "LKAS11", 0),
     ("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
@@ -113,7 +115,9 @@ def get_camera_parser(CP):
     ("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
     ("CF_Lkas_FusionState", "LKAS11", 0),
     ("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
-    ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
+    ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0),
+    ("CF_Lkas_ActToi", "LKAS11", 0),
+    ("CR_Lkas_StrToqReq", "LKAS11", 0)
   ]
 
   checks = []
@@ -166,7 +170,7 @@ def update(self, cp, cp_cam):
     self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
     self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
     self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
-    self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
+    self.v_wheel = 1.035 * (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
 
     self.low_speed_lockout = self.v_wheel < 1.0
 
@@ -191,17 +195,16 @@ def update(self, cp, cp_cam):
     self.left_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigLh']
     self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
     self.right_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh']
-    self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > 100.
+    self.steer_override = abs(cp.vl["MDPS12"]['CR_Mdps_StrColTq']) > 1.0
     self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
     self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
     self.brake_error = 0
-    self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq']
+    self.steer_torque_driver = cp.vl["MDPS12"]['CR_Mdps_StrColTq']
     self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq']
     self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
 
     self.user_brake = 0
 
-    self.brake_pressed = cp.vl["TCS13"]['DriverBraking']
     self.brake_lights = bool(self.brake_pressed)
     if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1):
       self.pedal_gas = 0
@@ -234,6 +237,7 @@ def update(self, cp, cp_cam):
     else:
       self.gear_shifter_cluster = "unknown"
 
-    # save the entire LKAS11 and CLU11
+    # save the entire LKAS11, CLU11 and MDPS12
     self.lkas11 = cp_cam.vl["LKAS11"]
     self.clu11 = cp.vl["CLU11"]
+    self.mdps12 = cp.vl["MDPS12"]

selfdrive/car/hyundai/hyundaican.py

@@ -1,5 +1,5 @@
 import crcmod
-from selfdrive.car.hyundai.values import CHECKSUM
+from selfdrive.car.hyundai.values import LKAS_FEATURES
 
 hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
 
@@ -8,7 +8,7 @@ def make_can_msg(addr, dat, alt):
 
 def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False):
   values = {
-    "CF_Lkas_Icon": 3 if enabled else 0,
+    "CF_Lkas_Icon": 2 if (car_fingerprint in LKAS_FEATURES["icon_basic"]) else (3 if enabled else 0),
     "CF_Lkas_LdwsSysState": 3 if steer_req else 1,
     "CF_Lkas_SysWarning": hud_alert,
     "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0,
@@ -26,21 +26,21 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled,
     "CF_Lkas_FcwCollisionWarning": lkas11["CF_Lkas_FcwCollisionWarning"] if keep_stock else 0,
     "CF_Lkas_FusionState": lkas11["CF_Lkas_FusionState"] if keep_stock else 0,
     "CF_Lkas_Chksum": 0,
-    "CF_Lkas_FcwOpt_USM": 2 if enabled else 1,
+    "CF_Lkas_FcwOpt_USM": lkas11["CF_Lkas_FcwOpt_USM"] if keep_stock else (2 if enabled else 1),
     "CF_Lkas_LdwsOpt_USM": lkas11["CF_Lkas_LdwsOpt_USM"] if keep_stock else 3,
   }
 
   dat = packer.make_can_msg("LKAS11", 0, values)[2]
 
-  if car_fingerprint in CHECKSUM["crc8"]:
+  if car_fingerprint in LKAS_FEATURES["crc8"]:
     # CRC Checksum as seen on 2019 Hyundai Santa Fe
     dat = dat[:6] + dat[7]
     checksum = hyundai_checksum(dat)
-  elif car_fingerprint in CHECKSUM["6B"]:
+  elif car_fingerprint in LKAS_FEATURES["6B"]:
     # Checksum of first 6 Bytes, as seen on 2018 Kia Sorento
     dat = [ord(i) for i in dat]
     checksum = sum(dat[:6]) % 256
-  elif car_fingerprint in CHECKSUM["7B"]:
+  elif car_fingerprint in LKAS_FEATURES["7B"]:
     # Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger
     dat = [ord(i) for i in dat]
     checksum = (sum(dat[:6]) + dat[7]) % 256

selfdrive/car/hyundai/values.py

@@ -43,10 +43,12 @@ class Buttons:
 
 CAMERA_MSGS = [832, 1156, 1191, 1342]
 
-CHECKSUM = {
-  "crc8": [CAR.SANTA_FE],
-  "6B": [CAR.KIA_SORENTO, CAR.GENESIS],
-  "7B": [CAR.KIA_STINGER, CAR.ELANTRA],
+# Lane Keep Assist related Features and Limitations
+LKAS_FEATURES = {
+  "6B": [CAR.KIA_SORENTO, CAR.GENESIS],     # 6 Bytes used in Checksum
+  "7B": [CAR.KIA_STINGER, CAR.ELANTRA],			# 7 Bytes used in Checksum
+  "crc8": [CAR.SANTA_FE],		                # CRC Checksum
+  "icon_basic": [CAR.GENESIS],		          # Anything but 2 for LKAS_Icon causes MDPS Fault
 }
 
 DBC = {