Controls - Speed Limit Controller - Read speed limits from the dash

Co-Authored-By: Efini <19368997+efini@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
Co-Authored-By: multikyd <72329880+multikyd@users.noreply.github.com>

selfdrive/car/hyundai/carstate.py

@@ -58,6 +58,17 @@ class CarState(CarStateBase):
     self.active_mode = 0
     self.drive_mode_prev = 0
 
+  # Traffic signals for Speed Limit Controller - Credit goes to Multikyd!
+  def calculate_speed_limit(self, cp, cp_cam):
+    if self.CP.carFingerprint in CANFD_CAR:
+      speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
+      return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
+    else:
+      if "SpeedLim_Nav_Clu" not in cp.vl["Navi_HU"]:
+        return 0
+      speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
+      return speed_limit if speed_limit not in (0, 255) else 0
+
   def update(self, cp, cp_cam, frogpilot_variables):
     if self.CP.carFingerprint in CANFD_CAR:
       return self.update_canfd(cp, cp_cam, frogpilot_variables)
@@ -175,6 +186,8 @@ class CarState(CarStateBase):
       self.main_enabled = not self.main_enabled
 
     # FrogPilot carstate functions
+    fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp, cp_cam) * speed_conv
+
     self.prev_distance_button = self.distance_button
     self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST
 
@@ -269,6 +282,8 @@ class CarState(CarStateBase):
                                           else cp_cam.vl["CAM_0x2a4"])
 
     # FrogPilot carstate functions
+    fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp, cp_cam) * speed_factor
+
     self.prev_distance_button = self.distance_button
     self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0
 
@@ -336,6 +351,10 @@ class CarState(CarStateBase):
     if CP.flags & HyundaiFlags.CAN_LFA_BTN:
       messages.append(("BCM_PO_11", 50))
 
+    messages += [
+      ("Navi_HU", 5),
+    ]
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
@@ -392,6 +411,9 @@ class CarState(CarStateBase):
         ("SCC_CONTROL", 50),
       ]
 
+    if CP.flags & HyundaiFlags.CANFD_HDA2:
+      messages.append(("CLUSTER_SPEED_LIMIT", 10))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
 
   @staticmethod
@@ -405,4 +427,7 @@ class CarState(CarStateBase):
         ("SCC_CONTROL", 50),
       ]
 
+    if not (CP.flags & HyundaiFlags.CANFD_HDA2):
+      messages.append(("CLUSTER_SPEED_LIMIT", 10))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)

selfdrive/car/toyota/carstate.py

@@ -58,6 +58,21 @@ class CarState(CarStateBase):
     self.zss_angle_offset = 0
     self.zss_threshold_count = 0
 
+  # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team!
+  def calculate_speed_limit(self, cp_cam, frogpilot_variables):
+    signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"]
+    traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals}
+
+    tsgn1 = traffic_signals.get("TSGN1", None)
+    spdval1 = traffic_signals.get("SPDVAL1", None)
+
+    if tsgn1 == 1 and not frogpilot_variables.force_mph_dashboard:
+      return spdval1 * CV.KPH_TO_MS
+    elif tsgn1 == 36 or frogpilot_variables.force_mph_dashboard:
+      return spdval1 * CV.MPH_TO_MS
+    else:
+      return 0
+
   def update(self, cp, cp_cam, frogpilot_variables):
     ret = car.CarState.new_message()
     fp_ret = custom.FrogPilotCarState.new_message()
@@ -196,6 +211,8 @@ class CarState(CarStateBase):
     self.cruise_decreased = self.pcm_acc_status == 10
     self.cruise_increased = self.pcm_acc_status == 9
 
+    fp_ret.dashboardSpeedLimit = self.calculate_speed_limit(cp_cam, frogpilot_variables)
+
     fp_ret.ecoGear = cp.vl["GEAR_PACKET"]['ECON_ON'] == 1
     fp_ret.sportGear = cp.vl["GEAR_PACKET"]['SPORT_ON_2' if self.CP.flags & ToyotaFlags.NO_DSU else 'SPORT_ON'] == 1
 
@@ -292,6 +309,11 @@ class CarState(CarStateBase):
   def get_cam_can_parser(CP):
     messages = []
 
+    messages += [
+      ("RSA1", 0),
+      ("RSA2", 0),
+    ]
+
     if CP.carFingerprint != CAR.PRIUS_V:
       messages += [
         ("LKAS_HUD", 1),

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -207,7 +207,7 @@ class FrogPilotPlanner:
 
     # Pfeiferj's Speed Limit Controller
     if frogpilot_toggles.speed_limit_controller:
-      SpeedLimitController.update(frogpilotNavigation.navigationSpeedLimit, v_ego, frogpilot_toggles)
+      SpeedLimitController.update(frogpilotCarState.dashboardSpeedLimit, frogpilotNavigation.navigationSpeedLimit, v_ego, frogpilot_toggles)
       unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
 
       if frogpilot_toggles.speed_limit_confirmation and self.slc_target != 0:

selfdrive/frogpilot/controls/lib/speed_limit_controller.py

@@ -25,11 +25,13 @@ class SpeedLimitController:
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
 
+    self.car_speed_limit = 0  # m/s
     self.map_speed_limit = 0  # m/s
     self.nav_speed_limit = 0  # m/s
     self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit")
 
-  def update(self, navigationSpeedLimit, v_ego, frogpilot_toggles):
+  def update(self, dashboardSpeedLimit, navigationSpeedLimit, v_ego, frogpilot_toggles):
+    self.car_speed_limit = dashboardSpeedLimit
     self.write_map_state(v_ego)
     self.nav_speed_limit = navigationSpeedLimit
 
@@ -70,7 +72,7 @@ class SpeedLimitController:
 
   @property
   def speed_limit(self):
-    limits = [self.map_speed_limit, self.nav_speed_limit]
+    limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
     filtered_limits = [limit for limit in limits if limit is not None and limit > 1]
 
     if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits:
@@ -79,6 +81,7 @@ class SpeedLimitController:
       return float(min(filtered_limits))
 
     speed_limits = {
+      "Dashboard": self.car_speed_limit,
       "Offline Maps": self.map_speed_limit,
       "Navigation": self.nav_speed_limit,
     }