Speed limit controller

Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself.

Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
Co-Authored-By: Efini <19368997+efini@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
Co-Authored-By: Jason Wen <haibin.wen3@gmail.com>

selfdrive/car/hyundai/carstate.py

@@ -11,6 +11,8 @@ from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_G
                                                    CANFD_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
 STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
@@ -55,6 +57,16 @@ class CarState(CarStateBase):
     # FrogPilot variables
     self.main_enabled = False
 
+  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)
@@ -180,6 +192,9 @@ class CarState(CarStateBase):
           self.fpf.update_experimental_mode()
       self.lkas_previously_pressed = lkas_pressed
 
+    SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv
+    SpeedLimitController.write_car_state()
+
     return ret
 
   def update_canfd(self, cp, cp_cam, frogpilot_variables):
@@ -272,6 +287,9 @@ class CarState(CarStateBase):
         self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode)
       self.lkas_previously_pressed = lkas_pressed
 
+    SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor
+    SpeedLimitController.write_car_state()
+
     return ret
 
   def get_can_parser(self, CP):
@@ -324,6 +342,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
@@ -379,6 +401,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
@@ -392,4 +417,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

@@ -12,6 +12,8 @@ from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_T
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS
 
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
+
 SteerControlType = car.CarParams.SteerControlType
 
 # These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
@@ -47,6 +49,8 @@ class CarState(CarStateBase):
 
     # FrogPilot variables
 
+    self.traffic_signals = {}
+
   def update(self, cp, cp_cam, frogpilot_variables):
     ret = car.CarState.new_message()
 
@@ -198,8 +202,31 @@ class CarState(CarStateBase):
 
       self.distance_previously_pressed = distance_pressed
 
+    # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team!
+    self.update_traffic_signals(cp_cam)
+    SpeedLimitController.car_speed_limit = self.calculate_speed_limit(frogpilot_variables)
+    SpeedLimitController.write_car_state()
+
     return ret
 
+  def update_traffic_signals(self, cp_cam):
+    signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"]
+    new_values = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals}
+
+    if new_values != self.traffic_signals:
+      self.traffic_signals.update(new_values)
+
+  def calculate_speed_limit(self, frogpilot_variables):
+    tsgn1 = self.traffic_signals.get("TSGN1", None)
+    spdval1 = self.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
+
   @staticmethod
   def get_can_parser(CP):
     messages = [
@@ -254,6 +281,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),