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: multikyd <72329880+multikyd@users.noreply.github.com>

cereal/car.capnp

@@ -127,6 +127,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     noLaneAvailable @130;
     openpilotCrashed @131;
     pedalInterceptorNoBrake @133;
+    speedLimitChanged @134;
     torqueNNLoad @135;
 
     radarCanErrorDEPRECATED @15;

cereal/custom.capnp

@@ -10,6 +10,7 @@ $Cxx.namespace("cereal");
 # you can rename the struct, but don't change the identifier
 struct FrogPilotCarControl @0x81c2f05a394cf4af {
   alwaysOnLateral @0 :Bool;
+  speedLimitChanged @1 :Bool;
 }
 
 struct FrogPilotCarState @0xaedffd8f31e7b55d {
@@ -46,8 +47,13 @@ struct FrogPilotPlan @0x80ae746ee2596b11 {
   redLight @12 :Bool;
   safeObstacleDistance @13 :Int16;
   safeObstacleDistanceStock @14 :Int16;
+  slcOverridden @15 :Bool;
+  slcOverriddenSpeed @16 :Float64;
+  slcSpeedLimit @17 :Float64;
+  slcSpeedLimitOffset @18 :Float32;
   stoppedEquivalenceFactor @19 :Int16;
   tFollow @20 :Float32;
+  unconfirmedSlcSpeedLimit @21 :Float64;
   vCruise @22 :Float32;
 }
 

common/params.cc

@@ -232,6 +232,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CameraViewReset", PERSISTENT},
     {"CarMake", PERSISTENT},
     {"CarModel", PERSISTENT},
+    {"CarSpeedLimit", PERSISTENT},
     {"CECurves", PERSISTENT},
     {"CECurvesLead", PERSISTENT},
     {"CENavigation", PERSISTENT},
@@ -281,6 +282,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"FingerprintLogged", CLEAR_ON_MANAGER_START},
     {"ForceAutoTune", PERSISTENT},
     {"ForceFingerprint", PERSISTENT},
+    {"ForceMPHDashboard", PERSISTENT},
     {"FPSCounter", PERSISTENT},
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"FrogsGoMoo", PERSISTENT},
@@ -320,6 +322,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MapboxPublicKey", PERSISTENT},
     {"MapboxSecretKey", PERSISTENT},
     {"MapsSelected", PERSISTENT},
+    {"MapSpeedLimit", PERSISTENT},
     {"MapTargetLatA", PERSISTENT},
     {"MapTargetVelocities", PERSISTENT},
     {"Model", PERSISTENT},
@@ -331,6 +334,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MTSCAggressiveness", PERSISTENT},
     {"MTSCCurvatureCheck", PERSISTENT},
     {"MTSCEnabled", PERSISTENT},
+    {"NavSpeedLimit", PERSISTENT},
     {"NNFF", PERSISTENT},
     {"NNFFLite", PERSISTENT},
     {"NNFFModelName", PERSISTENT},
@@ -339,6 +343,10 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NudgelessLaneChange", PERSISTENT},
     {"NumericalTemp", PERSISTENT},
     {"OfflineMode", PERSISTENT},
+    {"Offset1", PERSISTENT},
+    {"Offset2", PERSISTENT},
+    {"Offset3", PERSISTENT},
+    {"Offset4", PERSISTENT},
     {"OneLaneChange", PERSISTENT},
     {"OnroadDistanceButton", PERSISTENT},
     {"OnroadDistanceButtonPressed", PERSISTENT},
@@ -352,6 +360,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"PauseLateralSpeed", PERSISTENT},
     {"PedalsOnUI", PERSISTENT},
     {"PreferredSchedule", PERSISTENT},
+    {"PreviousSpeedLimit", PERSISTENT},
     {"PromptDistractedVolume", PERSISTENT},
     {"PromptVolume", PERSISTENT},
     {"QOLControls", PERSISTENT},
@@ -373,19 +382,37 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ScreenTimeout", PERSISTENT},
     {"ScreenTimeoutOnroad", PERSISTENT},
     {"SearchInput", PERSISTENT},
+    {"SetSpeedLimit", PERSISTENT},
     {"SetSpeedOffset", PERSISTENT},
     {"ShowCPU", PERSISTENT},
     {"ShowGPU", PERSISTENT},
     {"ShowIP", PERSISTENT},
     {"ShowJerk", PERSISTENT},
     {"ShowMemoryUsage", PERSISTENT},
+    {"ShowSLCOffset", PERSISTENT},
+    {"ShowSLCOffsetUI", PERSISTENT},
     {"ShowStorageLeft", PERSISTENT},
     {"ShowStorageUsed", PERSISTENT},
     {"ShowTuning", PERSISTENT},
     {"Sidebar", PERSISTENT},
+    {"SLCConfirmation", PERSISTENT},
+    {"SLCConfirmationLower", PERSISTENT},
+    {"SLCConfirmationHigher", PERSISTENT},
+    {"SLCConfirmed", PERSISTENT},
+    {"SLCConfirmedPressed", PERSISTENT},
+    {"SLCLookaheadHigher", PERSISTENT},
+    {"SLCLookaheadLower", PERSISTENT},
+    {"SLCFallback", PERSISTENT},
+    {"SLCOverride", PERSISTENT},
+    {"SLCPriority", PERSISTENT},
+    {"SLCPriority1", PERSISTENT},
+    {"SLCPriority2", PERSISTENT},
+    {"SLCPriority3", PERSISTENT},
     {"SmoothBraking", PERSISTENT},
     {"SmoothBrakingFarLead", PERSISTENT},
     {"SmoothBrakingJerk", PERSISTENT},
+    {"SpeedLimitController", PERSISTENT},
+    {"SpeedLimitChangedAlert", PERSISTENT},
     {"StandardFollow", PERSISTENT},
     {"StandardJerk", PERSISTENT},
     {"StandbyMode", PERSISTENT},
@@ -397,6 +424,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UnlockDoors", PERSISTENT},
     {"Updated", PERSISTENT},
     {"UseSI", PERSISTENT},
+    {"UseVienna", PERSISTENT},
     {"WarningImmediateVolume", PERSISTENT},
     {"WarningSoftVolume", PERSISTENT},
 };

release/files_common

@@ -570,6 +570,7 @@ selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
 selfdrive/frogpilot/controls/lib/frogpilot_functions.py
 selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py
 selfdrive/frogpilot/controls/lib/model_manager.py
+selfdrive/frogpilot/controls/lib/speed_limit_controller.py
 selfdrive/frogpilot/controls/lib/theme_manager.py
 selfdrive/frogpilot/fleet_manager/fleet_manager.py
 selfdrive/frogpilot/navigation/mapd.py

selfdrive/car/hyundai/carstate.py

@@ -55,6 +55,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)
@@ -177,6 +187,8 @@ class CarState(CarStateBase):
       self.lkas_previously_enabled = self.lkas_enabled
       self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
 
+    self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
+
     return ret
 
   def update_canfd(self, cp, cp_cam, frogpilot_variables):
@@ -268,6 +280,8 @@ class CarState(CarStateBase):
     self.lkas_previously_enabled = self.lkas_enabled
     self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]
 
+    self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
+
     return ret
 
   def get_can_parser(self, CP):
@@ -320,6 +334,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
@@ -375,6 +393,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
@@ -388,4 +409,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/interfaces.py

@@ -544,6 +544,10 @@ class CarStateBase(ABC):
     # FrogPilot variables
     self.params_memory = Params("/dev/shm/params")
 
+    self.cruise_decreased = False
+    self.cruise_decreased_previously = False
+    self.cruise_increased = False
+    self.cruise_increased_previously = False
     self.lkas_enabled = False
     self.lkas_previously_enabled = False
 

selfdrive/car/toyota/carstate.py

@@ -53,6 +53,21 @@ class CarState(CarStateBase):
     self.pcm_accel_net = 0.0
     self.pcm_neutral_force = 0.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()
 
@@ -192,6 +207,14 @@ class CarState(CarStateBase):
       message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
       self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys)
 
+    self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp_cam, frogpilot_variables))
+
+    self.cruise_decreased_previously = self.cruise_decreased
+    self.cruise_increased_previously = self.cruise_increased
+
+    self.cruise_decreased = self.pcm_acc_status == 10
+    self.cruise_increased = self.pcm_acc_status == 9
+
     self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]
     self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
 
@@ -256,6 +279,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/car/toyota/interface.py

@@ -223,6 +223,8 @@ class CarInterface(CarInterfaceBase):
 
     if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
       ret.buttonEvents = [
+        *create_button_events(self.CS.cruise_increased, self.CS.cruise_increased_previously, {1: ButtonType.accelCruise}),
+        *create_button_events(self.CS.cruise_decreased, self.CS.cruise_decreased_previously, {1: ButtonType.decelCruise}),
         *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
         *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
       ]

selfdrive/controls/controlsd.py

@@ -37,6 +37,7 @@ from openpilot.system.version import get_short_branch
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED, PROBABILITY, MovingAverageCalculator
 
 from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
@@ -196,6 +197,8 @@ class Controls:
     self.speed_check = False
 
     self.previous_lead_distance = 0
+    self.previous_speed_limit = 0
+    self.speed_limit_timer = 0
 
     self.green_light_mac = MovingAverageCalculator()
 
@@ -493,7 +496,7 @@ class Controls:
   def state_transition(self, CS):
     """Compute conditional state transitions and execute actions on state transitions"""
 
-    self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.frogpilot_variables)
+    self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.FPCC.speedLimitChanged, self.frogpilot_variables)
 
     # decrement the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
@@ -568,7 +571,7 @@ class Controls:
           else:
             self.state = State.enabled
           self.current_alert_types.append(ET.ENABLE)
-          self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.frogpilot_variables)
+          self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_variables)
 
     # Check if openpilot is engaged and actuators are enabled
     self.enabled = self.state in ENABLED_STATES
@@ -897,7 +900,7 @@ class Controls:
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
       if self.CP.openpilotLongitudinalControl and not self.frogpilot_variables.conditional_experimental_mode:
-        self.experimental_mode = self.params.get_bool("ExperimentalMode")
+        self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.speed_limit_controller and SpeedLimitController.experimental_mode
       self.personality = self.read_personality_param()
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@@ -952,7 +955,52 @@ class Controls:
     if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and not self.openpilot_crashed_triggered:
       self.events.add(EventName.openpilotCrashed)
 
-      self.openpilot_crashed_triggered = True
+    if self.speed_limit_alert or self.speed_limit_confirmation:
+      current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit
+      desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit
+
+      speed_limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1
+
+      speed_limit_changed_lower = speed_limit_changed and self.previous_speed_limit > desired_speed_limit
+      speed_limit_changed_higher = speed_limit_changed and self.previous_speed_limit < desired_speed_limit
+
+      self.previous_speed_limit = desired_speed_limit
+
+      if self.CP.pcmCruise and self.FPCC.speedLimitChanged:
+        if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents):
+          self.params_memory.put_bool("SLCConfirmed", True)
+          self.params_memory.put_bool("SLCConfirmedPressed", True)
+        elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents):
+          self.params_memory.put_bool("SLCConfirmed", False)
+          self.params_memory.put_bool("SLCConfirmedPressed", True)
+
+      if speed_limit_changed_lower:
+        if self.speed_limit_confirmation_lower:
+          self.FPCC.speedLimitChanged = True
+        else:
+          self.params_memory.put_bool("SLCConfirmed", True)
+      elif speed_limit_changed_higher:
+        if self.speed_limit_confirmation_higher:
+          self.FPCC.speedLimitChanged = True
+        else:
+          self.params_memory.put_bool("SLCConfirmed", True)
+
+      if self.params_memory.get_bool("SLCConfirmedPressed") or not self.speed_limit_confirmation:
+        self.FPCC.speedLimitChanged = False
+        self.params_memory.put_bool("SLCConfirmedPressed", False)
+
+      if (speed_limit_changed_lower or speed_limit_changed_higher) and self.speed_limit_alert:
+        self.events.add(EventName.speedLimitChanged)
+
+      if self.FPCC.speedLimitChanged:
+        self.speed_limit_timer += DT_CTRL
+        if self.speed_limit_timer >= 10:
+          self.FPCC.speedLimitChanged = False
+          self.speed_limit_timer = 0
+      else:
+        self.speed_limit_timer = 0
+    else:
+      self.FPCC.speedLimitChanged = False
 
     if self.sm.frame == 550 and self.CP.lateralTuning.which() == 'torque' and self.CI.use_nnff:
       self.events.add(EventName.torqueNNLoad)
@@ -1034,6 +1082,14 @@ class Controls:
     self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise")
     self.frogpilot_variables.set_speed_offset = self.params.get_int("SetSpeedOffset") * (1 if self.is_metric else CV.MPH_TO_KPH) if quality_of_life else 0
 
+    self.speed_limit_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("SpeedLimitController")
+    self.frogpilot_variables.force_mph_dashboard = self.speed_limit_controller and self.params.get_bool("ForceMPHDashboard")
+    self.frogpilot_variables.set_speed_limit = self.speed_limit_controller and self.params.get_bool("SetSpeedLimit")
+    self.speed_limit_alert = self.speed_limit_controller and self.params.get_bool("SpeedLimitChangedAlert")
+    self.speed_limit_confirmation = self.speed_limit_controller and self.params.get_bool("SLCConfirmation")
+    self.speed_limit_confirmation_lower = self.speed_limit_confirmation and self.params.get_bool("SLCConfirmationLower")
+    self.speed_limit_confirmation_higher = self.speed_limit_confirmation and self.params.get_bool("SLCConfirmationHigher")
+
     toyota_doors = self.params.get_bool("ToyotaDoors")
     self.frogpilot_variables.lock_doors = toyota_doors and self.params.get_bool("LockDoors")
     self.frogpilot_variables.unlock_doors = toyota_doors and self.params.get_bool("UnlockDoors")

selfdrive/controls/lib/drive_helpers.py

@@ -53,13 +53,13 @@ class VCruiseHelper:
   def v_cruise_initialized(self):
     return self.v_cruise_kph != V_CRUISE_UNSET
 
-  def update_v_cruise(self, CS, enabled, is_metric, frogpilot_variables):
+  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables):
     self.v_cruise_kph_last = self.v_cruise_kph
 
     if CS.cruiseState.available:
       if not self.CP.pcmCruise:
         # if stock cruise is completely disabled, then we can use our own set speed logic
-        self._update_v_cruise_non_pcm(CS, enabled, is_metric, frogpilot_variables)
+        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, frogpilot_variables)
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
@@ -69,7 +69,7 @@ class VCruiseHelper:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
 
-  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, frogpilot_variables):
+  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables):
     # handle button presses. TODO: this should be in state_control, but a decelCruise press
     # would have the effect of both enabling and changing speed is checked after the state transition
     if not enabled:
@@ -96,6 +96,17 @@ class VCruiseHelper:
     if button_type is None:
       return
 
+    # Confirm or deny the new speed limit value
+    if speed_limit_changed:
+      if button_type == ButtonType.accelCruise:
+        self.params_memory.put_bool("SLCConfirmed", True)
+        self.params_memory.put_bool("SLCConfirmedPressed", True)
+        return
+      elif button_type == ButtonType.decelCruise:
+        self.params_memory.put_bool("SLCConfirmed", False)
+        self.params_memory.put_bool("SLCConfirmedPressed", True)
+        return
+
     # Don't adjust speed when pressing resume to exit standstill
     cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill
     if button_type == ButtonType.accelCruise and cruise_standstill:
@@ -135,7 +146,7 @@ class VCruiseHelper:
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
         self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
 
-  def initialize_v_cruise(self, CS, experimental_mode: bool, frogpilot_variables) -> None:
+  def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_variables) -> None:
     # initializing is handled by the PCM
     if self.CP.pcmCruise:
       return
@@ -146,7 +157,10 @@ class VCruiseHelper:
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
       self.v_cruise_kph = self.v_cruise_kph_last
     else:
-      self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
+      if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit:
+        self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH))
+      else:
+        self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
 
     self.v_cruise_cluster_kph = self.v_cruise_kph
 

selfdrive/controls/lib/events.py

@@ -1072,6 +1072,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.HIGH, VisualAlert.wrongGear, AudibleAlert.promptRepeat, 4.),
   },
 
+  EventName.speedLimitChanged: {
+    ET.PERMANENT: Alert(
+      "Speed Limit Changed",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
   EventName.torqueNNLoad: {
     ET.PERMANENT: torque_nn_load_alert,
   },

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -21,6 +21,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode im
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController
 from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 # Acceleration profiles - Credit goes to the DragonPilot team!
                  # MPH = [0., 18,  36,  63,  94]
@@ -61,8 +62,12 @@ class FrogPilotPlanner:
 
     self.radarless_model = self.params.get("Model", block=True, encoding='utf-8') in RADARLESS_MODELS
 
+    self.override_slc = False
+
     self.jerk = 0
+    self.overridden_speed = 0
     self.mtsc_target = 0
+    self.slc_target = 0
     self.t_follow = 0
 
   def update(self, carState, controlsState, frogpilotCarControl, frogpilotNavigation, liveLocationKalman, modelData, radarState):
@@ -178,7 +183,54 @@ class FrogPilotPlanner:
     else:
       self.mtsc_target = v_cruise
 
-    targets = [self.mtsc_target]
+    # Pfeiferj's Speed Limit Controller
+    if self.speed_limit_controller:
+      SpeedLimitController.update(v_ego)
+      unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
+
+      # Check if the new speed limit has been confirmed by the user
+      if self.speed_limit_confirmation:
+        if self.params_memory.get_bool("SLCConfirmed") or self.slc_target == 0:
+          self.slc_target = unconfirmed_slc_target
+          self.params_memory.put_bool("SLCConfirmed", False)
+      else:
+        self.slc_target = unconfirmed_slc_target
+
+      # Override SLC upon gas pedal press and reset upon brake/cancel button
+      self.override_slc &= self.overridden_speed > self.slc_target
+      self.override_slc |= carState.gasPressed and v_ego > self.slc_target
+      self.override_slc &= enabled
+
+      # Use the override speed if SLC is being overridden
+      if self.override_slc:
+        if self.speed_limit_controller_override == 1:
+          # Set the speed limit to the manual set speed
+          if carState.gasPressed:
+            self.overridden_speed = v_ego + v_ego_diff
+          self.overridden_speed = np.clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff)
+        elif self.speed_limit_controller_override == 2:
+          # Set the speed limit to the max set speed
+          self.overridden_speed = v_cruise + v_cruise_diff
+      else:
+        self.overridden_speed = 0
+    else:
+      self.slc_target = v_cruise
+
+    # Pfeiferj's Vision Turn Controller
+    if self.vision_turn_controller and v_ego > CRUISING_SPEED and enabled:
+      orientation_rate = np.array(np.abs(modelData.orientationRate.z)) * self.curve_sensitivity
+      velocity = np.array(modelData.velocity.x)
+
+      max_pred_lat_acc = np.amax(orientation_rate * velocity)
+      max_curve = max_pred_lat_acc / (v_ego**2)
+      adjusted_target_lat_a = TARGET_LAT_A * self.turn_aggressiveness
+
+      self.vtsc_target = (adjusted_target_lat_a / max_curve)**0.5
+      self.vtsc_target = np.clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
+    else:
+      self.vtsc_target = v_cruise
+
+    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target]
     filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets]
 
     return min(filtered_targets)
@@ -208,6 +260,12 @@ class FrogPilotPlanner:
 
     frogpilotPlan.redLight = self.cem.red_light_detected
 
+    frogpilotPlan.slcOverridden = bool(self.override_slc)
+    frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
+    frogpilotPlan.slcSpeedLimit = self.slc_target
+    frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
+    frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit
+
     pm.send('frogpilotPlan', frogpilot_plan_send)
 
   def update_frogpilot_params(self):
@@ -247,3 +305,7 @@ class FrogPilotPlanner:
     self.map_turn_speed_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("MTSCEnabled")
     self.mtsc_curvature_check = self.map_turn_speed_controller and self.params.get_bool("MTSCCurvatureCheck")
     self.params_memory.put_float("MapTargetLatA", 2 * (self.params.get_int("MTSCAggressiveness") / 100))
+
+    self.speed_limit_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("SpeedLimitController")
+    self.speed_limit_confirmation = self.speed_limit_controller and self.params.get_bool("SLCConfirmation")
+    self.speed_limit_controller_override = self.speed_limit_controller and self.params.get_int("SLCOverride")

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -4,6 +4,8 @@ from openpilot.common.params import Params
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY, MovingAverageCalculator
 
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
+
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
 SLOW_DOWN_DISTANCE = [20, 30., 50., 70., 80., 90., 105., 120.]
 TRAJECTORY_SIZE = 33
@@ -73,6 +75,10 @@ class ConditionalExperimentalMode:
       self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8
       return True
 
+    if SpeedLimitController.experimental_mode:
+      self.status_value = 9
+      return True
+
     if (not self.lead_detected and v_ego <= self.limit) or (self.lead_detected and v_ego <= self.limit_lead):
       self.status_value = 10 if self.lead_detected else 11
       return True

selfdrive/frogpilot/controls/lib/speed_limit_controller.py

@@ -0,0 +1,160 @@
+# PFEIFER - SLC - Modified by FrogAi for FrogPilot
+import json
+import math
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+
+R = 6373000.0  # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+TO_DEGREES = 180 / math.pi
+
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx - ax) / 2) ** 2 + math.cos(ax) * math.cos(bx) * math.sin((by - ay) / 2) ** 2
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+  return R * c  # in meters
+
+class SpeedLimitController:
+  def __init__(self):
+    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 = 0  # m/s
+
+    self.lat = 0  # deg
+    self.lon = 0  # deg
+
+    self.update_frogpilot_params()
+
+  def update(self, v_ego):
+    self.car_speed_limit = self.get_param_memory("CarSpeedLimit")
+    self.write_map_state(v_ego)
+    self.nav_speed_limit = self.get_param_memory("NavSpeedLimit")
+
+    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
+      self.update_frogpilot_params()
+
+  def get_param_memory(self, key, is_json=False, default=None):
+    try:
+      data = self.params_memory.get(key)
+      if not is_json and data is not None:
+        return float(data.decode('utf-8'))
+      return json.loads(data) if is_json else data
+    except:
+      return default
+
+  def write_map_state(self, v_ego):
+    self.map_speed_limit = self.get_param_memory("MapSpeedLimit")
+
+    next_map_speed_limit = self.get_param_memory("NextMapSpeedLimit", is_json=True, default={})
+    next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0)
+    next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0)
+    next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0)
+
+    position = self.get_param_memory("LastGPSPosition", is_json=True, default={})
+    self.lat = position.get("latitude", 0)
+    self.lon = position.get("longitude", 0)
+
+    if self.prv_speed_limit < next_map_speed_limit_value > 1:
+      d = distance_to_point(self.lat * TO_RADIANS, self.lon * TO_RADIANS,
+                            next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS)
+      max_d = self.map_speed_lookahead_higher * v_ego
+      if d < max_d:
+        self.map_speed_limit = next_map_speed_limit_value
+
+    if self.prv_speed_limit > next_map_speed_limit_value > 1:
+      d = distance_to_point(self.lat * TO_RADIANS, self.lon * TO_RADIANS,
+                            next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS)
+      max_d = self.map_speed_lookahead_higher_lower * v_ego
+      if d < max_d:
+        self.map_speed_limit = next_map_speed_limit_value
+
+  @property
+  def speed_limit(self):
+    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.highest and filtered_limits:
+      return float(max(filtered_limits))
+    elif self.lowest and filtered_limits:
+      return float(min(filtered_limits))
+
+    speed_limits = {
+      "Dashboard": self.car_speed_limit,
+      "Offline Maps": self.map_speed_limit,
+      "Navigation": self.nav_speed_limit,
+    }
+
+    priorities = [
+      self.speed_limit_priority1,
+      self.speed_limit_priority2,
+      self.speed_limit_priority3,
+    ]
+
+    for priority in priorities:
+      if priority in speed_limits and speed_limits[priority] in filtered_limits:
+        return float(speed_limits[priority])
+
+    if self.use_previous_limit:
+      return float(self.prv_speed_limit)
+
+    return 0
+
+  @property
+  def offset(self):
+    if self.speed_limit < 13.5:
+      return self.offset1
+    elif self.speed_limit < 24:
+      return self.offset2
+    elif self.speed_limit < 29:
+      return self.offset3
+    else:
+      return self.offset4
+
+  @property
+  def desired_speed_limit(self):
+    if self.speed_limit > 1:
+      self.update_previous_limit(self.speed_limit)
+      return self.speed_limit + self.offset
+    else:
+      return 0
+
+  def update_previous_limit(self, speed_limit):
+    if self.prv_speed_limit != speed_limit:
+      self.params.put_float("PreviousSpeedLimit", speed_limit)
+      self.prv_speed_limit = speed_limit
+
+  @property
+  def experimental_mode(self):
+    return self.speed_limit == 0 and self.use_experimental_mode
+
+  def update_frogpilot_params(self):
+    conversion = CV.KPH_TO_MS if self.params.get_bool("IsMetric") else CV.MPH_TO_MS
+
+    self.map_speed_lookahead_higher = self.params.get_int("SLCLookaheadHigher")
+    self.map_speed_lookahead_lower = self.params.get_int("SLCLookaheadLower")
+
+    self.offset1 = self.params.get_int("Offset1") * conversion
+    self.offset2 = self.params.get_int("Offset2") * conversion
+    self.offset3 = self.params.get_int("Offset3") * conversion
+    self.offset4 = self.params.get_int("Offset4") * conversion
+
+    self.speed_limit_priority1 = self.params.get("SLCPriority1", encoding='utf-8')
+    self.speed_limit_priority2 = self.params.get("SLCPriority2", encoding='utf-8')
+    self.speed_limit_priority3 = self.params.get("SLCPriority3", encoding='utf-8')
+
+    self.highest = self.speed_limit_priority1 == "Highest"
+    self.lowest = self.speed_limit_priority1 == "Lowest"
+
+    slc_fallback = self.params.get_int("SLCFallback")
+    self.use_experimental_mode = slc_fallback == 1
+    self.use_previous_limit = slc_fallback == 2
+
+    self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit")
+
+SpeedLimitController = SpeedLimitController()

selfdrive/frogpilot/fleetmanager/helpers.py

@@ -452,7 +452,8 @@ def get_all_toggle_values():
     "LaneDetectionWidth", "OneLaneChange", "LaneDetectionWidth", "QOLControls", "CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "HigherBitrate",
     "OnroadDistanceButton", "KaofuiIcons", "PauseLateralSpeed", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset", "SpeedLimitController", "Offset1",
     "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority", "SLCConfirmation", "SLCConfirmationLower", "SLCConfirmationHigher",
-    "ForceMPHDashboard", "SetSpeedLimit", "ShowSLCOffset", "ShowSLCOffsetUI", "UseVienna", "VisionTurnControl", "DisableVTSCSmoothing", "CurveSensitivity",
+    "ForceMPHDashboard", "SLCLookaheadHigher", "SLCLookaheadLower", "SetSpeedLimit", "ShowSLCOffset", "ShowSLCOffsetUI", "UseVienna", "VisionTurnControl",
+    "DisableVTSCSmoothing", "CurveSensitivity", "TurnAggressiveness",
     "TurnAggressiveness", "ForceFingerprint", "DisableOpenpilotLongitudinal", "EVTable", "LongPitch", "GasRegenCmd", "CrosstrekTorque", "LockDoors", "StockTune",
     "CydiaTune", "DragonPilotTune", "FrogsGoMooTune", "LockDoors", "SNGHack",
     "AlertVolumeControl", "DisengageVolume", "EngageVolume", "PromptVolume", "PromptDistractedVolume", "RefuseVolume", "WarningSoftVolume",

selfdrive/frogpilot/ui/qt/offroad/control_settings.cc

@@ -108,6 +108,26 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"PauseLateralSpeed", tr("Pause Lateral Below"), tr("Pause lateral control on all speeds below the set speed."), ""},
     {"ReverseCruise", tr("Reverse Cruise Increase"), tr("Reverses the 'long press' functionality logic to increase the max set speed by 5 instead of 1. Useful to increase the max speed quickly."), ""},
     {"SetSpeedOffset", tr("Set Speed Offset"), tr("Set an offset for your desired set speed."), ""},
+
+    {"SpeedLimitController", tr("Speed Limit Controller"), tr("Automatically adjust the max speed to match the current speed limit using 'Open Street Maps', 'Navigate On openpilot', or your car's dashboard (Toyotas/Lexus/HKG only)."), "../assets/offroad/icon_speed_limit.png"},
+    {"SLCControls", tr("Controls Settings"), tr("Manage toggles related to 'Speed Limit Controller's controls."), ""},
+    {"Offset1", tr("Speed Limit Offset (0-34 mph)"), tr("Speed limit offset for speed limits between 0-34 mph."), ""},
+    {"Offset2", tr("Speed Limit Offset (35-54 mph)"), tr("Speed limit offset for speed limits between 35-54 mph."), ""},
+    {"Offset3", tr("Speed Limit Offset (55-64 mph)"), tr("Speed limit offset for speed limits between 55-64 mph."), ""},
+    {"Offset4", tr("Speed Limit Offset (65-99 mph)"), tr("Speed limit offset for speed limits between 65-99 mph."), ""},
+    {"SLCFallback", tr("Fallback Method"), tr("Choose your fallback method when there is no speed limit available."), ""},
+    {"SLCOverride", tr("Override Method"), tr("Choose your preferred method to override the current speed limit."), ""},
+    {"SLCPriority", tr("Priority Order"), tr("Configure the speed limit priority order."), ""},
+    {"SLCQOL", tr("Quality of Life Settings"), tr("Manage toggles related to 'Speed Limit Controller's quality of life features."), ""},
+    {"SLCConfirmation", tr("Confirm New Speed Limits"), tr("Don't automatically start using the new speed limit until it's been manually confirmed."), ""},
+    {"ForceMPHDashboard", tr("Force MPH From Dashboard Readings"), tr("Force MPH readings from the dashboard. Only use this if you live in an area where the speed limits from your dashboard are in KPH, but you use MPH."), ""},
+    {"SLCLookaheadHigher", tr("Prepare For Higher Speed Limits"), tr("Set a 'lookahead' value to prepare for upcoming speed limits higher than your current speed limit using the data stored in 'Open Street Maps'."), ""},
+    {"SLCLookaheadLower", tr("Prepare For Lower Speed Limits"), tr("Set a 'lookahead' value to prepare for upcoming speed limits lower than your current speed limit using the data stored in 'Open Street Maps'."), ""},
+    {"SetSpeedLimit", tr("Use Current Speed Limit As Set Speed"), tr("Sets your max speed to the current speed limit if one is populated when you initially enable openpilot."), ""},
+    {"SLCVisuals", tr("Visuals Settings"), tr("Manage toggles related to 'Speed Limit Controller's visuals."), ""},
+    {"ShowSLCOffset", tr("Show Speed Limit Offset"), tr("Show the speed limit offset separated from the speed limit in the onroad UI when using 'Speed Limit Controller'."), ""},
+    {"SpeedLimitChangedAlert", tr("Speed Limit Changed Alert"), tr("Trigger an alert whenever the speed limit changes."), ""},
+    {"UseVienna", tr("Use Vienna Speed Limit Signs"), tr("Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US)."), ""},
   };
 
   for (const auto &[param, title, desc, icon] : controlToggles) {
@@ -573,6 +593,129 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     } else if (param == "LaneDetectionWidth") {
       toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 100, std::map<int, QString>(), this, false, " feet", 10);
 
+    } else if (param == "SpeedLimitController") {
+      FrogPilotParamManageControl *speedLimitControllerToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
+      QObject::connect(speedLimitControllerToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        openParentToggle();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end());
+        }
+      });
+      toggle = speedLimitControllerToggle;
+    } else if (param == "SLCControls") {
+      FrogPilotParamManageControl *manageSLCControlsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true);
+      QObject::connect(manageSLCControlsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        openParentToggle();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(speedLimitControllerControlsKeys.find(key.c_str()) != speedLimitControllerControlsKeys.end());
+          openSubParentToggle();
+        }
+        slcPriorityButton->setVisible(true);
+      });
+      toggle = manageSLCControlsToggle;
+    } else if (param == "SLCQOL") {
+      FrogPilotParamManageControl *manageSLCQOLToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true);
+      QObject::connect(manageSLCQOLToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        openParentToggle();
+        for (auto &[key, toggle] : toggles) {
+          std::set<QString> modifiedSpeedLimitControllerQOLKeys = speedLimitControllerQOLKeys;
+
+          if (hasPCMCruise) {
+            modifiedSpeedLimitControllerQOLKeys.erase("SetSpeedLimit");
+          }
+
+          if (!isToyota) {
+            modifiedSpeedLimitControllerQOLKeys.erase("ForceMPHDashboard");
+          }
+
+          toggle->setVisible(modifiedSpeedLimitControllerQOLKeys.find(key.c_str()) != modifiedSpeedLimitControllerQOLKeys.end());
+          openSubParentToggle();
+        }
+      });
+      toggle = manageSLCQOLToggle;
+    } else if (param == "SLCConfirmation") {
+      std::vector<QString> slcConfirmationToggles{"SLCConfirmationLower", "SLCConfirmationHigher"};
+      std::vector<QString> slcConfirmationNames{tr("Lower Limits"), tr("Higher Limits")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, slcConfirmationToggles, slcConfirmationNames);
+    } else if (param == "SLCLookaheadHigher" || param == "SLCLookaheadLower") {
+      toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 60, std::map<int, QString>(), this, false, " seconds");
+    } else if (param == "SLCVisuals") {
+      FrogPilotParamManageControl *manageSLCVisualsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true);
+      QObject::connect(manageSLCVisualsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        openParentToggle();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(speedLimitControllerVisualsKeys.find(key.c_str()) != speedLimitControllerVisualsKeys.end());
+          openSubParentToggle();
+        }
+      });
+      toggle = manageSLCVisualsToggle;
+    } else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") {
+      toggle = new FrogPilotParamValueControl(param, title, desc, icon, -99, 99, std::map<int, QString>(), this, false, tr(" mph"));
+    } else if (param == "ShowSLCOffset") {
+      std::vector<QString> slcOffsetToggles{"ShowSLCOffsetUI"};
+      std::vector<QString> slcOffsetToggleNames{tr("Control Via UI")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, slcOffsetToggles, slcOffsetToggleNames);
+    } else if (param == "SLCFallback") {
+      std::vector<QString> fallbackOptions{tr("Set Speed"), tr("Experimental Mode"), tr("Previous Limit")};
+      FrogPilotButtonParamControl *fallbackSelection = new FrogPilotButtonParamControl(param, title, desc, icon, fallbackOptions);
+      toggle = fallbackSelection;
+    } else if (param == "SLCOverride") {
+      std::vector<QString> overrideOptions{tr("None"), tr("Manual Set Speed"), tr("Set Speed")};
+      FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions);
+      toggle = overrideSelection;
+    } else if (param == "SLCPriority") {
+      slcPriorityButton = new ButtonControl(title, tr("SELECT"), desc);
+      QStringList primaryPriorities = {tr("None"), tr("Dashboard"), tr("Navigation"), tr("Offline Maps"), tr("Highest"), tr("Lowest")};
+      QStringList secondaryTertiaryPriorities = {tr("None"), tr("Dashboard"), tr("Navigation"), tr("Offline Maps")};
+      QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")};
+
+      QObject::connect(slcPriorityButton, &ButtonControl::clicked, [=]() {
+        QStringList selectedPriorities;
+
+        for (int i = 1; i <= 3; ++i) {
+          QStringList currentPriorities = (i == 1) ? primaryPriorities : secondaryTertiaryPriorities;
+          QStringList prioritiesToDisplay = currentPriorities;
+          for (const auto &selectedPriority : qAsConst(selectedPriorities)) {
+            prioritiesToDisplay.removeAll(selectedPriority);
+          }
+
+          if (!hasDashSpeedLimits) {
+            prioritiesToDisplay.removeAll(tr("Dashboard"));
+          }
+
+          if (prioritiesToDisplay.size() == 1 && prioritiesToDisplay.contains(tr("None"))) {
+            break;
+          }
+
+          QString priorityKey = QString("SLCPriority%1").arg(i);
+          QString selection = MultiOptionDialog::getSelection(priorityPrompts[i - 1], prioritiesToDisplay, "", this);
+
+          if (selection.isEmpty()) break;
+
+          params.put(priorityKey.toStdString(), selection.toStdString());
+          selectedPriorities.append(selection);
+
+          if (selection == tr("Lowest") || selection == tr("Highest") || selection == tr("None")) break;
+
+          updateToggles();
+        }
+
+        selectedPriorities.removeAll(tr("None"));
+        slcPriorityButton->setValue(selectedPriorities.join(", "));
+      });
+
+      QStringList initialPriorities;
+      for (int i = 1; i <= 3; ++i) {
+        QString priorityKey = QString("SLCPriority%1").arg(i);
+        QString priority = QString::fromStdString(params.get(priorityKey.toStdString()));
+
+        if (!priority.isEmpty() && primaryPriorities.contains(priority) && priority != tr("None")) {
+          initialPriorities.append(priority);
+        }
+      }
+      slcPriorityButton->setValue(initialPriorities.join(", "));
+      addItem(slcPriorityButton);
+
     } else {
       toggle = new ParamControl(param, title, desc, icon, this);
     }
@@ -688,6 +831,7 @@ void FrogPilotControlsPanel::updateCarToggles() {
     hasAutoTune = (carName == "hyundai" || carName == "toyota") && CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::TORQUE;
     uiState()->scene.has_auto_tune = hasAutoTune;
     hasCommaNNFFSupport = checkCommaNNFFSupport(carFingerprint);
+    hasDashSpeedLimits = carName == "hyundai" || carName == "toyota";
     hasNNFFLog = checkNNFFLogFileExists(carFingerprint);
     hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal");
     hasPCMCruise = CP.getPcmCruise();
@@ -718,6 +862,10 @@ void FrogPilotControlsPanel::updateMetric() {
     params.putIntNonBlocking("CustomCruise", std::nearbyint(params.getInt("CustomCruise") * speedConversion));
     params.putIntNonBlocking("CustomCruiseLong", std::nearbyint(params.getInt("CustomCruiseLong") * speedConversion));
     params.putIntNonBlocking("LaneDetectionWidth", std::nearbyint(params.getInt("LaneDetectionWidth") * distanceConversion));
+    params.putIntNonBlocking("Offset1", std::nearbyint(params.getInt("Offset1") * speedConversion));
+    params.putIntNonBlocking("Offset2", std::nearbyint(params.getInt("Offset2") * speedConversion));
+    params.putIntNonBlocking("Offset3", std::nearbyint(params.getInt("Offset3") * speedConversion));
+    params.putIntNonBlocking("Offset4", std::nearbyint(params.getInt("Offset4") * speedConversion));
     params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion));
     params.putIntNonBlocking("PauseLateralOnSignal", std::nearbyint(params.getInt("PauseLateralOnSignal") * speedConversion));
     params.putIntNonBlocking("PauseLateralSpeed", std::nearbyint(params.getInt("PauseLateralSpeed") * speedConversion));
@@ -728,23 +876,55 @@ void FrogPilotControlsPanel::updateMetric() {
   FrogPilotParamValueControl *customCruiseToggle = static_cast<FrogPilotParamValueControl*>(toggles["CustomCruise"]);
   FrogPilotParamValueControl *customCruiseLongToggle = static_cast<FrogPilotParamValueControl*>(toggles["CustomCruiseLong"]);
   FrogPilotParamValueControl *laneWidthToggle = static_cast<FrogPilotParamValueControl*>(toggles["LaneDetectionWidth"]);
+  FrogPilotParamValueControl *offset1Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset1"]);
+  FrogPilotParamValueControl *offset2Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset2"]);
+  FrogPilotParamValueControl *offset3Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset3"]);
+  FrogPilotParamValueControl *offset4Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset4"]);
   FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast<FrogPilotParamValueControl*>(toggles["PauseAOLOnBrake"]);
   FrogPilotParamValueControl *pauseLateralToggle = static_cast<FrogPilotParamValueControl*>(toggles["PauseLateralSpeed"]);
   FrogPilotParamValueControl *setSpeedOffsetToggle = static_cast<FrogPilotParamValueControl*>(toggles["SetSpeedOffset"]);
   FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(toggles["StoppingDistance"]);
 
   if (isMetric) {
+    offset1Toggle->setTitle(tr("Speed Limit Offset (0-34 kph)"));
+    offset2Toggle->setTitle(tr("Speed Limit Offset (35-54 kph)"));
+    offset3Toggle->setTitle(tr("Speed Limit Offset (55-64 kph)"));
+    offset4Toggle->setTitle(tr("Speed Limit Offset (65-99 kph)"));
+
+    offset1Toggle->setDescription(tr("Set speed limit offset for limits between 0-34 kph."));
+    offset2Toggle->setDescription(tr("Set speed limit offset for limits between 35-54 kph."));
+    offset3Toggle->setDescription(tr("Set speed limit offset for limits between 55-64 kph."));
+    offset4Toggle->setDescription(tr("Set speed limit offset for limits between 65-99 kph."));
+
     customCruiseToggle->updateControl(1, 150, tr(" kph"));
     customCruiseLongToggle->updateControl(1, 150, tr(" kph"));
     laneWidthToggle->updateControl(0, 30, tr(" meters"), 10);
+    offset1Toggle->updateControl(-99, 99, tr(" kph"));
+    offset2Toggle->updateControl(-99, 99, tr(" kph"));
+    offset3Toggle->updateControl(-99, 99, tr(" kph"));
+    offset4Toggle->updateControl(-99, 99, tr(" kph"));
     pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" kph"));
     pauseLateralToggle->updateControl(0, 99, tr(" kph"));
     setSpeedOffsetToggle->updateControl(0, 150, tr(" kph"));
     stoppingDistanceToggle->updateControl(0, 5, tr(" meters"));
   } else {
+    offset1Toggle->setTitle(tr("Speed Limit Offset (0-34 mph)"));
+    offset2Toggle->setTitle(tr("Speed Limit Offset (35-54 mph)"));
+    offset3Toggle->setTitle(tr("Speed Limit Offset (55-64 mph)"));
+    offset4Toggle->setTitle(tr("Speed Limit Offset (65-99 mph)"));
+
+    offset1Toggle->setDescription(tr("Set speed limit offset for limits between 0-34 mph."));
+    offset2Toggle->setDescription(tr("Set speed limit offset for limits between 35-54 mph."));
+    offset3Toggle->setDescription(tr("Set speed limit offset for limits between 55-64 mph."));
+    offset4Toggle->setDescription(tr("Set speed limit offset for limits between 65-99 mph."));
+
     customCruiseToggle->updateControl(1, 99, tr(" mph"));
     customCruiseLongToggle->updateControl(1, 99, tr(" mph"));
     laneWidthToggle->updateControl(0, 100, tr(" feet"), 10);
+    offset1Toggle->updateControl(-99, 99, tr(" mph"));
+    offset2Toggle->updateControl(-99, 99, tr(" mph"));
+    offset3Toggle->updateControl(-99, 99, tr(" mph"));
+    offset4Toggle->updateControl(-99, 99, tr(" mph"));
     pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" mph"));
     pauseLateralToggle->updateControl(0, 99, tr(" mph"));
     setSpeedOffsetToggle->updateControl(0, 99, tr(" mph"));
@@ -754,6 +934,10 @@ void FrogPilotControlsPanel::updateMetric() {
   customCruiseToggle->refresh();
   customCruiseLongToggle->refresh();
   laneWidthToggle->refresh();
+  offset1Toggle->refresh();
+  offset2Toggle->refresh();
+  offset3Toggle->refresh();
+  offset4Toggle->refresh();
   pauseAOLOnBrakeToggle->refresh();
   pauseLateralToggle->refresh();
   setSpeedOffsetToggle->refresh();
@@ -767,11 +951,12 @@ void FrogPilotControlsPanel::hideToggles() {
   deleteModelBtn->setVisible(false);
   downloadModelBtn->setVisible(false);
   selectModelBtn->setVisible(false);
+  slcPriorityButton->setVisible(false);
   standardProfile->setVisible(false);
   relaxedProfile->setVisible(false);
 
   std::set<QString> longitudinalKeys = {"ConditionalExperimental", "CustomPersonalities", "ExperimentalModeActivation",
-                                        "LongitudinalTune", "MTSCEnabled"};
+                                        "LongitudinalTune", "MTSCEnabled", "SpeedLimitController"};
 
   for (auto &[key, toggle] : toggles) {
     toggle->setVisible(false);
@@ -801,8 +986,10 @@ void FrogPilotControlsPanel::hideToggles() {
 }
 
 void FrogPilotControlsPanel::hideSubToggles() {
+  slcPriorityButton->setVisible(false);
+
   for (auto &[key, toggle] : toggles) {
-    bool isVisible = false;
+    bool isVisible = speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end();
     toggle->setVisible(isVisible);
   }
 

selfdrive/frogpilot/ui/qt/offroad/control_settings.h

@@ -28,6 +28,7 @@ private:
   ButtonControl *deleteModelBtn;
   ButtonControl *downloadModelBtn;
   ButtonControl *selectModelBtn;
+  ButtonControl *slcPriorityButton;
 
   FrogPilotDualParamControl *aggressiveProfile;
   FrogPilotDualParamControl *conditionalSpeedsImperial;
@@ -46,10 +47,10 @@ private:
   std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "SmoothBraking", "StoppingDistance"};
   std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"};
   std::set<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"};
-  std::set<QString> speedLimitControllerKeys = {};
-  std::set<QString> speedLimitControllerControlsKeys = {};
-  std::set<QString> speedLimitControllerQOLKeys = {};
-  std::set<QString> speedLimitControllerVisualsKeys = {};
+  std::set<QString> speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"};
+  std::set<QString> speedLimitControllerControlsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"};
+  std::set<QString> speedLimitControllerQOLKeys = {"ForceMPHDashboard", "SetSpeedLimit", "SLCConfirmation", "SLCLookaheadHigher", "SLCLookaheadLower"};
+  std::set<QString> speedLimitControllerVisualsKeys = {"ShowSLCOffset", "SpeedLimitChangedAlert", "UseVienna"};
   std::set<QString> visionTurnControlKeys = {};
 
   std::map<std::string, ParamControl*> toggles;

selfdrive/manager/manager.py

@@ -250,6 +250,8 @@ def manager_init(frogpilot_functions) -> None:
     ("SLCConfirmationLower", "1"),
     ("SLCConfirmationHigher", "1"),
     ("SLCFallback", "2"),
+    ("SLCLookaheadHigher", "5"),
+    ("SLCLookaheadLower", "5"),
     ("SLCOverride", "1"),
     ("SLCPriority1", "Dashboard"),
     ("SLCPriority2", "Offline Maps"),

selfdrive/navd/navd.py

@@ -268,6 +268,7 @@ class RouteEngine:
 
     if self.step_idx is None:
       msg.valid = False
+      self.params_memory.put_float("NavSpeedLimit", 0)
       self.pm.send('navInstruction', msg)
       return
 
@@ -342,6 +343,9 @@ class RouteEngine:
 
     if ('maxspeed' in closest.annotations) and self.localizer_valid:
       msg.navInstruction.speedLimit = closest.annotations['maxspeed']
+      self.params_memory.put_float("NavSpeedLimit", closest.annotations['maxspeed'])
+    if not self.localizer_valid or ('maxspeed' not in closest.annotations):
+      self.params_memory.put_float("NavSpeedLimit", 0)
 
     # Speed limit sign type
     if 'speedLimitSign' in step:

selfdrive/sentry.py

@@ -101,8 +101,8 @@ def capture_fingerprint(params, candidate, blocked=False):
     "LaneDetectionWidth", "OneLaneChange", "LaneDetectionWidth", "QOLControls", "CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "HigherBitrate",
     "OnroadDistanceButton", "KaofuiIcons", "PauseLateralSpeed", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset", "SpeedLimitController", "Offset1",
     "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority", "SLCConfirmation", "SLCConfirmationLower", "SLCConfirmationHigher",
-    "ForceMPHDashboard", "SetSpeedLimit", "ShowSLCOffset", "ShowSLCOffsetUI", "UseVienna", "VisionTurnControl", "DisableVTSCSmoothing", "CurveSensitivity",
-    "TurnAggressiveness",
+    "ForceMPHDashboard", "SLCLookaheadHigher", "SLCLookaheadLower", "SetSpeedLimit", "ShowSLCOffset", "ShowSLCOffsetUI", "UseVienna", "VisionTurnControl",
+    "DisableVTSCSmoothing", "CurveSensitivity", "TurnAggressiveness",
   ], [
     "ForceFingerprint", "DisableOpenpilotLongitudinal", "EVTable", "LongPitch", "GasRegenCmd", "CrosstrekTorque", "LockDoors", "StockTune", "CydiaTune",
     "DragonPilotTune", "FrogsGoMooTune", "LockDoors", "SNGHack",

selfdrive/ui/qt/onroad.cc

@@ -115,7 +115,19 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
   QRect hideSpeedRect(rect().center().x() - 175, 50, 350, 350);
   bool isSpeedClicked = hideSpeedRect.contains(e->pos()) && scene.hide_speed_ui;
 
-  if (isMaxSpeedClicked || isSpeedClicked) {
+  // Speed limit confirmation buttons
+  QSize size = this->size();
+  QRect leftRect(0, 0, size.width() / 2, size.height());
+  QRect rightRect = leftRect.translated(size.width() / 2, 0);
+
+  bool isLeftSideClicked = leftRect.contains(e->pos()) && scene.speed_limit_changed;
+  bool isRightSideClicked = rightRect.contains(e->pos()) && scene.speed_limit_changed;
+
+  // Speed limit offset button
+  QRect speedLimitRect(7, 250, 225, 225);
+  bool isSpeedLimitClicked = speedLimitRect.contains(e->pos()) && scene.show_slc_offset_ui;
+
+  if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) {
     if (isMaxSpeedClicked) {
       std::thread([this]() {
         bool currentReverseCruise = scene.reverse_cruise;
@@ -133,8 +145,19 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
 
       uiState()->scene.hide_speed = !currentHideSpeed;
       params.putBoolNonBlocking("HideSpeed", !currentHideSpeed);
+    } else if (isSpeedLimitClicked) {
+      bool currentShowSLCOffset = scene.show_slc_offset;
+
+      scene.show_slc_offset = !currentShowSLCOffset;
+      params.putBoolNonBlocking("ShowSLCOffset", !currentShowSLCOffset);
     }
 
+    widgetClicked = true;
+  } else if (isLeftSideClicked || isRightSideClicked) {
+    bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive;
+    paramsMemory.putBoolNonBlocking("SLCConfirmed", slcConfirmed);
+    paramsMemory.putBoolNonBlocking("SLCConfirmedPressed", true);
+
     widgetClicked = true;
   // If the click wasn't for anything specific, change the value of "ExperimentalMode"
   } else if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) {
@@ -502,11 +525,14 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
 
   auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
-  speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
+  speedLimit = slcOverridden ? scene.speed_limit_overridden_speed : speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
   speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+  if (speedLimitController && !slcOverridden) {
+    speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
+  }
 
-  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
-  has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
+  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign);
+  has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) && !(speedLimitController && !useViennaSLCSign) || (speedLimitController && useViennaSLCSign);
   is_metric = s.scene.is_metric;
   speedUnit =  s.scene.is_metric ? tr("km/h") : tr("mph");
   hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE || customSignals != 0 && (turnSignalLeft || turnSignalRight) || bigMapOpen);
@@ -541,6 +567,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
 
   QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
+  QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
   QString speedStr = QString::number(std::nearbyint(speed));
   QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
 
@@ -615,11 +642,23 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
       p.setPen(QPen(blackColor(), 6));
       p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
 
-      p.setFont(InterFont(28, QFont::DemiBold));
-      p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
-      p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
-      p.setFont(InterFont(70, QFont::Bold));
-      p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+      p.save();
+      p.setOpacity(slcOverridden ? 0.25 : 1.0);
+      if (speedLimitController && showSLCOffset && !slcOverridden) {
+        p.setFont(InterFont(28, QFont::DemiBold));
+        p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+        p.setFont(InterFont(70, QFont::Bold));
+        p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+        p.setFont(InterFont(50, QFont::DemiBold));
+        p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
+      } else {
+        p.setFont(InterFont(28, QFont::DemiBold));
+        p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
+        p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+        p.setFont(InterFont(70, QFont::Bold));
+        p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+      }
+      p.restore();
     }
 
     // EU (Vienna style) sign
@@ -630,9 +669,19 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
       p.setPen(QPen(Qt::red, 20));
       p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
 
-      p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+      p.save();
+      p.setOpacity(slcOverridden ? 0.25 : 1.0);
       p.setPen(blackColor());
-      p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
+      if (showSLCOffset) {
+        p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+        p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr);
+        p.setFont(InterFont(40, QFont::DemiBold));
+        p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
+      } else {
+        p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+        p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
+      }
+      p.restore();
     }
   }
 
@@ -1213,6 +1262,12 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   roadNameUI = scene.road_name_ui;
 
+  speedLimitController = scene.speed_limit_controller;
+  showSLCOffset = speedLimitController && scene.show_slc_offset;
+  slcOverridden = speedLimitController && scene.speed_limit_overridden;
+  slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
+  useViennaSLCSign = scene.use_vienna_slc_sign;
+
   turnSignalLeft = scene.turn_signal_left;
   turnSignalRight = scene.turn_signal_right;
 
@@ -1272,6 +1327,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
     drawLeadInfo(p);
   }
 
+  if (scene.speed_limit_changed) {
+    drawSLCConfirmation(p);
+  }
+
   bool enableCompass = compass && !hideBottomIcons;
   compass_img->setVisible(enableCompass);
   if (enableCompass) {
@@ -1609,6 +1668,36 @@ void PedalIcons::paintEvent(QPaintEvent *event) {
   p.drawPixmap(gasX, (height() - img_size) / 2, gas_pedal_img);
 }
 
+void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) {
+  p.save();
+
+  QSize size = this->size();
+
+  QRect leftRect(0, 0, size.width() / 2, size.height());
+  QRect rightRect = leftRect.translated(size.width() / 2, 0);
+
+  p.setOpacity(0.5);
+  p.fillRect(leftRect, rightHandDM ? redColor() : greenColor());
+  p.fillRect(rightRect, rightHandDM ? greenColor() : redColor());
+  p.setOpacity(1.0);
+
+  p.setFont(InterFont(75, QFont::Bold));
+  p.setPen(Qt::white);
+
+  QString unitText = is_metric ? tr("kph") : tr("mph");
+  QString speedText = QString::number(std::nearbyint(scene.unconfirmed_speed_limit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText;
+  QString confirmText = tr("Confirm speed limit\n") + speedText;
+  QString ignoreText = tr("Ignore speed limit\n") + speedText;
+
+  QRect textLeftRect = QRect(leftRect.left(), leftRect.top() + leftRect.height() / 2 - 225, leftRect.width(), leftRect.height() / 2);
+  QRect textRightRect = QRect(rightRect.left(), rightRect.top() + rightRect.height() / 2 - 225, rightRect.width(), rightRect.height() / 2);
+
+  p.drawText(textLeftRect, Qt::AlignCenter, rightHandDM ? ignoreText : confirmText);
+  p.drawText(textRightRect, Qt::AlignCenter, rightHandDM ? confirmText : ignoreText);
+
+  p.restore();
+}
+
 void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.save();
 

selfdrive/ui/qt/onroad.h

@@ -187,6 +187,7 @@ private:
   void updateFrogPilotWidgets();
 
   void drawLeadInfo(QPainter &p);
+  void drawSLCConfirmation(QPainter &p);
   void drawStatusBar(QPainter &p);
   void drawTurnSignals(QPainter &p);
 
@@ -213,14 +214,19 @@ private:
   bool roadNameUI;
   bool showAlwaysOnLateralStatusBar;
   bool showConditionalExperimentalStatusBar;
+  bool showSLCOffset;
+  bool slcOverridden;
+  bool speedLimitController;
   bool turnSignalLeft;
   bool turnSignalRight;
+  bool useViennaSLCSign;
 
   float cruiseAdjustment;
   float distanceConversion;
   float laneDetectionWidth;
   float laneWidthLeft;
   float laneWidthRight;
+  float slcSpeedLimitOffset;
   float speedConversion;
 
   int alertSize;

selfdrive/ui/ui.cc

@@ -252,6 +252,7 @@ static void update_state(UIState *s) {
   if (sm.updated("frogpilotCarControl")) {
     auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
     scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
+    scene.speed_limit_changed = scene.speed_limit_controller && frogpilotCarControl.getSpeedLimitChanged();
   }
   if (sm.updated("frogpilotPlan")) {
     auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
@@ -265,7 +266,12 @@ static void update_state(UIState *s) {
     scene.lane_width_right = frogpilotPlan.getLaneWidthRight();
     scene.obstacle_distance = frogpilotPlan.getSafeObstacleDistance();
     scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock();
+    scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
+    scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
+    scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
+    scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed();
     scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor();
+    scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -380,6 +386,11 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.screen_timeout = screen_management ? params.getInt("ScreenTimeout") : 30;
   scene.screen_timeout_onroad = screen_management ? params.getInt("ScreenTimeoutOnroad") : 10;
   scene.standby_mode = screen_management && params.getBool("StandbyMode");
+
+  scene.speed_limit_controller = scene.longitudinal_control && params.getBool("SpeedLimitController");
+  scene.show_slc_offset = scene.speed_limit_controller && params.getBool("ShowSLCOffset");
+  scene.show_slc_offset_ui = scene.speed_limit_controller && params.getBool("ShowSLCOffsetUI");
+  scene.use_vienna_slc_sign = scene.speed_limit_controller && params.getBool("UseVienna");
 }
 
 void UIState::updateStatus() {

selfdrive/ui/ui.h

@@ -223,7 +223,12 @@ typedef struct UIScene {
   bool show_aol_status_bar;
   bool show_cem_status_bar;
   bool show_jerk;
+  bool show_slc_offset;
+  bool show_slc_offset_ui;
   bool show_tuning;
+  bool speed_limit_changed;
+  bool speed_limit_controller;
+  bool speed_limit_overridden;
   bool standby_mode;
   bool standstill;
   bool tethering_enabled;
@@ -232,6 +237,7 @@ typedef struct UIScene {
   bool unlimited_road_ui_length;
   bool use_kaofui_icons;
   bool use_si;
+  bool use_vienna_slc_sign;
   bool wake_up_screen;
 
   float acceleration;
@@ -249,6 +255,10 @@ typedef struct UIScene {
   float path_edge_width;
   float path_width;
   float road_edge_width;
+  float speed_limit;
+  float speed_limit_offset;
+  float speed_limit_overridden_speed;
+  float unconfirmed_speed_limit;
 
   int alert_size;
   int bearing_deg;