From 9213866846927bc61f5f0e035ac3a23f0badc42b Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Mon, 5 Feb 2024 02:21:21 -0600
Subject: [PATCH] wip

---
 selfdrive/car/hyundai/carstate.py             | 13 +++++++++++++
 selfdrive/car/hyundai/interface.py            |  4 +++-
 selfdrive/frogpilot/ui/frogpilot_functions.cc |  2 +-
 3 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 26b014f..f0a6ea4 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -10,6 +10,7 @@ from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
 from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \
                                                    CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
 from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
 
 PREV_BUTTON_SAMPLES = 8
 CLUSTER_SAMPLE_RATE = 20  # frames
@@ -295,6 +296,11 @@ class CarState(CarStateBase):
       self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
                                           else cp_cam.vl["CAM_0x2a4"])
 
+    if self.CP.nav_msg:
+      SpeedLimitController.load_state()
+      SpeedLimitController.car_speed_limit = self.calculate_speed_limit_canfd(self.CP, cp, cp_cam) * speed_conv
+      SpeedLimitController.write_car_state()
+
     self.custom_speed_up = False
     self.custom_speed_down = False
 
@@ -345,6 +351,10 @@ class CarState(CarStateBase):
 
     return ret
 
+  def calculate_speed_limit_canfd(self, CP, cp, cp_cam):
+    self._speed_limit_clu = cp.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
+    return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0
+
   def get_can_parser(self, CP):
     if CP.carFingerprint in CANFD_CAR:
       return self.get_can_parser_canfd(CP)
@@ -462,4 +472,7 @@ class CarState(CarStateBase):
         ("SCC_CONTROL", 50),
       ]
 
+    if CP.nav_msg:
+      messages.append(("CLUSTER_SPEED_LIMIT", 10))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 3099d87..1992743 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -293,9 +293,11 @@ class CarInterface(CarInterfaceBase):
     # *** feature detection ***
     if candidate in CANFD_CAR:
       ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN]
+      ret.nav_msg = 0x544 in fingerprint[0]
     else:
       ret.enableBsm = 0x58b in fingerprint[0]
-
+      ret.nav_msg = False
+      
     # *** panda safety config ***
     ret.safetyConfigs = set_safety_config_hyundai(candidate, CAN, can_fd=(candidate in CANFD_CAR))
 
diff --git a/selfdrive/frogpilot/ui/frogpilot_functions.cc b/selfdrive/frogpilot/ui/frogpilot_functions.cc
index 5cd0c99..0f11d65 100644
--- a/selfdrive/frogpilot/ui/frogpilot_functions.cc
+++ b/selfdrive/frogpilot/ui/frogpilot_functions.cc
@@ -151,7 +151,7 @@ void setDefaultParams() {
     {"StoppingDistance", FrogsGoMoo ? "6" : "0"},
     {"TSS2Tune", "1"},
     {"TurnAggressiveness", FrogsGoMoo ? "150" : "100"},
-    {"TurnDesires", "1"},
+    {"TurnDesires", "0"},
     {"UnlimitedLength", "1"},
     {"UseSI", FrogsGoMoo ? "1" : "0"},
     {"UseVienna", "0"},