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>

cereal/custom.capnp

@@ -37,6 +37,10 @@ struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 {
   redLight @4 :Bool;
   safeObstacleDistance @5 :Int16;
   safeObstacleDistanceStock @6 :Int16;
+  slcOverridden @7 :Bool;
+  slcOverriddenSpeed @8 :Float32;
+  slcSpeedLimit @9 :Float32;
+  slcSpeedLimitOffset @10 :Float32;
   stoppedEquivalenceFactor @11 :Int16;
 }
 

common/params.cc

@@ -229,6 +229,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CameraView", PERSISTENT},
     {"CarMake", PERSISTENT},
     {"CarModel", PERSISTENT},
+    {"CarSpeedLimit", PERSISTENT},
     {"CECurves", PERSISTENT},
     {"CECurvesLead", PERSISTENT},
     {"CENavigation", PERSISTENT},
@@ -273,6 +274,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MapboxPublicKey", PERSISTENT},
     {"MapboxSecretKey", PERSISTENT},
     {"MapsSelected", PERSISTENT},
+    {"MapSpeedLimit", PERSISTENT},
+    {"MapSpeedLimitControl", PERSISTENT},
     {"MapTargetVelocities", PERSISTENT},
     {"Model", PERSISTENT},
     {"ModelList", PERSISTENT},
@@ -284,6 +287,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MuteOverheated", PERSISTENT},
     {"MuteSeatbelt", PERSISTENT},
     {"NavEnable", PERSISTENT},
+    {"NavSpeedLimit", PERSISTENT},
+    {"NavSpeedLimitControl", PERSISTENT},
     {"NNFF", PERSISTENT},
     {"NNFFModelFuzzyMatch", PERSISTENT},
     {"NNFFModelName", PERSISTENT},
@@ -291,6 +296,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},
     {"OSMDownloadLocations", PERSISTENT},
     {"OSMDownloadProgress", CLEAR_ON_MANAGER_START},
@@ -298,6 +307,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"PathWidth", PERSISTENT},
     {"PauseLateralOnSignal", PERSISTENT},
     {"PreferredSchedule", PERSISTENT},
+    {"PreviousSpeedLimit", PERSISTENT},
     {"RelaxedFollow", PERSISTENT},
     {"RelaxedJerk", PERSISTENT},
     {"ReverseCruise", PERSISTENT},
@@ -312,12 +322,18 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ShowFPS", PERSISTENT},
     {"ShowGPU", PERSISTENT},
     {"ShowMemoryUsage", PERSISTENT},
+    {"ShowSLCOffset", PERSISTENT},
     {"ShowStorageLeft", PERSISTENT},
     {"ShowStorageUsed", PERSISTENT},
     {"Sidebar", PERSISTENT},
     {"SilentMode", PERSISTENT},
+    {"SLCExperimentalMode", PERSISTENT},
+    {"SLCFallback", PERSISTENT},
+    {"SLCOverride", PERSISTENT},
+    {"SLCPriority", PERSISTENT},
     {"SmoothBraking", PERSISTENT},
     {"SNGHack", PERSISTENT},
+    {"SpeedLimitController", PERSISTENT},
     {"StandardFollow", PERSISTENT},
     {"StandardJerk", PERSISTENT},
     {"StoppingDistance", PERSISTENT},

release/files_common

@@ -561,3 +561,4 @@ selfdrive/frogpilot/functions/conditional_experimental_mode.py
 selfdrive/frogpilot/functions/frogpilot_planner.py
 selfdrive/frogpilot/functions/map_turn_speed_controller.py
 selfdrive/frogpilot/functions/model_switcher.py
+selfdrive/frogpilot/functions/speed_limit_controller.py

selfdrive/car/toyota/carstate.py

@@ -10,6 +10,7 @@ from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
 
 SteerControlType = car.CarParams.SteerControlType
 
@@ -46,6 +47,8 @@ class CarState(CarStateBase):
 
     # FrogPilot variables
 
+    self.traffic_signals = {}
+
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
 
@@ -178,8 +181,32 @@ class CarState(CarStateBase):
           self.param.put_bool("ExperimentalMode", not experimental_mode)
       self.lkas_previously_pressed = lkas_pressed
 
+    # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team!
+    self.update_traffic_signals(cp_cam)
+    SpeedLimitController.load_state()
+    SpeedLimitController.car_speed_limit = self.calculate_speed_limit()
+    SpeedLimitController.write_car_state()
+
     return ret
 
+  def update_traffic_signals(self, cp_cam):
+    signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"]
+    new_values = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals}
+
+    if new_values != self.traffic_signals:
+      self.traffic_signals.update(new_values)
+
+  def calculate_speed_limit(self):
+    tsgn1 = self.traffic_signals.get("TSGN1", None)
+    spdval1 = self.traffic_signals.get("SPDVAL1", None)
+
+    if tsgn1 == 1:
+      return spdval1 * CV.KPH_TO_MS
+    elif tsgn1 == 36:
+      return spdval1 * CV.MPH_TO_MS
+    else:
+      return 0
+
   @staticmethod
   def get_can_parser(CP):
     messages = [
@@ -231,6 +258,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/controls/controlsd.py

@@ -950,7 +950,7 @@ class Controls:
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
       if self.CP.openpilotLongitudinalControl and not self.conditional_experimental_mode:
-        self.experimental_mode = self.params.get_bool("ExperimentalMode")
+        self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.params_memory.get_bool("SLCExperimentalMode")
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")
       time.sleep(0.1)

selfdrive/frogpilot/functions/conditional_experimental_mode.py

@@ -99,6 +99,11 @@ class ConditionalExperimentalMode:
       self.status_value = 5
       return True
 
+    # Speed Limit Controller check
+    if self.params_memory.get_bool("SLCExperimentalMode"):
+      self.status_value = 6
+      return True
+
     # Speed check
     if (not self.lead_detected and v_ego < self.limit) or (self.lead_detected and v_ego < self.limit_lead):
       self.status_value = 7 if self.lead_detected else 8

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -10,6 +10,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 
 from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
 
 # Acceleration profiles - Credit goes to the DragonPilot team!
                  # MPH = [0.,  35,   35,  40,    40,  45,    45,  67,    67,   67, 123]
@@ -54,7 +55,11 @@ class FrogPilotPlanner:
     self.cem = ConditionalExperimentalMode()
     self.mtsc = MapTurnSpeedController()
 
+    self.override_slc = False
+
+    self.overridden_speed = 0
     self.mtsc_target = 0
+    self.slc_target = 0
     self.v_cruise = 0
 
     self.x_desired_trajectory = np.zeros(CONTROL_N)
@@ -99,8 +104,39 @@ class FrogPilotPlanner:
     else:
       self.mtsc_target = v_cruise
 
+    # Pfeiferj's Speed Limit Controller
+    if self.speed_limit_controller:
+      SpeedLimitController.update_current_max_velocity(v_cruise)
+      self.slc_target = SpeedLimitController.desired_speed_limit
+
+      # Override SLC upon gas pedal press and reset upon brake/cancel button
+      if self.speed_limit_controller_override:
+        self.override_slc |= carState.gasPressed
+        self.override_slc &= enabled
+        self.override_slc &= v_ego > self.slc_target
+      else:
+        self.override_slc = False
+
+      self.overridden_speed *= enabled
+
+      # Use the override speed if SLC is being overridden
+      if self.override_slc:
+        if self.speed_limit_controller_override == 1:
+          # Set the max speed to the manual set speed
+          if carState.gasPressed:
+            self.overridden_speed = np.clip(v_ego, self.slc_target, v_cruise)
+          self.slc_target = self.overridden_speed
+        elif self.speed_limit_controller_override == 2:
+          self.overridden_speed = v_cruise
+          self.slc_target = v_cruise
+      if self.slc_target == 0:
+        self.slc_target = v_cruise
+    else:
+      self.overriden_speed = 0
+      self.slc_target = v_cruise
+
     v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
-    return min(v_cruise, self.mtsc_target) - v_ego_diff
+    return min(v_cruise, self.mtsc_target, self.slc_target) - v_ego_diff
 
   def publish_lateral(self, sm, pm, DH):
     frogpilot_lateral_plan_send = messaging.new_message('frogpilotLateralPlan')
@@ -127,6 +163,11 @@ class FrogPilotPlanner:
     frogpilotLongitudinalPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor
     frogpilotLongitudinalPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock
 
+    frogpilotLongitudinalPlan.slcOverridden = self.override_slc
+    frogpilotLongitudinalPlan.slcOverriddenSpeed = float(self.overridden_speed)
+    frogpilotLongitudinalPlan.slcSpeedLimit = float(self.slc_target)
+    frogpilotLongitudinalPlan.slcSpeedLimitOffset = float(SpeedLimitController.offset)
+
     pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send)
 
   def update_frogpilot_params(self, params):
@@ -163,3 +204,8 @@ class FrogPilotPlanner:
     self.lane_change_delay = params.get_int("LaneChangeTime") if self.nudgeless else 0
     self.lane_detection = params.get_bool("LaneDetection") if self.nudgeless else False
     self.one_lane_change = params.get_bool("OneLaneChange") if self.nudgeless else False
+
+    self.speed_limit_controller = params.get_bool("SpeedLimitController")
+    if self.speed_limit_controller:
+      self.speed_limit_controller_override = params.get_int("SLCOverride")
+      SpeedLimitController.update_frogpilot_params()

selfdrive/frogpilot/functions/speed_limit_controller.py

@@ -0,0 +1,119 @@
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+import json
+
+params = Params()
+params_memory = Params("/dev/shm/params")
+
+class SpeedLimitController:
+  car_speed_limit: float = 0  # m/s
+  lst_speed_limit: float = 0  # m/s
+  map_speed_limit: float = 0  # m/s
+  nav_speed_limit: float = 0  # m/s
+
+  def __init__(self) -> None:
+    self.update_frogpilot_params()
+    self.write_car_state()
+    self.write_map_state()
+    self.write_nav_state()
+
+  def update_current_max_velocity(self, max_v: float, load_state: bool = True, write_state: bool = True) -> None:
+    if load_state:
+      self.load_state()
+
+  @property
+  def offset(self) -> float:
+    if self.speed_limit < 14:
+      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 speed_limit(self) -> float:
+    params_memory.put_bool("SLCExperimentalMode", False)
+
+    limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
+    filtered_limits = [limit for limit in limits if limit > 0]
+
+    if self.highest and filtered_limits:
+      return max(filtered_limits)
+    if self.lowest and filtered_limits:
+      return min(filtered_limits)
+
+    priority_orders = [
+      ["nav", "car", "map"],
+      ["nav", "map", "car"],
+      ["nav", "map"],
+      ["nav", "car"],
+      ["nav"],
+      ["map", "car", "nav"],
+      ["map", "nav", "car"],
+      ["map", "nav"],
+      ["map", "car"],
+      ["map"],
+      ["car", "nav", "map"],
+      ["car", "map", "nav"],
+      ["car", "map"],
+      ["car", "nav"],
+      ["car"]
+    ]
+
+    if self.speed_limit_priority < len(priority_orders):
+      for source in priority_orders[self.speed_limit_priority]:
+        limit = getattr(self, f"{source}_speed_limit", 0)
+        if limit > 0:
+          self.prv_speed_limit = limit
+          return limit
+
+    if self.use_experimental_mode:
+      params_memory.put_bool("SLCExperimentalMode", True)
+      return 0
+    elif self.use_previous_limit:
+      if self.lst_speed_limit != self.prv_speed_limit:
+        params.put_int("PreviousSpeedLimit", self.prv_speed_limit * 100)
+      self.lst_speed_limit = self.prv_speed_limit
+      return self.prv_speed_limit
+
+    return 0
+
+  @property
+  def desired_speed_limit(self):
+    return self.speed_limit + self.offset if self.speed_limit else 0
+
+  def load_state(self):
+    self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit"))
+    self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit"))
+    self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit"))
+
+  def write_car_state(self):
+    params_memory.put("CarSpeedLimit", json.dumps(self.car_speed_limit))
+
+  def write_map_state(self):
+    params_memory.put("MapSpeedLimit", json.dumps(self.map_speed_limit))
+
+  def write_nav_state(self):
+    params_memory.put("NavSpeedLimit", json.dumps(self.nav_speed_limit))
+
+  def update_frogpilot_params(self):
+    conversion = CV.KPH_TO_MS if params.get_bool("IsMetric") else CV.MPH_TO_MS
+
+    self.offset1 = params.get_int("Offset1") * conversion
+    self.offset2 = params.get_int("Offset2") * conversion
+    self.offset3 = params.get_int("Offset3") * conversion
+    self.offset4 = params.get_int("Offset4") * conversion
+
+    self.speed_limit_priority = params.get_int("SLCPriority")
+    self.highest = self.speed_limit_priority == 15
+    self.lowest = self.speed_limit_priority == 16
+
+    slc_fallback = params.get_int("SLCFallback")
+    self.use_experimental_mode = slc_fallback == 1
+    self.use_previous_limit = slc_fallback == 2
+
+    self.prv_speed_limit = params.get_int("PreviousSpeedLimit") / 100
+
+SpeedLimitController = SpeedLimitController()

selfdrive/frogpilot/ui/control_settings.cc

@@ -41,6 +41,15 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"LaneDetection", "Lane Detection", "Block nudgeless lane changes when a lane isn't detected.", ""},
     {"OneLaneChange", "One Lane Change Per Signal", "Limit to one nudgeless lane change per turn signal activation.", ""},
     {"PauseLateralOnSignal", "Pause Lateral On Turn Signal", "Temporarily disable lateral control during turn signal use.", ""},
+
+    {"SpeedLimitController", "Speed Limit Controller", "Automatically adjust vehicle speed to match speed limits using 'Open Street Map's, 'Navigate On openpilot', or your car's dashboard (TSS2 Toyotas only).", "../assets/offroad/icon_speed_limit.png"},
+    {"Offset1", "Speed Limit Offset (0-34 mph)", "Speed limit offset for speed limits between 0-34 mph.", ""},
+    {"Offset2", "Speed Limit Offset (35-54 mph)", "Speed limit offset for speed limits between 35-54 mph.", ""},
+    {"Offset3", "Speed Limit Offset (55-64 mph)", "Speed limit offset for speed limits between 55-64 mph.", ""},
+    {"Offset4", "Speed Limit Offset (65-99 mph)", "Speed limit offset for speed limits between 65-99 mph.", ""},
+    {"SLCFallback", "Fallback Method", "Choose your fallback method for when there are no speed limits currently being obtained from Navigation, OSM, and the car's dashboard.", ""},
+    {"SLCOverride", "Override Method", "Choose your preferred method to override the current speed limit.", ""},
+    {"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""},
   };
 
   for (const auto &[param, title, desc, icon] : controlToggles) {
@@ -223,6 +232,91 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
       }
       toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, laneChangeTimeLabels, this, false);
 
+    } else if (param == "SpeedLimitController") {
+      FrogPilotParamManageControl *speedLimitControllerToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
+      QObject::connect(speedLimitControllerToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        parentToggleClicked();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end());
+        }
+        slscPriorityButton->setVisible(true);
+      });
+      toggle = speedLimitControllerToggle;
+    } else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") {
+      toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map<int, QString>(), this, false, " mph");
+    } else if (param == "SLCFallback") {
+      std::vector<QString> fallbackOptions{tr("None"), 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("Max Set Speed")};
+      FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions);
+      toggle = overrideSelection;
+    } else if (param == "SLCPriority") {
+      const QStringList priorities {
+        "Navigation, Dashboard, Offline Maps",
+        "Navigation, Offline Maps, Dashboard",
+        "Navigation, Offline Maps",
+        "Navigation, Dashboard",
+        "Navigation",
+        "Offline Maps, Dashboard, Navigation",
+        "Offline Maps, Navigation, Dashboard",
+        "Offline Maps, Navigation",
+        "Offline Maps, Dashboard",
+        "Offline Maps",
+        "Dashboard, Navigation, Offline Maps",
+        "Dashboard, Offline Maps, Navigation",
+        "Dashboard, Offline Maps",
+        "Dashboard, Navigation",
+        "Dashboard",
+        "Highest",
+        "Lowest",
+        "",
+      };
+
+      slscPriorityButton = new ButtonControl(title, tr("SELECT"), desc);
+      QObject::connect(slscPriorityButton, &ButtonControl::clicked, this, [this, priorities]() {
+        QStringList availablePriorities = {"Dashboard", "Navigation", "Offline Maps", "Highest", "Lowest", "None"};
+        QStringList selectedPriorities;
+        int priorityValue = -1;
+
+        QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")};
+
+        for (int i = 0; i < 3; ++i) {
+          QString selection = MultiOptionDialog::getSelection(priorityPrompts[i], availablePriorities, "", this);
+          if (selection.isEmpty()) break;
+
+          if (selection == "None") {
+            priorityValue = 17;
+            break;
+          } else if (selection == "Highest") {
+            priorityValue = 15;
+            break;
+          } else if (selection == "Lowest") {
+            priorityValue = 16;
+            break;
+          } else {
+            selectedPriorities.append(selection);
+            availablePriorities.removeAll(selection);
+            availablePriorities.removeAll("Highest");
+            availablePriorities.removeAll("Lowest");
+          }
+        }
+
+        if (priorityValue == -1 && !selectedPriorities.isEmpty()) {
+          QString priorityString = selectedPriorities.join(", ");
+          priorityValue = priorities.indexOf(priorityString);
+        }
+
+        if (priorityValue != -1) {
+          slscPriorityButton->setValue(priorities[priorityValue]);
+          params.putInt("SLCPriority", priorityValue);
+          updateToggles();
+        }
+      });
+      slscPriorityButton->setValue(priorities[params.getInt("SLCPriority")]);
+      addItem(slscPriorityButton);
+
     } else {
       toggle = new ParamControl(param, title, desc, icon, this);
     }
@@ -261,7 +355,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
   laneChangeKeys = {"LaneChangeTime", "LaneDetection", "OneLaneChange", "PauseLateralOnSignal"};
   lateralTuneKeys = {"AverageCurvature", "NNFF"};
   longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
-  speedLimitControllerKeys = {};
+  speedLimitControllerKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"};
   visionTurnControlKeys = {};
 
   QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotControlsPanel::hideSubToggles);
@@ -289,17 +383,57 @@ void FrogPilotControlsPanel::updateMetric() {
     double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE;
     params.putInt("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion));
     params.putInt("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion));
+    params.putInt("Offset1", std::nearbyint(params.getInt("Offset1") * speedConversion));
+    params.putInt("Offset2", std::nearbyint(params.getInt("Offset2") * speedConversion));
+    params.putInt("Offset3", std::nearbyint(params.getInt("Offset3") * speedConversion));
+    params.putInt("Offset4", std::nearbyint(params.getInt("Offset4") * speedConversion));
     params.putInt("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion));
   }
 
+  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 *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(toggles["StoppingDistance"]);
 
   if (isMetric) {
+    offset1Toggle->setTitle("Speed Limit Offset (0-34 kph)");
+    offset2Toggle->setTitle("Speed Limit Offset (35-54 kph)");
+    offset3Toggle->setTitle("Speed Limit Offset (55-64 kph)");
+    offset4Toggle->setTitle("Speed Limit Offset (65-99 kph)");
+
+    offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 kph.");
+    offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 kph.");
+    offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 kph.");
+    offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 kph.");
+
+    offset1Toggle->updateControl(0, 99, " kph");
+    offset2Toggle->updateControl(0, 99, " kph");
+    offset3Toggle->updateControl(0, 99, " kph");
+    offset4Toggle->updateControl(0, 99, " kph");
     stoppingDistanceToggle->updateControl(0, 5, " meters");
   } else {
+    offset1Toggle->setTitle("Speed Limit Offset (0-34 mph)");
+    offset2Toggle->setTitle("Speed Limit Offset (35-54 mph)");
+    offset3Toggle->setTitle("Speed Limit Offset (55-64 mph)");
+    offset4Toggle->setTitle("Speed Limit Offset (65-99 mph)");
+
+    offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 mph.");
+    offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 mph.");
+    offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 mph.");
+    offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 mph.");
+
+    offset1Toggle->updateControl(0, 99, " mph");
+    offset2Toggle->updateControl(0, 99, " mph");
+    offset3Toggle->updateControl(0, 99, " mph");
+    offset4Toggle->updateControl(0, 99, " mph");
     stoppingDistanceToggle->updateControl(0, 10, " feet");
   }
 
+  offset1Toggle->refresh();
+  offset2Toggle->refresh();
+  offset3Toggle->refresh();
+  offset4Toggle->refresh();
   stoppingDistanceToggle->refresh();
 
   previousIsMetric = isMetric;
@@ -310,6 +444,7 @@ void FrogPilotControlsPanel::parentToggleClicked() {
   conditionalSpeedsImperial->setVisible(false);
   conditionalSpeedsMetric->setVisible(false);
   modelSelectorButton->setVisible(false);
+  slscPriorityButton->setVisible(false);
   standardProfile->setVisible(false);
   relaxedProfile->setVisible(false);
 
@@ -321,6 +456,7 @@ void FrogPilotControlsPanel::hideSubToggles() {
   conditionalSpeedsImperial->setVisible(false);
   conditionalSpeedsMetric->setVisible(false);
   modelSelectorButton->setVisible(true);
+  slscPriorityButton->setVisible(false);
   standardProfile->setVisible(false);
   relaxedProfile->setVisible(false);
 

selfdrive/frogpilot/ui/control_settings.h

@@ -22,6 +22,8 @@ private:
   void updateMetric();
   void updateToggles();
 
+  ButtonControl *slscPriorityButton;
+
   FrogPilotButtonIconControl *modelSelectorButton;
 
   FrogPilotDualParamControl *aggressiveProfile;

selfdrive/manager/process_config.py

@@ -56,7 +56,7 @@ def enable_logging(started, params, CP: car.CarParams) -> bool:
   return not (params.get_bool("FireTheBabysitter") and params.get_bool("NoLogging"))
 
 def osm(started, params, CP: car.CarParams) -> bool:
-  return params.get_bool("RoadNameUI")
+  return params.get_bool("RoadNameUI") or params.get_bool("SpeedLimitController")
 
 procs = [
   DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),

selfdrive/navd/navd.py

@@ -18,6 +18,8 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
                                     parse_banner_instructions)
 from openpilot.common.swaglog import cloudlog
 
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
+
 REROUTE_DISTANCE = 25
 MANEUVER_TRANSITION_THRESHOLD = 10
 REROUTE_COUNTER_MIN = 3
@@ -230,6 +232,12 @@ class RouteEngine:
 
     if self.step_idx is None:
       msg.valid = False
+      SpeedLimitController.load_state()
+      SpeedLimitController.nav_speed_limit = 0
+      SpeedLimitController.write_nav_state()
+
+      if SpeedLimitController.desired_speed_limit != 0:
+        msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
       self.pm.send('navInstruction', msg)
       return
 
@@ -304,6 +312,16 @@ class RouteEngine:
 
     if ('maxspeed' in closest.annotations) and self.localizer_valid:
       msg.navInstruction.speedLimit = closest.annotations['maxspeed']
+      SpeedLimitController.load_state()
+      SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed']
+      SpeedLimitController.write_nav_state()
+    if not self.localizer_valid or ('maxspeed' not in closest.annotations):
+      SpeedLimitController.load_state()
+      SpeedLimitController.nav_speed_limit = 0
+      SpeedLimitController.write_nav_state()
+
+    if SpeedLimitController.desired_speed_limit != 0:
+      msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
 
     # Speed limit sign type
     if 'speedLimitSign' in step:

selfdrive/ui/qt/onroad.cc

@@ -129,16 +129,23 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
   QRect speedRect(rect().center().x() - 175, 50, 350, 350);
   bool isSpeedClicked = speedRect.contains(e->pos());
 
-  if (isMaxSpeedClicked || isSpeedClicked) {
+  // Speed limit offset button
+  const QRect speedLimitRect(7, 250, 225, 225);
+  const bool isSpeedLimitClicked = speedLimitRect.contains(e->pos());
+
+  if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) {
     // Check if the click was within the max speed area
     if (isMaxSpeedClicked) {
       reverseCruise = !params.getBool("ReverseCruise");
       params.putBoolNonBlocking("ReverseCruise", reverseCruise);
       paramsMemory.putBoolNonBlocking("FrogPilotTogglesUpdated", true);
     // Check if the click was within the speed text area
-    } else {
+    } else if (isSpeedClicked) {
       speedHidden = !params.getBool("HideSpeed");
       params.putBoolNonBlocking("HideSpeed", speedHidden);
+    } else {
+      showSLCOffset = !params.getBool("ShowSLCOffset");
+      params.putBoolNonBlocking("ShowSLCOffset", showSLCOffset);
     }
     widgetClicked = true;
   // If the click wasn't for anything specific, change the value of "ExperimentalMode"
@@ -490,10 +497,13 @@ 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 ? slcOverriddenSpeed : slcSpeedLimit ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
   speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+  if (slcSpeedLimit && !slcOverridden) {
+    speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
+  }
 
-  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
+  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || slcSpeedLimit;
   has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
   is_metric = s.scene.is_metric;
   speedUnit =  s.scene.is_metric ? tr("km/h") : tr("mph");
@@ -527,6 +537,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 = (showSLCOffset) ? "+" + QString::number(std::nearbyint(slcSpeedLimitOffset)) : "–";
   QString speedStr = QString::number(std::nearbyint(speed));
   QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
 
@@ -599,12 +610,24 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     p.setPen(QPen(blackColor(), 6));
     p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
 
+    p.save();
+    p.setOpacity(slcOverridden ? 0.25 : 1.0);
+    if (showSLCOffset) {
+      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
   if (has_eu_speed_limit) {
@@ -1106,6 +1129,10 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
     reverseCruise = true;
   }
 
+  if (params.getBool("ShowSLCOffset")) {
+    showSLCOffset = true;
+  }
+
   // Custom themes configuration
   themeConfiguration = {
     {1, {QString("frog_theme"), {QColor(23, 134, 68, 242), {{0.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.9))},
@@ -1160,6 +1187,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
   obstacleDistanceStock = scene.obstacle_distance_stock;
   roadNameUI = scene.road_name_ui;
   showDriverCamera = scene.show_driver_camera;
+  slcOverridden = scene.speed_limit_overridden;
+  slcOverriddenSpeed = scene.speed_limit_overridden_speed;
+  slcSpeedLimit = scene.speed_limit;
+  slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
   stoppedEquivalence = scene.stopped_equivalence;
   turnSignalLeft = scene.turn_signal_left;
   turnSignalRight = scene.turn_signal_right;

selfdrive/ui/qt/onroad.h

@@ -18,6 +18,7 @@ const int img_size = (btn_size / 4) * 3;
 
 // FrogPilot global variables
 static bool reverseCruise;
+static bool showSLCOffset;
 static bool speedHidden;
 static double fps;
 
@@ -169,6 +170,7 @@ private:
   bool muteDM;
   bool roadNameUI;
   bool showDriverCamera;
+  bool slcOverridden;
   bool turnSignalLeft;
   bool turnSignalRight;
   bool useSI;
@@ -176,6 +178,9 @@ private:
   float cruiseAdjustment;
   float laneWidthLeft;
   float laneWidthRight;
+  float slcOverriddenSpeed;
+  float slcSpeedLimit;
+  float slcSpeedLimitOffset;
   int bearingDeg;
   int cameraView;
   int conditionalSpeed;

selfdrive/ui/ui.cc

@@ -255,6 +255,12 @@ static void update_state(UIState *s) {
       scene.obstacle_distance_stock = frogpilotLongitudinalPlan.getSafeObstacleDistanceStock();
       scene.stopped_equivalence = frogpilotLongitudinalPlan.getStoppedEquivalenceFactor();
     }
+    if (scene.speed_limit_controller) {
+      scene.speed_limit = frogpilotLongitudinalPlan.getSlcSpeedLimit();
+      scene.speed_limit_offset = frogpilotLongitudinalPlan.getSlcSpeedLimitOffset();
+      scene.speed_limit_overridden = frogpilotLongitudinalPlan.getSlcOverridden();
+      scene.speed_limit_overridden_speed = frogpilotLongitudinalPlan.getSlcOverriddenSpeed();
+    }
     scene.adjusted_cruise = frogpilotLongitudinalPlan.getAdjustedCruise();
   }
   if (sm.updated("liveLocationKalman")) {
@@ -321,6 +327,7 @@ void ui_update_params(UIState *s) {
   scene.mute_dm = params.getBool("FireTheBabysitter") && params.getBool("MuteDM");
   scene.rotating_wheel = params.getBool("RotatingWheel");
   scene.screen_brightness = params.getInt("ScreenBrightness");
+  scene.speed_limit_controller = params.getBool("SpeedLimitController");
   scene.wheel_icon = params.getInt("WheelIcon");
 }
 

selfdrive/ui/ui.h

@@ -193,6 +193,8 @@ typedef struct UIScene {
   bool rotating_wheel;
   bool show_driver_camera;
   bool show_fps;
+  bool speed_limit_controller;
+  bool speed_limit_overridden;
   bool tethering_enabled;
   bool turn_signal_left;
   bool turn_signal_right;
@@ -205,6 +207,9 @@ 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;
   int bearing_deg;
   int camera_view;
   int conditional_speed;