Speed Limit Controller

Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>

frogpilot/controls/frogpilot_planner.py

@@ -135,7 +135,7 @@ class FrogPilotPlanner:
 
     frogpilotPlan.desiredFollowDistance = int(self.frogpilot_following.desired_follow_distance)
 
-    frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode
+    frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode
 
     frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop
     frogpilotPlan.forcingStopLength = self.frogpilot_vcruise.tracked_model_length
@@ -158,6 +158,16 @@ class FrogPilotPlanner:
 
     frogpilotPlan.roadCurvature = self.road_curvature
 
+    frogpilotPlan.slcMapSpeedLimit = self.frogpilot_vcruise.slc.map_speed_limit
+    frogpilotPlan.slcMapboxSpeedLimit = self.frogpilot_vcruise.slc.mapbox_limit
+    frogpilotPlan.slcNextSpeedLimit = self.frogpilot_vcruise.slc.next_speed_limit
+    frogpilotPlan.slcOverriddenSpeed = self.frogpilot_vcruise.slc.overridden_speed
+    frogpilotPlan.slcSpeedLimit = self.frogpilot_vcruise.slc_target
+    frogpilotPlan.slcSpeedLimitOffset = self.frogpilot_vcruise.slc_offset
+    frogpilotPlan.slcSpeedLimitSource = self.frogpilot_vcruise.slc.source
+    frogpilotPlan.speedLimitChanged = self.frogpilot_vcruise.slc.speed_limit_changed_timer > DT_MDL
+    frogpilotPlan.unconfirmedSlcSpeedLimit = self.frogpilot_vcruise.slc.unconfirmed_speed_limit
+
     frogpilotPlan.themeUpdated = theme_updated
 
     frogpilotPlan.vCruise = float(self.v_cruise)

frogpilot/controls/lib/frogpilot_events.py

@@ -178,6 +178,9 @@ class FrogPilotEvents:
       else:
         self.events.add(FrogPilotEventName.openpilotCrashed)
 
+    if self.frogpilot_planner.frogpilot_vcruise.slc.speed_limit_changed_timer == DT_MDL and frogpilot_toggles.speed_limit_changed_alert:
+      self.events.add(FrogPilotEventName.speedLimitChanged)
+
     self.startup_seen |= sm["frogpilotSelfdriveState"].alertText1 == frogpilot_toggles.startup_alert_top and sm["frogpilotSelfdriveState"].alertText2 == frogpilot_toggles.startup_alert_bottom
 
     self.played_events.update(FROGPILOT_EVENT_NAME[event] for event in self.events.names)

frogpilot/controls/lib/frogpilot_vcruise.py

@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_MDL
 
 from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME
 from openpilot.frogpilot.controls.lib.curve_speed_controller import CurveSpeedController
+from openpilot.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 OVERRIDE_FORCE_STOP_TIMER = 10
 
@@ -12,6 +13,7 @@ class FrogPilotVCruise:
     self.frogpilot_planner = FrogPilotPlanner
 
     self.csc = CurveSpeedController(self)
+    self.slc = SpeedLimitController(self)
 
     self.forcing_stop = False
     self.override_force_stop = False
@@ -57,6 +59,24 @@ class FrogPilotVCruise:
 
       self.csc_target = v_cruise
 
+    # Pfeiferj's Speed Limit Controller
+    self.slc.frogpilot_toggles = frogpilot_toggles
+
+    if frogpilot_toggles.speed_limit_controller:
+      self.slc.update_limits(sm["frogpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm)
+      self.slc.update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm)
+
+      self.slc_offset = self.slc.offset
+      self.slc_target = self.slc.target
+    elif frogpilot_toggles.show_speed_limits:
+      self.slc.update_limits(sm["frogpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm)
+
+      self.slc_offset = 0
+      self.slc_target = self.slc.target
+    else:
+      self.slc_offset = 0
+      self.slc_target = 0
+
     if force_stop_enabled and not self.override_force_stop:
       self.forcing_stop |= not sm["carState"].standstill
 
@@ -69,6 +89,8 @@ class FrogPilotVCruise:
       self.tracked_model_length = self.frogpilot_planner.model_length
 
       targets = [self.csc_target, v_cruise]
+      if frogpilot_toggles.speed_limit_controller:
+        targets.append(max(self.slc.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff)
       v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets])
 
     return v_cruise

frogpilot/controls/lib/speed_limit_controller.py

@@ -0,0 +1,348 @@
+#!/usr/bin/env python3
+# PFEIFER - SLC - Modified by FrogAi for FrogPilot
+import calendar
+import numpy as np
+import requests
+
+from concurrent.futures import ThreadPoolExecutor
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+
+from openpilot.frogpilot.common.frogpilot_utilities import calculate_bearing_offset, calculate_distance_to_point, is_url_pingable
+
+FREE_MAPBOX_REQUESTS = 100_000
+
+OFFSET_MAP_IMPERIAL = [
+  (0, 11.2, "speed_limit_offset1"),     # 0–24 mph
+  (11.2, 15.2, "speed_limit_offset2"),  # 25–34
+  (15.2, 19.6, "speed_limit_offset3"),  # 35–44
+  (19.6, 24.1, "speed_limit_offset4"),  # 45–54
+  (24.1, 28.6, "speed_limit_offset5"),  # 55–64
+  (28.6, 33.1, "speed_limit_offset6"),  # 65–74
+  (33.1, 44.2, "speed_limit_offset7"),  # 75–99
+]
+
+OFFSET_MAP_METRIC = [
+  (0, 8.1, "speed_limit_offset1"),      # 0–29 km/h
+  (8.1, 13.6, "speed_limit_offset2"),   # 30–49
+  (13.6, 16.4, "speed_limit_offset3"),  # 50–59
+  (16.4, 21.9, "speed_limit_offset4"),  # 60–79
+  (21.9, 27.5, "speed_limit_offset5"),  # 80–99
+  (27.5, 33.1, "speed_limit_offset6"),  # 100–119
+  (33.1, 38.9, "speed_limit_offset7"),  # 120–140
+]
+
+class SpeedLimitController:
+  def __init__(self, FrogPilotVCruise):
+    self.frogpilot_planner = FrogPilotVCruise.frogpilot_planner
+
+    self.calling_mapbox = False
+    self.override_slc = False
+
+    self.denied_target = 0
+    self.map_speed_limit = 0
+    self.mapbox_limit = 0
+    self.next_speed_limit = 0
+    self.overridden_speed = 0
+    self.segment_distance = 0
+    self.speed_limit_changed_timer = 0
+    self.target = 0
+    self.unconfirmed_speed_limit = 0
+
+    self.previous_source = "None"
+    self.source = "None"
+
+    self.mapbox_requests = self.frogpilot_planner.params.get("MapBoxRequests")
+    self.mapbox_requests.setdefault("total_requests", 0)
+    self.mapbox_requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - (28 * 100))
+
+    self.mapbox_host = "https://api.mapbox.com"
+    self.mapbox_token = self.frogpilot_planner.params.get("MapboxSecretKey")
+
+    self.previous_target = self.frogpilot_planner.params.get("PreviousSpeedLimit")
+
+    self.executor = ThreadPoolExecutor(max_workers=1)
+
+    self.session = requests.Session()
+    self.session.headers.update({"Accept-Language": "en"})
+    self.session.headers.update({"User-Agent": "frogpilot-mapbox-speed-limit-retriever/1.0 (https://github.com/FrogAi/FrogPilot)"})
+
+  @property
+  def experimental_mode(self):
+    return self.target == 0 and self.frogpilot_toggles.slc_fallback_experimental_mode
+
+  @property
+  def offset(self):
+    offset_map = OFFSET_MAP_METRIC if self.frogpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL
+    return next((getattr(self.frogpilot_toggles, offset) for low, high, offset in offset_map if low < self.target < high), 0)
+
+  def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm):
+    if not self.frogpilot_planner.gps_position or not self.mapbox_token or (sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45:
+      self.mapbox_limit = 0
+      self.segment_distance = 0
+      return
+
+    if v_ego < 1:
+      return
+
+    if self.segment_distance > 0:
+      self.segment_distance -= v_ego * DT_MDL
+      return
+
+    if self.calling_mapbox:
+      self.segment_distance = v_ego
+      return
+
+    def make_request():
+      try:
+        self.calling_mapbox = True
+
+        successful = False
+
+        if not is_url_pingable(self.mapbox_host):
+          self.segment_distance = 1000
+          return None
+
+        if time_validated:
+          current_month = now.month
+          if current_month != self.mapbox_requests.get("month"):
+            self.mapbox_requests.update({
+              "month": current_month,
+              "total_requests": 0,
+              "max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, current_month)[1] * 100,
+            })
+
+        self.mapbox_requests["total_requests"] += 1
+        self.frogpilot_planner.params.put_nonblocking("MapBoxRequests", self.mapbox_requests)
+
+        current_bearing = self.frogpilot_planner.gps_position.get("bearing")
+        current_latitude = self.frogpilot_planner.gps_position.get("latitude")
+        current_longitude = self.frogpilot_planner.gps_position.get("longitude")
+
+        future_latitude, future_longitude = calculate_bearing_offset(current_latitude, current_longitude, current_bearing, v_ego)
+
+        url = (
+          f"{self.mapbox_host}/matching/v5/mapbox/driving/"
+          f"{current_longitude},{current_latitude};"
+          f"{future_longitude},{future_latitude}.json"
+        )
+
+        mapbox_params = {
+          "access_token": self.mapbox_token,
+          "annotations": "maxspeed,distance",
+          "geometries": "polyline6",
+          "overview": "full",
+          "steps": "false",
+          "radiuses": "10;10",
+          "tidy": "true",
+        }
+
+        response = self.session.get(url, params=mapbox_params, timeout=10)
+        response.raise_for_status()
+
+        successful = True
+
+        return response.json()
+      except Exception as exception:
+        print(f"Unexpected error in Mapbox request: {exception}")
+      finally:
+        self.calling_mapbox = False
+
+        if not successful:
+          self.mapbox_limit = 0
+          self.segment_distance = v_ego
+
+          return None
+
+    def complete_request(future):
+      try:
+        data = future.result()
+        if data:
+          matchings = data.get("matchings") or []
+          if not matchings:
+            self.mapbox_limit = 0
+            self.segment_distance = v_ego
+            return
+
+          legs = (matchings[0] or {}).get("legs") or []
+          if not legs:
+            self.mapbox_limit = 0
+            self.segment_distance = v_ego
+            return
+
+          annotation = legs[0].get("annotation") or {}
+
+          distances = annotation.get("distance") or [v_ego]
+          segment_distance = distances[0]
+
+          speed_data = annotation.get("maxspeed", [])
+          speed_limit_kph = 0
+          if speed_data:
+            first_segment_speed = speed_data[0]
+            speed_limit_kph = (first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0) or 0
+
+          if speed_limit_kph > 0:
+            self.mapbox_limit = speed_limit_kph * CV.KPH_TO_MS
+            self.segment_distance = segment_distance
+            return
+
+        self.mapbox_limit = 0
+        self.segment_distance = v_ego
+
+      except Exception as exception:
+        print(f"Mapbox Callback Error: {exception}")
+        self.mapbox_limit = 0
+        self.segment_distance = v_ego
+
+    future = self.executor.submit(make_request)
+    future.add_done_callback(complete_request)
+
+  def handle_limit_change(self, desired_source, desired_target, sm):
+    self.speed_limit_changed_timer += DT_MDL
+
+    speed_limit_accepted = (sm["frogpilotCarState"].accelPressed and sm["carControl"].longActive) or self.frogpilot_planner.params_memory.get_bool("SpeedLimitAccepted")
+    speed_limit_denied = sm["frogpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30)
+
+    if speed_limit_accepted:
+      self.overridden_speed = 0
+
+      self.source = desired_source
+      self.target = desired_target
+
+      self.frogpilot_planner.params_memory.remove("SpeedLimitAccepted")
+
+    elif speed_limit_denied:
+      self.denied_target = desired_target
+
+      self.previous_source = desired_source
+      self.previous_target = desired_target
+
+    elif desired_target < self.target and not self.frogpilot_toggles.speed_limit_confirmation_lower:
+      self.source = desired_source
+      self.target = desired_target
+
+    elif desired_target > self.target and not self.frogpilot_toggles.speed_limit_confirmation_higher:
+      self.source = desired_source
+      self.target = desired_target
+
+    else:
+      self.source = "None"
+      self.unconfirmed_speed_limit = desired_target
+
+    if self.target != self.previous_target and self.target > 0 and not speed_limit_denied:
+      self.denied_target = 0
+
+      self.previous_source = self.source
+      self.previous_target = self.target
+
+      self.frogpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target)
+
+  def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm):
+    self.update_map_speed_limit(v_ego)
+
+    limits = {
+      "Dashboard": dashboard_speed_limit,
+      "Map Data": self.map_speed_limit
+    }
+    filtered_limits = {source: limit for source, limit in limits.items() if limit >= 1}
+
+    if self.frogpilot_toggles.speed_limit_priority_highest:
+      desired_source = max(filtered_limits, key=filtered_limits.get, default="None")
+      desired_target = filtered_limits.get(desired_source, 0)
+
+    elif self.frogpilot_toggles.speed_limit_priority_lowest:
+      desired_source = min(filtered_limits, key=filtered_limits.get, default="None")
+      desired_target = filtered_limits.get(desired_source, 0)
+
+    elif filtered_limits:
+      for priority in [
+        self.frogpilot_toggles.speed_limit_priority1,
+        self.frogpilot_toggles.speed_limit_priority2
+      ]:
+        if priority in filtered_limits:
+          desired_source = priority
+          desired_target = filtered_limits[desired_source]
+          break
+      else:
+        desired_source = "None"
+        desired_target = 0
+
+    else:
+      desired_source = "None"
+      desired_target = 0
+
+    if desired_target == 0 or self.target == 0:
+      if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"] and self.frogpilot_toggles.slc_mapbox_filler:
+        self.get_mapbox_speed_limit(now, time_validated, v_ego, sm)
+
+        if self.mapbox_limit >= 1:
+          desired_source = "Mapbox"
+          desired_target = self.mapbox_limit
+
+      if desired_target == 0 or self.target == 0:
+        if self.denied_target != self.previous_target > 0 and self.frogpilot_toggles.slc_fallback_previous_speed_limit:
+          desired_source = self.previous_source
+          desired_target = self.previous_target
+
+          self.target = desired_target
+
+        elif sm["selfdriveState"].enabled and self.frogpilot_toggles.slc_fallback_set_speed:
+          desired_source = "None"
+          desired_target = v_cruise
+    else:
+      self.mapbox_limit = 0
+      self.segment_distance = 0
+
+    if abs(desired_target - self.previous_target) >= 1:
+      self.handle_limit_change(desired_source, desired_target, sm)
+    elif desired_source != self.source and abs(desired_target - self.target) < 1:
+      self.source = desired_source
+    else:
+      self.speed_limit_changed_timer = 0
+      self.unconfirmed_speed_limit = 0
+
+  def update_map_speed_limit(self, v_ego):
+    if not self.frogpilot_planner.gps_position:
+      return
+
+    self.map_speed_limit = self.frogpilot_planner.params_memory.get("MapSpeedLimit")
+
+    next_map_speed_limit = self.frogpilot_planner.params_memory.get("NextMapSpeedLimit")
+    self.next_speed_limit = next_map_speed_limit.get("speedlimit", 0)
+
+    if self.next_speed_limit:
+      current_latitude = self.frogpilot_planner.gps_position.get("latitude")
+      current_longitude = self.frogpilot_planner.gps_position.get("longitude")
+
+      next_latitude = next_map_speed_limit.get("latitude")
+      next_longitude = next_map_speed_limit.get("longitude")
+
+      distance_to_upcoming = calculate_distance_to_point(current_latitude, current_longitude, next_latitude, next_longitude)
+
+      if self.map_speed_limit < self.next_speed_limit:
+        max_lookahead = self.frogpilot_toggles.map_speed_lookahead_higher * v_ego
+      elif self.map_speed_limit > self.next_speed_limit:
+        max_lookahead = self.frogpilot_toggles.map_speed_lookahead_lower * v_ego
+      else:
+        max_lookahead = 0
+
+      if distance_to_upcoming < max_lookahead:
+        self.map_speed_limit = self.next_speed_limit
+
+  def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm):
+    self.override_slc = self.overridden_speed > self.target + self.offset > 0
+    self.override_slc |= sm["carState"].gasPressed and v_ego > self.target + self.offset > 0
+    self.override_slc &= sm["selfdriveState"].enabled
+
+    if self.override_slc:
+      if self.frogpilot_toggles.speed_limit_controller_override_manual:
+        if sm["carState"].gasPressed:
+          self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed)
+        self.overridden_speed = float(np.clip(self.overridden_speed, self.target + self.offset, v_cruise + v_cruise_diff))
+      elif self.frogpilot_toggles.speed_limit_controller_override_set_speed:
+        self.overridden_speed = v_cruise + v_cruise_diff
+
+      self.source = "None"
+    else:
+      self.overridden_speed = 0

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -8,7 +8,11 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent)
   brakePedalImg = loadPixmap("../../frogpilot/assets/other_images/brake_pedal.png", {btn_size, btn_size});
   curveSpeedIcon = loadPixmap("../../frogpilot/assets/other_images/curve_speed.png", {btn_size, btn_size});
   curveSpeedIconFlipped = curveSpeedIcon.transformed(QTransform().scale(-1, 1));
+  dashboardIcon = loadPixmap("../../frogpilot/assets/other_images/dashboard_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation);
   gasPedalImg = loadPixmap("../../frogpilot/assets/other_images/gas_pedal.png", {btn_size, btn_size});
+  mapboxIcon = loadPixmap("../../frogpilot/assets/other_images/mapbox_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation);
+  mapDataIcon = loadPixmap("../../frogpilot/assets/other_images/offline_maps_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation);
+  nextMapsIcon = loadPixmap("../../frogpilot/assets/other_images/next_maps_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation);
   pausedIcon = loadPixmap("../../frogpilot/assets/other_images/paused_icon.png", {widget_size, widget_size});
   speedIcon = loadPixmap("../../frogpilot/assets/other_images/speed_icon.png", {widget_size, widget_size});
   stopSignImg = loadPixmap("../../frogpilot/assets/other_images/stop_sign.png", {btn_size, btn_size});
@@ -155,6 +159,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   cscControllingSpeed = frogpilotPlan.getCscControllingSpeed();
   cscSpeed = frogpilotPlan.getCscSpeed();
   cscTraining = frogpilotPlan.getCscTraining();
+  dashboardSpeedLimit = frogpilotCarState.getDashboardSpeedLimit();
   desiredFollowDistance = frogpilotPlan.getDesiredFollowDistance();
   experimentalMode = selfdriveState.getExperimentalMode();
   forceCoast = frogpilotCarState.getForceCoast();
@@ -162,15 +167,30 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   laneWidthRight = frogpilotPlan.getLaneWidthRight();
   lateralPaused = frogpilotCarState.getPauseLateral();
   longitudinalPaused = frogpilotCarState.getPauseLongitudinal();
+  mapSpeedLimit = frogpilotPlan.getSlcMapSpeedLimit();
+  mapboxSpeedLimit = frogpilotPlan.getSlcMapboxSpeedLimit();
+  nextSpeedLimit = frogpilotPlan.getSlcNextSpeedLimit();
   redLight = frogpilotPlan.getRedLight();
   roadCurvature = frogpilotPlan.getRoadCurvature();
   roadName = QString::fromStdString(params_memory.get("RoadName"));
+  slcOverriddenSpeed = frogpilotPlan.getSlcOverriddenSpeed();
+  speedLimit = slcOverriddenSpeed != 0 ? slcOverriddenSpeed : frogpilotPlan.getSlcSpeedLimit();
+  speedLimitChanged = frogpilotPlan.getSpeedLimitChanged();
+  speedLimitSource = frogpilotPlan.getSlcSpeedLimitSource();
   stoppingDistance = modelV2.getPosition().getX().size() > 33 - 1 ? modelV2.getPosition().getX()[33 - 1] : 0.0;
+  unconfirmedSpeedLimit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
 
   hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE;
   hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE;
   hideBottomIcons |= signalStyle.startsWith("traditional") && (blinkerLeft || blinkerRight);
 
+  if (slcOverriddenSpeed == 0 && !frogpilot_toggles.value("show_speed_limit_offset").toBool()) {
+    speedLimit += frogpilotPlan.getSlcSpeedLimitOffset();
+  }
+  speedLimit *= (scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+  float speedLimitOffset = frogpilotPlan.getSlcSpeedLimitOffset() * speedConversion;
+  speedLimitOffsetStr = (speedLimitOffset != 0) ? QString::number(speedLimitOffset, 'f', 0).prepend((speedLimitOffset > 0) ? "+" : "-") : "–";
+
   static int lastFrameIndex;
   if (lastFrameIndex > animationFrameIndex && frogpilot_toggles.value("signal_icons").toString() == "frog") {
     frogHopCount++;
@@ -194,6 +214,14 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
     glowTimer.invalidate();
   }
 
+  if (speedLimitChanged) {
+    if (!pendingLimitTimer.isValid()) {
+      pendingLimitTimer.start();
+    }
+  } else {
+    pendingLimitTimer.invalidate();
+  }
+
   if (frogpilot_scene.standstill && frogpilot_toggles.value("stopped_timer").toBool()) {
     if (!standstillTimer.isValid()) {
       standstillTimer.start();
@@ -207,6 +235,12 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
 }
 
 void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) {
+  if (speedLimitChanged && newSpeedLimitRect.contains(mouseEvent->pos())) {
+    params_memory.putBool("SpeedLimitAccepted", true);
+    mouseEvent->accept();
+    return;
+  }
+
   mouseEvent->ignore();
 }
 
@@ -225,7 +259,7 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
     compassPosition.setY(0);
   }
 
-  if (!(signalStyle == "static" && blinkerLeft) && frogpilot_toggles.value("csc_status").toBool()) {
+  if (!speedLimitChanged && !(signalStyle == "static" && blinkerLeft) && frogpilot_toggles.value("csc_status").toBool()) {
     if (cscTraining) {
       paintCurveSpeedControlTraining(p);
     } else if (isCruiseSet && cscControllingSpeed) {
@@ -248,6 +282,10 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
     paintPedalIcons(p);
   }
 
+  if (speedLimitChanged) {
+    paintPendingSpeedLimit(p);
+  }
+
   if (frogpilot_toggles.value("radar_tracks").toBool()) {
     paintRadarTracks(p);
   }
@@ -256,6 +294,17 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
     paintRoadName(p);
   }
 
+  bool hideSpeedLimit = !speedLimitChanged && frogpilot_toggles.value("hide_speed_limit").toBool();
+  if (!hideSpeedLimit && (frogpilot_toggles.value("show_speed_limits").toBool() || frogpilot_toggles.value("speed_limit_controller").toBool())) {
+    paintSpeedLimit(p);
+  } else {
+    speedLimitHeight = 0;
+  }
+
+  if (frogpilot_toggles.value("speed_limit_sources").toBool()) {
+    paintSpeedLimitSources(p);
+  }
+
   if (standstillDuration != 0) {
     paintStandstillTimer(p);
   }
@@ -725,6 +774,41 @@ void FrogPilotAnnotatedCameraWidget::paintPedalIcons(QPainter &p) {
   p.restore();
 }
 
+void FrogPilotAnnotatedCameraWidget::paintPendingSpeedLimit(QPainter &p) {
+  p.save();
+
+  QString newSpeedLimitStr = (unconfirmedSpeedLimit > 1) ? QString::number(std::nearbyint(unconfirmedSpeedLimit * speedConversion)) : "–";
+  newSpeedLimitRect = speedLimitRect.translated(speedLimitRect.width() + UI_BORDER_SIZE, 0);
+
+  if (!frogpilot_toggles.value("speed_limit_vienna").toBool()) {
+    newSpeedLimitRect.setWidth(newSpeedLimitStr.size() >= 3 ? 200 : 175);
+
+    p.setBrush(whiteColor());
+    p.setPen(Qt::NoPen);
+    p.drawRoundedRect(newSpeedLimitRect, 24, 24);
+    p.setPen(pendingLimitTimer.elapsed() % 1000 < 500 ? QPen(blackColor(), 6) : QPen(redColor(), 6));
+    p.drawRoundedRect(newSpeedLimitRect.adjusted(9, 9, -9, -9), 16, 16);
+
+    p.setFont(InterFont(28, QFont::DemiBold));
+    p.drawText(newSpeedLimitRect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("PENDING"));
+    p.drawText(newSpeedLimitRect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+    p.setFont(InterFont(70, QFont::Bold));
+    p.drawText(newSpeedLimitRect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, newSpeedLimitStr);
+  } else {
+    p.setBrush(whiteColor());
+    p.setPen(Qt::NoPen);
+    p.drawEllipse(newSpeedLimitRect);
+    p.setPen(QPen(Qt::red, 20));
+    p.drawEllipse(newSpeedLimitRect.adjusted(16, 16, -16, -16));
+
+    p.setPen(pendingLimitTimer.elapsed() % 1000 < 500 ? QPen(blackColor(), 6) : QPen(redColor(), 6));
+    p.setFont(InterFont((newSpeedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+    p.drawText(newSpeedLimitRect, Qt::AlignCenter, newSpeedLimitStr);
+  }
+
+  p.restore();
+}
+
 void FrogPilotAnnotatedCameraWidget::paintRainbowPath(QPainter &p, QLinearGradient &bg, float lin_grad_point) {
   p.save();
 
@@ -797,6 +881,146 @@ void FrogPilotAnnotatedCameraWidget::paintRoadName(QPainter &p) {
   p.restore();
 }
 
+void FrogPilotAnnotatedCameraWidget::paintSpeedLimit(QPainter &p) {
+  if (setSpeedRect.isEmpty()) {
+    return;
+  }
+
+  p.save();
+
+  QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
+
+  bool hasUsSpeedLimit = !frogpilot_toggles.value("speed_limit_vienna").toBool();
+  bool hasEuSpeedLimit = !hasUsSpeedLimit;
+
+  int euSignSize = 176;
+  int usSignHeight = 186;
+  int signMargin = 12;
+
+  if (hasUsSpeedLimit) {
+    speedLimitHeight = usSignHeight + signMargin;
+  } else if (hasEuSpeedLimit) {
+    speedLimitHeight = euSignSize + signMargin;
+  }
+
+  QRect signRect;
+  if (hasUsSpeedLimit) {
+    signRect = QRect(setSpeedRect.x() + signMargin, setSpeedRect.bottom() - speedLimitHeight, setSpeedRect.width() - 2 * signMargin, usSignHeight);
+  } else if (hasEuSpeedLimit) {
+    signRect = QRect(setSpeedRect.x() + signMargin, setSpeedRect.bottom() - speedLimitHeight, setSpeedRect.width() - 2 * signMargin, euSignSize);
+  }
+  speedLimitRect = signRect;
+
+  if (hasUsSpeedLimit) {
+    p.setPen(Qt::NoPen);
+    p.setBrush(whiteColor());
+    p.drawRoundedRect(signRect, 24, 24);
+    p.setPen(QPen(blackColor(), 6));
+    p.drawRoundedRect(signRect.adjusted(9, 9, -9, -9), 16, 16);
+
+    p.setOpacity(slcOverriddenSpeed == 0 ? 1.0 : 0.25);
+    if (slcOverriddenSpeed == 0 && frogpilot_toggles.value("show_speed_limit_offset").toBool()) {
+      p.setFont(InterFont(28, QFont::DemiBold));
+      p.drawText(signRect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+      p.setFont(InterFont(70, QFont::Bold));
+      p.drawText(signRect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+      p.setFont(InterFont(50, QFont::DemiBold));
+      p.drawText(signRect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
+    } else {
+      p.setFont(InterFont(28, QFont::DemiBold));
+      p.drawText(signRect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
+      p.drawText(signRect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+      p.setFont(InterFont(70, QFont::Bold));
+      p.drawText(signRect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+    }
+  }
+
+  if (hasEuSpeedLimit) {
+    p.setPen(Qt::NoPen);
+    p.setBrush(whiteColor());
+    p.drawEllipse(signRect);
+    p.setPen(QPen(Qt::red, 20));
+    p.drawEllipse(signRect.adjusted(16, 16, -16, -16));
+
+    p.setOpacity(slcOverriddenSpeed == 0 ? 1.0 : 0.25);
+    p.setPen(blackColor());
+    if (frogpilot_toggles.value("show_speed_limit_offset").toBool()) {
+      p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+      p.drawText(signRect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr);
+      p.setFont(InterFont(40, QFont::DemiBold));
+      p.drawText(signRect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
+    } else {
+      p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+      p.drawText(signRect, Qt::AlignCenter, speedLimitStr);
+    }
+  }
+
+  p.restore();
+}
+
+void FrogPilotAnnotatedCameraWidget::paintSpeedLimitSources(QPainter &p) {
+  p.save();
+
+  std::function<void(QRect&, QPixmap&, const QString&, const double)> drawSource = [&](QRect &rect, QPixmap &icon, const QString &title, double speedLimitValue) {
+    bool isActive = QString::fromUtf8(speedLimitSource.c_str()) == title && speedLimitValue != 0;
+
+    if (isActive) {
+      p.setBrush(redColor(166));
+      p.setFont(InterFont(35, QFont::Bold));
+      p.setPen(QPen(redColor(), 10));
+    } else {
+      p.setBrush(blackColor(166));
+      p.setFont(InterFont(35, QFont::DemiBold));
+      p.setPen(QPen(blackColor(), 10));
+    }
+
+    QSize size(img_size / 4, img_size / 4);
+    QRect iconRect = QStyle::alignedRect(Qt::LeftToRight, Qt::AlignLeft | Qt::AlignVCenter, size, rect.adjusted(20, 0, 0, 0));
+
+    QString speedText;
+    if (speedLimitValue != 0) {
+      speedText = QString::number(std::nearbyint(speedLimitValue)) + speedUnit;
+    } else {
+      speedText = "N/A";
+    }
+
+    QString fullText = tr(title.toUtf8().constData()) + " - " + speedText;
+
+    p.setOpacity(1.0);
+    p.drawRoundedRect(rect, 24, 24);
+    p.drawPixmap(iconRect, icon);
+
+    p.setPen(QPen(whiteColor(), 6));
+    QRect textRect(iconRect.right() + 10, rect.y(), rect.width() - iconRect.width() - 30, rect.height());
+
+    if (isActive) {
+      QFontMetrics fm(p.font());
+      int textYPosition = textRect.y() + (textRect.height() - fm.height()) / 2 + fm.ascent();
+
+      QPainterPath path;
+      path.addText(textRect.x(), textYPosition, p.font(), fullText);
+      p.strokePath(path, QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+      p.drawText(textRect.x(), textYPosition, fullText);
+    } else {
+      p.drawText(textRect, Qt::AlignVCenter | Qt::AlignLeft, fullText);
+    }
+  };
+
+  int signMargin = 12;
+
+  QRect dashboardRect(speedLimitRect.x() - signMargin, speedLimitRect.y() + speedLimitRect.height() + UI_BORDER_SIZE, 450, 60);
+  QRect mapDataRect(dashboardRect.x(), dashboardRect.y() + dashboardRect.height() + UI_BORDER_SIZE / 2, 450, 60);
+  QRect mapboxRect(mapDataRect.x(), mapDataRect.y() + mapDataRect.height() + UI_BORDER_SIZE / 2, 450, 60);
+  QRect nextLimitRect(mapboxRect.x(), mapboxRect.y() + mapboxRect.height() + UI_BORDER_SIZE / 2, 450, 60);
+
+  drawSource(dashboardRect, dashboardIcon, "Dashboard", dashboardSpeedLimit * speedConversion);
+  drawSource(mapDataRect, mapDataIcon, "Map Data", mapSpeedLimit * speedConversion);
+  drawSource(mapboxRect, mapboxIcon, "Mapbox", mapboxSpeedLimit * speedConversion);
+  drawSource(nextLimitRect, nextMapsIcon, "Upcoming", nextSpeedLimit * speedConversion);
+
+  p.restore();
+}
+
 void FrogPilotAnnotatedCameraWidget::paintStandstillTimer(QPainter &p) {
   p.save();
 

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -25,6 +25,7 @@ public:
   bool rightHandDM;
 
   int alertHeight;
+  int speedLimitHeight;
   int standstillDuration;
 
   float speed;
@@ -65,8 +66,11 @@ private:
   void paintLateralPaused(QPainter &p);
   void paintLongitudinalPaused(QPainter &p);
   void paintPedalIcons(QPainter &p);
+  void paintPendingSpeedLimit(QPainter &p);
   void paintRadarTracks(QPainter &p);
   void paintRoadName(QPainter &p);
+  void paintSpeedLimit(QPainter &p);
+  void paintSpeedLimitSources(QPainter &p);
   void paintStandstillTimer(QPainter &p);
   void paintStoppingPoint(QPainter &p);
   void paintTurnSignals(QPainter &p);
@@ -84,6 +88,7 @@ private:
   bool lateralPaused;
   bool longitudinalPaused;
   bool redLight;
+  bool speedLimitChanged;
 
   int animationFrameIndex;
   int desiredFollowDistance;
@@ -96,14 +101,23 @@ private:
 
   float accelerationEgo;
   float cscSpeed;
+  float dashboardSpeedLimit;
   float distanceConversion;
   float laneWidthLeft;
   float laneWidthRight;
+  float mapSpeedLimit;
+  float mapboxSpeedLimit;
+  float nextSpeedLimit;
   float roadCurvature;
   float setSpeed;
+  float slcOverriddenSpeed;
   float speedConversion;
   float speedConversionMetrics;
+  float speedLimit;
   float stoppingDistance;
+  float unconfirmedSpeedLimit;
+
+  std::string speedLimitSource;
 
   Params params;
   Params params_memory{"", true};
@@ -112,12 +126,17 @@ private:
   QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }
 
   QElapsedTimer glowTimer;
+  QElapsedTimer pendingLimitTimer;
   QElapsedTimer standstillTimer;
 
   QPixmap brakePedalImg;
   QPixmap curveSpeedIcon;
   QPixmap curveSpeedIconFlipped;
+  QPixmap dashboardIcon;
   QPixmap gasPedalImg;
+  QPixmap mapboxIcon;
+  QPixmap mapDataIcon;
+  QPixmap nextMapsIcon;
   QPixmap pausedIcon;
   QPixmap speedIcon;
   QPixmap stopSignImg;
@@ -127,6 +146,9 @@ private:
   QPoint compassPosition;
   QPoint lateralPausedPosition;
 
+  QRect newSpeedLimitRect;
+  QRect speedLimitRect;
+
   QSharedPointer<QMovie> cemCurveIcon;
   QSharedPointer<QMovie> cemLeadIcon;
   QSharedPointer<QMovie> cemSpeedIcon;
@@ -138,6 +160,7 @@ private:
   QString leadDistanceUnit;
   QString leadSpeedUnit;
   QString roadName;
+  QString speedLimitOffsetStr;
   QString speedUnit;
 
   QTimer *animationTimer;

opendbc_repo/opendbc/car/toyota/carstate.py

@@ -25,6 +25,19 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
 PERM_STEER_FAULTS = (3, 17)
 
 
+# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team!
+def calculate_speed_limit(cp_cam):
+  speed_limit_unit = cp_cam.vl["RSA1"]["TSGN1"]
+  speed_limit_value = cp_cam.vl["RSA1"]["SPDVAL1"]
+
+  if speed_limit_unit == 1:
+    return speed_limit_value * CV.KPH_TO_MS
+  elif speed_limit_unit == 36:
+    return speed_limit_value * CV.MPH_TO_MS
+  else:
+    return 0
+
+
 class CarState(CarStateBase):
   def __init__(self, CP, FPCP):
     super().__init__(CP, FPCP)
@@ -217,6 +230,8 @@ class CarState(CarStateBase):
       *create_button_events(self.pcm_acc_status == 10, False, {1: ButtonType.decelCruise}),
     ]
 
+    fp_ret.dashboardSpeedLimit = calculate_speed_limit(cp_cam)
+
     if not self.CP.flags & ToyotaFlags.SECOC.value:
       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

selfdrive/selfdrived/events.py

@@ -1180,6 +1180,14 @@ FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.HIGHEST, VisualAlert.none, AudibleAlert.prompt, .1),
   },
 
+  FrogPilotEventName.speedLimitChanged: {
+    ET.PERMANENT: Alert(
+      "Speed limit changed",
+      "",
+      FrogPilotAlertStatus.frogpilot, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
   # Random Events
   FrogPilotEventName.accel30: {
     ET.WARNING: Alert(

selfdrive/ui/qt/onroad/hud.cc

@@ -64,6 +64,12 @@ void HudRenderer::drawSetSpeed(QPainter &p, const QRect &surface_rect) {
   QSize set_speed_size = is_metric ? QSize(200, 204) : default_size;
 
   // FrogPilot variables
+  if (frogpilot_nvg->speedLimitHeight != 0) {
+    set_speed_size.rheight() += frogpilot_nvg->speedLimitHeight;
+    if (frogpilot_toggles.value("speed_limit_vienna").toBool()) {
+      set_speed_size.rwidth() = 200;
+    }
+  }
 
   QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);