Speed Limit: Resolver (#1256)

* init * some fixes * move * more * old navd helpers * bring back cereal * fix linting * more * add to cereal first * sp events * lint * implement in long plan * fixme-sp * refactor state machine * wrong state * start refactor controller * some type hints * init these * enable debug print * ui? ui! * print them out * fix spinner import * fix path * let's use gps chips directly for now * service missing * publish events * no nav for now * need to sub * no car state speed yet * missed event * Car: `CarStateSP` * fix tests * bring back car state speed limit * fix * use old controller for now * fix * fix source * type hints * none for now * formatting * more * create directory if does not exist * mypy my bt * policy param catch exceptions * handle all params with exceptions * more * single method * define types in init * rename * simpler op enabled check * more mypy stuff * rename * no need for brake pressed * don't reset if gas pressed * type hint all * type hint all * back to upstream * in another pr * no longer need data type * qlog * slc in another pr * use horizontal accuracy * use horizontal accuracy * set core affinity for all realtime processes * unused * sort * unused * type hint and slight cleanup * from old implementation * use directly * combine pm * slight more cleanup * type hints * even more type hint * Revert "slc in another pr" This reverts commit 3a6987e6 * Revert "in another pr" This reverts commit a29bccff12b9ed9d64efe7a5722bf614214002c4. * rebump * no need to check alive * use it directly * fix test * refactor * use gps data directly * quote...? * lint * fix tests * use CC.longActive * user confirm in another PR * rename * fix import * params fix * no more * fix * drop new state machine for now * more fixes * internalize output * unused * rearrange * auto draft * rename * this * no * no need * use existing * wrong cruise speed * fix * not used for now * Revert "not used for now" This reverts commit f0083d6241380e84ed259c402941fa2c78e03fdf. * some * use frames instead * split speed limit resolver out of slc * no need to pass sm * fix params * test init * use frame instead of time * track session * some tests * too limiting * bump * always reset state * end session if long_active but slc inactive at any given time * off * no warning in this PR * no speed factor engage type yet * wide open * no * introduce disabled, no longer transitions at inactive * fix tests * no more tempinactive * clean * rename * offset default > off * new tests, fixes controller * more tests * not really needed yet * lint * fix * some more tests * wrap * more * more * use vCruiseCluster for set speed * init better * finish it up * no * typo * one method state machine * refactor preactive timeout check * refactor new session check * directly return statuses * comments * v_target * refactor speed limit resolver * turn off debug * more resolver refactor * no longer needed * lint * more lint * fix * move around * fix events * update event * already happens while in enabled * add carstateSP * less * Speed Limit Control -> Speed Limit Assist * in another PR * more rename * overriding state * fix * make sure to return the correct type * just slr in this one * more * update * redundant * fix * fix * lint * fix * fix * match toggle * fix priority checks * fix combined source for picking 0 limit * no need to wrap * add speed limit offset to resolver * add speed limit offset * make sure it displays distance when higher * Revert "make sure it displays distance when higher" This reverts commit 15c6834d4e68e2ba8f717c9442ec8d4d269d2b83. * some rename * translations * unused for now * more * lint --------- Co-authored-by: nayan <nayan8teen@gmail.com> Co-authored-by: DevTekVE <devtekve@gmail.com>
2025-09-19 18:32:20 -04:00
parent 28098bb7c4
commit ddf63701e8
20 changed files with 723 additions and 10 deletions
--- a/cereal/custom.capnp
+++ b/cereal/custom.capnp
@@ -145,6 +145,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
  dec @0 :DynamicExperimentalControl;
  longitudinalPlanSource @1 :LongitudinalPlanSource;
  smartCruiseControl @2 :SmartCruiseControl;
+  speedLimit @3 :SpeedLimit;

  struct DynamicExperimentalControl {
    state @0 :DynamicExperimentalControlState;
@@ -180,6 +181,23 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
    }
  }

+  struct SpeedLimit {
+    resolver @0 :Resolver;
+
+    struct Resolver {
+      speedLimit @0 :Float32;
+      distToSpeedLimit @1 :Float32;
+      source @2 :Source;
+      speedLimitOffset @3 :Float32;
+    }
+
+    enum Source {
+      none @0;
+      car @1;
+      map @2;
+    }
+  }
+
  enum LongitudinalPlanSource {
    cruise @0;
    sccVision @1;
@@ -316,6 +334,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 }

 struct CarStateSP @0xb86e6369214c01c8 {
+  speedLimit @0 :Float32;
 }

 struct LiveMapDataSP @0xf416ec09499d9d19 {
--- a/common/params_keys.h
+++ b/common/params_keys.h
@@ -225,4 +225,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"OsmStateTitle", {PERSISTENT, STRING}},
    {"OsmWayTest", {PERSISTENT, STRING}},
    {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
+
+    // Speed Limit
+    {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
+    {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
 };
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
+from openpilot.common.gps import get_gps_location_service
 from openpilot.common.params import Params
 from openpilot.common.realtime import Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
@@ -16,10 +17,13 @@ def main():
  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
  cloudlog.info("plannerd got CarParams: %s", CP.brand)

+  gps_location_service = get_gps_location_service(params)
+
  ldw = LaneDepartureWarning()
  longitudinal_planner = LongitudinalPlanner(CP)
  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
+                            'liveMapDataSP', 'carStateSP', gps_location_service],
                           poll='modelV2')

  while True:
--- a/selfdrive/test/longitudinal_maneuvers/plant.py
+++ b/selfdrive/test/longitudinal_maneuvers/plant.py
@@ -67,6 +67,9 @@ class Plant:
    lp = messaging.new_message('liveParameters')
    car_control = messaging.new_message('carControl')
    model = messaging.new_message('modelV2')
+    car_state_sp = messaging.new_message('carStateSP')
+    live_map_data_sp = messaging.new_message('liveMapDataSP')
+    gps_data = messaging.new_message('gpsLocation')
    a_lead = (v_lead - self.v_lead_prev)/self.ts
    self.v_lead_prev = v_lead

@@ -133,7 +136,10 @@ class Plant:
          'controlsState': control.controlsState,
          'selfdriveState': ss.selfdriveState,
          'liveParameters': lp.liveParameters,
-          'modelV2': model.modelV2}
+          'modelV2': model.modelV2,
+          'carStateSP': car_state_sp.carStateSP,
+          'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+          'gpsLocation': gps_data.gpsLocation}
    self.planner.update(sm)
    self.acceleration = self.planner.output_a_target
    self.speed = self.speed + self.acceleration * self.ts
--- a/selfdrive/ui/sunnypilot/SConscript
+++ b/selfdrive/ui/sunnypilot/SConscript
@@ -54,6 +54,8 @@ lateral_panel_qt_src = [

 longitudinal_panel_qt_src = [
  "sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc",
+  "sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc",
+  "sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc",
 ]

 network_src = [
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+#pragma once
+
+enum class SpeedLimitOffsetType {
+  NONE,
+  FIXED,
+  PERCENT,
+};
+
+inline const QString SpeedLimitOffsetTypeTexts[]{
+  QObject::tr("None"),
+  QObject::tr("Fixed"),
+  QObject::tr("Percent"),
+};
+
+enum class SpeedLimitSourcePolicy {
+  CAR_ONLY,
+  MAP_ONLY,
+  CAR_FIRST,
+  MAP_FIRST,
+  COMBINED,
+};
+
+inline const QString SpeedLimitSourcePolicyTexts[]{
+  QObject::tr("Car\nOnly"),
+  QObject::tr("Map\nOnly"),
+  QObject::tr("Car\nFirst"),
+  QObject::tr("Map\nFirst"),
+  QObject::tr("Combined\nData")
+};
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h"
+
+SpeedLimitPolicy::SpeedLimitPolicy(QWidget *parent) : QWidget(parent) {
+  QVBoxLayout *main_layout = new QVBoxLayout(this);
+  main_layout->setContentsMargins(0, 0, 0, 0);
+  main_layout->setSpacing(0);
+
+  // Back button
+  PanelBackButton *back = new PanelBackButton(tr("Back"));
+  connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
+  main_layout->addWidget(back, 0, Qt::AlignLeft);
+
+  main_layout->addSpacing(10);
+
+  ListWidgetSP *list = new ListWidgetSP(this);
+
+  std::vector<QString> speed_limit_policy_texts{
+    SpeedLimitSourcePolicyTexts[static_cast<int>(SpeedLimitSourcePolicy::CAR_ONLY)],
+    SpeedLimitSourcePolicyTexts[static_cast<int>(SpeedLimitSourcePolicy::MAP_ONLY)],
+    SpeedLimitSourcePolicyTexts[static_cast<int>(SpeedLimitSourcePolicy::CAR_FIRST)],
+    SpeedLimitSourcePolicyTexts[static_cast<int>(SpeedLimitSourcePolicy::MAP_FIRST)],
+    SpeedLimitSourcePolicyTexts[static_cast<int>(SpeedLimitSourcePolicy::COMBINED)]
+  };
+  speed_limit_policy = new ButtonParamControlSP(
+    "SpeedLimitPolicy",
+    tr("Speed Limit Source"),
+    "",
+    "",
+    speed_limit_policy_texts,
+    250);
+  list->addItem(speed_limit_policy);
+  connect(speed_limit_policy, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitPolicy::refresh);
+
+  refresh();
+  main_layout->addWidget(list);
+};
+
+void SpeedLimitPolicy::refresh() {
+  SpeedLimitSourcePolicy policy_param = static_cast<SpeedLimitSourcePolicy>(std::atoi(params.get("SpeedLimitPolicy").c_str()));
+  speed_limit_policy->setDescription(sourceDescription(policy_param));
+}
+
+void SpeedLimitPolicy::showEvent(QShowEvent *event) {
+  refresh();
+  speed_limit_policy->showDescription();
+}
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+
+class SpeedLimitPolicy : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit SpeedLimitPolicy(QWidget *parent = nullptr);
+  void refresh();
+  void showEvent(QShowEvent *event) override;
+
+signals:
+  void backPress();
+
+private:
+  Params params;
+  ButtonParamControlSP *speed_limit_policy;
+
+  static QString sourceDescription(SpeedLimitSourcePolicy type = SpeedLimitSourcePolicy::CAR_ONLY) {
+    QString car_only = tr("⦿ Car Only: Use Speed Limit data only from Car");
+    QString map_only = tr("⦿ Map Only: Use Speed Limit data only from OpenStreetMaps");
+    QString car_first = tr("⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps");
+    QString map_first = tr("⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car");
+    QString combined = tr("⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps");
+
+    if (type == SpeedLimitSourcePolicy::CAR_ONLY) {
+      car_only = "<font color='white'><b>" + car_only + "</b></font>";
+    } else if (type == SpeedLimitSourcePolicy::MAP_ONLY) {
+      map_only = "<font color='white'><b>" + map_only + "</b></font>";
+    } else if (type == SpeedLimitSourcePolicy::CAR_FIRST) {
+      car_first = "<font color='white'><b>" + car_first + "</b></font>";
+    } else if (type == SpeedLimitSourcePolicy::MAP_FIRST) {
+      map_first = "<font color='white'><b>" + map_first + "</b></font>";
+    } else {
+      combined = "<font color='white'><b>" + combined + "</b></font>";
+    }
+
+    return QString("%1<br>%2<br>%3<br>%4<br>%5")
+        .arg(car_only)
+        .arg(map_only)
+        .arg(car_first)
+        .arg(map_first)
+        .arg(combined);
+  }
+};
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h"
+
+SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) {
+  subPanelFrame = new QFrame();
+  QVBoxLayout *subPanelLayout = new QVBoxLayout(subPanelFrame);
+  subPanelLayout->setContentsMargins(0, 0, 0, 0);
+  subPanelLayout->setSpacing(0);
+
+  // Back button
+  PanelBackButton *back = new PanelBackButton(tr("Back"));
+  connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
+  subPanelLayout->addWidget(back, 0, Qt::AlignLeft);
+
+  subPanelLayout->addSpacing(20);
+
+  ListWidgetSP *list = new ListWidgetSP(this);
+
+  auto *speedLimitBtnFrame = new QFrame(this);
+  auto *speedLimitBtnFrameLayout = new QGridLayout();
+  speedLimitBtnFrame->setLayout(speedLimitBtnFrameLayout);
+  speedLimitBtnFrameLayout->setContentsMargins(0, 40, 0, 40);
+  speedLimitBtnFrameLayout->setSpacing(0);
+
+  speedLimitPolicyScreen = new SpeedLimitPolicy(this);
+
+  speedLimitSource = new PushButtonSP(tr("Customize Source"));
+  connect(speedLimitSource, &QPushButton::clicked, [&]() {
+    setCurrentWidget(speedLimitPolicyScreen);
+    speedLimitPolicyScreen->refresh();
+  });
+  connect(speedLimitPolicyScreen, &SpeedLimitPolicy::backPress, [&]() {
+    setCurrentWidget(subPanelFrame);
+    showEvent(new QShowEvent());
+  });
+
+  speedLimitSource->setFixedWidth(720);
+  speedLimitBtnFrameLayout->addWidget(speedLimitSource, 0, 0, Qt::AlignLeft);
+  list->addItem(speedLimitBtnFrame);
+
+  QFrame *offsetFrame = new QFrame(this);
+  QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame);
+
+  std::vector<QString> speed_limit_offset_texts{
+    SpeedLimitOffsetTypeTexts[static_cast<int>(SpeedLimitOffsetType::NONE)],
+    SpeedLimitOffsetTypeTexts[static_cast<int>(SpeedLimitOffsetType::FIXED)],
+    SpeedLimitOffsetTypeTexts[static_cast<int>(SpeedLimitOffsetType::PERCENT)]
+  };
+  speed_limit_offset_settings = new ButtonParamControlSP(
+    "SpeedLimitOffsetType",
+    tr("Speed Limit Offset"),
+    "",
+    "",
+    speed_limit_offset_texts,
+    500);
+
+  offsetLayout->addWidget(speed_limit_offset_settings);
+
+  speed_limit_offset = new OptionControlSP(
+    "SpeedLimitValueOffset",
+    "",
+    "",
+    "",
+    {-30, 30}
+    );
+  offsetLayout->addWidget(speed_limit_offset);
+
+  list->addItem(offsetFrame);
+
+  connect(speed_limit_offset, &OptionControlSP::updateLabels, this, &SpeedLimitSettings::refresh);
+  connect(speed_limit_offset_settings, &ButtonParamControlSP::showDescriptionEvent, speed_limit_offset, &OptionControlSP::showDescription);
+  connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
+
+  refresh();
+  subPanelLayout->addWidget(list);
+  addWidget(subPanelFrame);
+  addWidget(speedLimitPolicyScreen);
+  setCurrentWidget(subPanelFrame);
+}
+
+void SpeedLimitSettings::refresh() {
+  bool is_metric_param = params.getBool("IsMetric");
+  SpeedLimitOffsetType offset_type_param = static_cast<SpeedLimitOffsetType>(std::atoi(params.get("SpeedLimitOffsetType").c_str()));
+  QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset"));
+
+  speed_limit_offset->setDescription(offsetDescription(offset_type_param));
+
+  if (offset_type_param == SpeedLimitOffsetType::PERCENT) {
+    offsetLabel += "%";
+  } else if (offset_type_param == SpeedLimitOffsetType::FIXED) {
+    offsetLabel += QString(" %1").arg(is_metric_param ? "km/h" : "mph");
+  }
+
+  if (offset_type_param == SpeedLimitOffsetType::NONE) {
+    speed_limit_offset->setVisible(false);
+  } else {
+    speed_limit_offset->setVisible(true);
+    speed_limit_offset->setLabel(offsetLabel);
+    speed_limit_offset->showDescription();
+  }
+}
+
+void SpeedLimitSettings::showEvent(QShowEvent *event) {
+  refresh();
+  speed_limit_offset->showDescription();
+}
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+
+class SpeedLimitSettings : public QStackedWidget {
+  Q_OBJECT
+
+public:
+  SpeedLimitSettings(QWidget *parent = nullptr);
+  void refresh();
+  void showEvent(QShowEvent *event) override;
+
+signals:
+  void backPress();
+
+private:
+  Params params;
+  QFrame *subPanelFrame;
+  PushButtonSP *speedLimitSource;
+  SpeedLimitPolicy *speedLimitPolicyScreen;
+  ButtonParamControlSP *speed_limit_offset_settings;
+  OptionControlSP *speed_limit_offset;
+
+  static QString offsetDescription(SpeedLimitOffsetType type = SpeedLimitOffsetType::NONE) {
+    QString none_str = tr("⦿ None: No Offset");
+    QString fixed_str = tr("⦿ Fixed: Adds a fixed offset [Speed Limit + Offset]");
+    QString percent_str = tr("⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)]");
+
+    if (type == SpeedLimitOffsetType::FIXED) {
+      fixed_str = "<font color='white'><b>" + fixed_str + "</b></font>";
+    } else if (type == SpeedLimitOffsetType::PERCENT) {
+      percent_str = "<font color='white'><b>" + percent_str + "</b></font>";
+    } else {
+      none_str = "<font color='white'><b>" + none_str + "</b></font>";
+    }
+
+    return QString("%1<br>%2<br>%3")
+        .arg(none_str)
+        .arg(fixed_str)
+        .arg(percent_str);
+  }
+};
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc
@@ -8,6 +8,21 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h"

 LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
+  setStyleSheet(R"(
+    #back_btn {
+      font-size: 50px;
+      margin: 0px;
+      padding: 15px;
+      border-width: 0;
+      border-radius: 30px;
+      color: #dddddd;
+      background-color: #393939;
+    }
+    #back_btn:pressed {
+      background-color:  #4a4a4a;
+    }
+  )");
+
  main_layout = new QStackedLayout(this);
  ListWidget *list = new ListWidget(this, false);

@@ -40,7 +55,21 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {

  QObject::connect(uiState(), &UIState::offroadTransition, this, &LongitudinalPanel::refresh);

+  speedLimitSettings = new PushButtonSP(tr("Speed Limit"), 750, this);
+  connect(speedLimitSettings, &QPushButton::clicked, [&]() {
+    cruisePanelScroller->setLastScrollPosition();
+    main_layout->setCurrentWidget(speedLimitScreen);
+  });
+  list->addItem(speedLimitSettings);
+
+  speedLimitScreen = new SpeedLimitSettings(this);
+  connect(speedLimitScreen, &SpeedLimitSettings::backPress, [=]() {
+    cruisePanelScroller->restoreScrollPosition();
+    main_layout->setCurrentWidget(cruisePanelScreen);
+  });
+
  main_layout->addWidget(cruisePanelScreen);
+  main_layout->addWidget(speedLimitScreen);
  main_layout->setCurrentWidget(cruisePanelScreen);
  refresh(offroad);
 }
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h
@@ -8,6 +8,7 @@
 #pragma once

 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"

@@ -32,4 +33,6 @@ private:
  CustomAccIncrement *customAccIncrement = nullptr;
  ParamControl *SmartCruiseControlVision;
  ParamControl *intelligentCruiseButtonManagement = nullptr;
+  SpeedLimitSettings *speedLimitScreen;
+  PushButtonSP *speedLimitSettings;
 };
--- a/sunnypilot/mapd/live_map_data/debug.py
+++ b/sunnypilot/mapd/live_map_data/debug.py
@@ -37,15 +37,14 @@ def live_map_data_sp_thread():


 def live_map_data_sp_thread_debug(gps_location_service):
-  _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', gps_location_service])
+  _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service])
  _sub_master.update()

  v_ego = _sub_master['carState'].vEgo
-  long_spl = _sub_master['longitudinalPlanSP'].speedLimit
-  _policy = Policy.car_state_priority
-  _resolver = SpeedLimitResolver(_policy)
-  _speed_limit, _distance, _source = _resolver.resolve(v_ego, long_spl, _sub_master)
-  print(_speed_limit, _distance, _source, " <-> ", long_spl)
+  _resolver = SpeedLimitResolver()
+  _resolver.policy = Policy.car_state_priority
+  _resolver.update(v_ego, _sub_master)
+  print(_resolver.speed_limit, _resolver.distance, _resolver.source)


 def main():
--- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
@@ -9,6 +9,7 @@ from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
 from openpilot.sunnypilot.models.helpers import get_active_bundle

 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
@@ -19,6 +20,7 @@ class LongitudinalPlannerSP:
  def __init__(self, CP: structs.CarParams, mpc):
    self.dec = DynamicExperimentalController(CP, mpc)
    self.scc = SmartCruiseControl()
+    self.resolver = SpeedLimitResolver()
    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
    self.source = Source.cruise

@@ -36,6 +38,9 @@ class LongitudinalPlannerSP:
  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
    self.scc.update(sm, v_ego, a_ego, v_cruise)

+    # Speed Limit Resolver
+    self.resolver.update(v_ego, sm)
+
    targets = {
      Source.cruise: (v_cruise, a_ego),
      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target)
@@ -75,4 +80,12 @@ class LongitudinalPlannerSP:
    sccVision.enabled = self.scc.vision.is_enabled
    sccVision.active = self.scc.vision.is_active

+    # Speed Limit
+    speedLimit = longitudinalPlanSP.speedLimit
+    resolver = speedLimit.resolver
+    resolver.speedLimit = float(self.resolver.speed_limit)
+    resolver.speedLimitOffset = float(self.resolver.speed_limit_offset)
+    resolver.distToSpeedLimit = float(self.resolver.distance)
+    resolver.source = self.resolver.source
+
    pm.send('longitudinalPlanSP', plan_sp_send)
--- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py
+++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py
@@ -102,7 +102,7 @@ class SmartCruiseControlVision:
      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5

  def _update_state_machine(self) -> tuple[bool, bool]:
-    # ENABLED, ENTERING, TURNING, LEAVING
+    # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
    if self.state != VisionState.disabled:
      # longitudinal and feature disable always have priority in a non-disabled state
      if not self.long_enabled or not self.enabled:
@@ -163,7 +163,7 @@ class SmartCruiseControlVision:
    return enabled, active

  def _update_solution(self) -> float:
-    # DISABLED, ENABLED
+    # DISABLED, ENABLED, OVERRIDING
    if self.state not in ACTIVE_STATES:
      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
      # the smooth deceleration.
--- a/sunnypilot/selfdrive/controls/lib/speed_limit/init.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit/init.py
@@ -0,0 +1,8 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
+LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
--- a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py
@@ -0,0 +1,21 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from enum import IntEnum
+
+
+class Policy(IntEnum):
+  car_state_only = 0
+  map_data_only = 1
+  car_state_priority = 2
+  map_data_priority = 3
+  combined = 4
+
+
+class OffsetType(IntEnum):
+  off = 0
+  fixed = 1
+  percentage = 2
--- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py
@@ -0,0 +1,152 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import time
+
+import cereal.messaging as messaging
+from cereal import custom
+from openpilot.common.constants import CV
+from openpilot.common.gps import get_gps_location_service
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+
+ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
+
+
+class SpeedLimitResolver:
+  limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
+  distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
+  v_ego: float
+  speed_limit: float
+  distance: float
+  source: custom.LongitudinalPlanSP.SpeedLimit.Source
+  speed_limit_offset: float
+
+  def __init__(self):
+    self.params = Params()
+    self.frame = -1
+
+    self._gps_location_service = get_gps_location_service(self.params)
+    self.limit_solutions = {}  # Store for speed limit solutions from different sources
+    self.distance_solutions = {}  # Store for distance to current speed limit start for different sources
+
+    self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
+    self._policy_to_sources_map = {
+      Policy.car_state_only: [SpeedLimitSource.car],
+      Policy.map_data_only: [SpeedLimitSource.map],
+      Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
+      Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
+      Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
+    }
+    self.source = SpeedLimitSource.none
+    for source in ALL_SOURCES:
+      self._reset_limit_sources(source)
+
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
+    self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+
+  def update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
+      self.is_metric = self.params.get_bool("IsMetric")
+      self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
+      self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+
+  def _get_speed_limit_offset(self) -> float:
+    if self.offset_type == OffsetType.off:
+      return 0
+    elif self.offset_type == OffsetType.fixed:
+      return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS))
+    elif self.offset_type == OffsetType.percentage:
+      return float(self.offset_value * 0.01 * self.speed_limit)
+    else:
+      raise NotImplementedError("Offset not supported")
+
+  def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None:
+    self.limit_solutions[source] = 0.
+    self.distance_solutions[source] = 0.
+
+  def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
+    self._reset_limit_sources(SpeedLimitSource.car)
+    self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
+    self.distance_solutions[SpeedLimitSource.car] = 0.
+
+  def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
+    self._reset_limit_sources(SpeedLimitSource.map)
+    self._process_map_data(sm)
+
+  def _process_map_data(self, sm: messaging.SubMaster) -> None:
+    gps_data = sm[self._gps_location_service]
+    map_data = sm['liveMapDataSP']
+
+    gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
+    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
+      return
+
+    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
+    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
+
+    self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
+
+  def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
+    gps_data = sm[self._gps_location_service]
+    map_data = sm['liveMapDataSP']
+
+    distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
+    distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
+
+    self.limit_solutions[SpeedLimitSource.map] = speed_limit
+    self.distance_solutions[SpeedLimitSource.map] = 0.
+
+    if 0. < next_speed_limit < self.v_ego:
+      adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC
+      adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
+
+      if distance_to_speed_limit_ahead <= adapt_distance:
+        self.limit_solutions[SpeedLimitSource.map] = next_speed_limit
+        self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
+
+  def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source:
+    sources_for_policy = self._policy_to_sources_map[self.policy]
+
+    if self.policy != Policy.combined:
+      # They are ordered in the order of preference, so we pick the first that's non-zero
+      for source in sources_for_policy:
+        if self.limit_solutions[source] > 0.:
+          return source
+      return SpeedLimitSource.none
+
+    sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.]
+    if sources_with_limits:
+      return min(sources_with_limits, key=lambda x: x[1])[0]
+
+    return SpeedLimitSource.none
+
+  def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]:
+    """Get limit solutions from each data source"""
+    self._get_from_car_state(sm)
+    self._get_from_map_data(sm)
+
+    source = self._get_source_solution_according_to_policy()
+    speed_limit = self.limit_solutions[source] if source else 0.
+    distance = self.distance_solutions[source] if source else 0.
+
+    return speed_limit, distance, source
+
+  def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
+    self.v_ego = v_ego
+    self.update_params()
+
+    self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm)
+    self.speed_limit_offset = self._get_speed_limit_offset()
+
+    self.frame += 1
--- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/init.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/init.py
--- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py
@@ -0,0 +1,144 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import random
+import time
+
+import pytest
+from pytest_mock import MockerFixture
+
+from cereal import custom
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
+
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+
+
+def create_mock(properties, mocker: MockerFixture):
+  mock = mocker.MagicMock()
+  for _property, value in properties.items():
+    setattr(mock, _property, value)
+  return mock
+
+
+def setup_sm_mock(mocker: MockerFixture):
+  cruise_speed_limit = random.uniform(0, 120)
+  live_map_data_limit = random.uniform(0, 120)
+
+  car_state = create_mock({
+    'gasPressed': False,
+    'brakePressed': False,
+    'standstill': False,
+  }, mocker)
+  car_state_sp = create_mock({
+    'speedLimit': cruise_speed_limit,
+  }, mocker)
+  live_map_data = create_mock({
+    'speedLimit': live_map_data_limit,
+    'speedLimitValid': True,
+    'speedLimitAhead': 0.,
+    'speedLimitAheadValid': 0.,
+    'speedLimitAheadDistance': 0.,
+  }, mocker)
+  gps_data = create_mock({
+    'unixTimestampMillis': time.monotonic() * 1e3,
+  }, mocker)
+  sm_mock = mocker.MagicMock()
+  sm_mock.__getitem__.side_effect = lambda key: {
+    'carState': car_state,
+    'liveMapDataSP': live_map_data,
+    'carStateSP': car_state_sp,
+    'gpsLocation': gps_data,
+  }[key]
+  return sm_mock
+
+
+parametrized_policies = pytest.mark.parametrize(
+  "policy, sm_key, function_key", [
+    (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
+    (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
+    (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
+    (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
+  ],
+  ids=lambda val: val.name if hasattr(val, 'name') else str(val)
+)
+
+
+@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
+class TestSpeedLimitResolverValidation:
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_initial_state(self, resolver_class, policy):
+    resolver = resolver_class()
+    resolver.policy = policy
+    for source in ALL_SOURCES:
+      if source in resolver.limit_solutions:
+        assert resolver.limit_solutions[source] == 0.
+        assert resolver.distance_solutions[source] == 0.
+
+  @parametrized_policies
+  def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = setup_sm_mock(mocker)
+    source_speed_limit = sm_mock[sm_key].speedLimit
+
+    # Assert the resolver
+    resolver.update(source_speed_limit, sm_mock)
+    assert resolver.speed_limit == source_speed_limit
+    assert resolver.source == ALL_SOURCES[function_key]
+
+  def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = Policy.combined
+    sm_mock = setup_sm_mock(mocker)
+    socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
+    minimum_key, minimum_speed_limit = min(
+      ((key, sm_mock[key].speedLimit) for key in
+       socket_to_source.keys()), key=lambda x: x[1])
+
+    # Assert the resolver
+    resolver.update(minimum_speed_limit, sm_mock)
+    assert resolver.speed_limit == minimum_speed_limit
+    assert resolver.source == socket_to_source[minimum_key]
+
+  @parametrized_policies
+  def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = setup_sm_mock(mocker)
+    source_speed_limit = sm_mock[sm_key].speedLimit
+
+    # Assert the parsing
+    resolver.update(source_speed_limit, sm_mock)
+    assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
+    assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
+    v_ego = 50
+    resolver = resolver_class()
+    resolver.policy = policy
+
+    sm_mock = setup_sm_mock(mocker)
+    resolver.update(v_ego, sm_mock)
+
+    # After resolution
+    assert resolver.speed_limit is not None
+    assert resolver.distance is not None
+    assert resolver.source is not None
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = mocker.MagicMock()
+    sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
+    resolver._get_from_map_data(sm_mock)
+    assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
+    assert resolver.distance_solutions[SpeedLimitSource.map] == 0.