HKG: parse speed limit (commaai#118)

HKG: parse speed limits
Coral-coder · Feb 20, 2023 · 643fe3b · 643fe3b
1 parent f3a44c1
commit 643fe3b
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 0 deletions.
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
@@ -49,6 +49,7 @@ def __init__(self, CP):
     self.escc_aeb_dec_cmd_act = 0
     self.escc_cmd_act = 0
     self.escc_aeb_dec_cmd = 0
+    self._speed_limit_clu = 0
 
   def update(self, cp, cp_cam):
     if self.CP.carFingerprint in CANFD_CAR:
@@ -174,6 +175,9 @@ def update(self, cp, cp_cam):
     self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
     self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
 
+    self._update_traffic_signals(self.CP, cp, cp_cam)
+    ret.cruiseState.speedLimit = self._calculate_speed_limit() * speed_conv
+
     return ret
 
   def update_canfd(self, cp, cp_cam):
@@ -247,8 +251,20 @@ def update_canfd(self, cp, cp_cam):
     if self.CP.flags & HyundaiFlags.CANFD_HDA2:
       self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"])
 
+    if self.CP.flags & HyundaiFlags.SP_NAV_MSG:
+      self._update_traffic_signals(self.CP, cp, cp_cam)
+      ret.cruiseState.speedLimit = self._calculate_speed_limit() * speed_factor
+
     return ret
 
+  def _update_traffic_signals(self, CP, cp, cp_cam):
+    speed_limit_clu_canfd = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
+    self._speed_limit_clu = speed_limit_clu_canfd.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"] if CP.carFingerprint in CANFD_CAR else\
+                            cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
+
+  def _calculate_speed_limit(self):
+    return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0
+
   @staticmethod
   def get_can_parser(CP):
     if CP.carFingerprint in CANFD_CAR:
@@ -404,6 +420,10 @@ def get_can_parser(CP):
         ]
       checks.append(("ESCC", 50))
 
+    if CP.flags & HyundaiFlags.SP_NAV_MSG:
+      signals.append(("SpeedLim_Nav_Clu", "Navi_HU"))
+      checks.append(("Navi_HU", 5))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
 
   @staticmethod
@@ -554,6 +574,10 @@ def get_can_parser_canfd(CP):
         ("ACCELERATOR_BRAKE_ALT", 100),
       ]
 
+    if CP.flags & HyundaiFlags.CANFD_HDA2 and CP.flags & HyundaiFlags.SP_NAV_MSG:
+      signals.append(("SPEED_LIMIT_1", "CLUSTER_SPEED_LIMIT"))
+      checks.append(("CLUSTER_SPEED_LIMIT", 10))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, get_e_can_bus(CP))
 
   @staticmethod
@@ -581,4 +605,8 @@ def get_cam_can_parser_canfd(CP):
         ("SCC_CONTROL", 50),
       ]
 
+    if not (CP.flags & HyundaiFlags.CANFD_HDA2) and CP.flags & HyundaiFlags.SP_NAV_MSG:
+      signals.append(("SPEED_LIMIT_1", "CLUSTER_SPEED_LIMIT"))
+      checks.append(("CLUSTER_SPEED_LIMIT", 10))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6)
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
@@ -252,9 +252,15 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long):
     if candidate in CANFD_CAR:
       bus = 5 if ret.flags & HyundaiFlags.CANFD_HDA2 else 4
       ret.enableBsm = 0x1e5 in fingerprint[bus]
+
+      if 0x1fa in fingerprint[bus]:
+        ret.flags |= HyundaiFlags.SP_NAV_MSG.value
     else:
       ret.enableBsm = 0x58b in fingerprint[0]
 
+      if 0x544 in fingerprint[0]:
+        ret.flags |= HyundaiFlags.SP_NAV_MSG.value
+
     # *** panda safety config ***
     if candidate in CANFD_CAR:
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),

diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
@@ -64,6 +64,7 @@ class HyundaiFlags(IntFlag):
   USE_FCA = 256
   SP_ENHANCED_SCC = 512
   SP_CAN_LFA_BTN = 1024
+  SP_NAV_MSG = 2048
 
 
 class CAR: