Merge branch 'master' of https://github.com/sunnypilot/sunnypilot

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;
@@ -231,7 +249,7 @@ struct OnroadEventSP @0xda96579883444c35 {
 struct CarParamsSP @0x80ae746ee2596b11 {
   flags @0 :UInt32;        # flags for car specific quirks in sunnypilot
   safetyParam @1 : Int16;  # flags for sunnypilot's custom safety flags
-  pcmCruiseSpeed @3 :Bool = true;
+  pcmCruiseSpeed @3 :Bool;
   intelligentCruiseButtonManagementAvailable @4 :Bool;
 
   neuralNetworkLateralControl @2 :NeuralNetworkLateralControl;
@@ -261,7 +279,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
 
     valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility.
   }
-  
+
   enum ParamType {
     string @0;
     bool @1;
@@ -316,6 +334,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 }
 
 struct CarStateSP @0xb86e6369214c01c8 {
+  speedLimit @0 :Float32;
 }
 
 struct LiveMapDataSP @0xf416ec09499d9d19 {

common/params_keys.h

@@ -235,4 +235,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"OsmStateTitle", {PERSISTENT, STRING}},
     {"OsmWayTest", {PERSISTENT, STRING}},
     {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
+
+    // Speed Limit
+    {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}},
+    {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
+    {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
 };

common/util.h

@@ -36,6 +36,7 @@ const double MS_TO_KPH = 3.6;
 const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE;
 const double METER_TO_MILE = KM_TO_MILE / 1000.0;
 const double METER_TO_FOOT = 3.28084;
+const double METER_TO_KM = 1. / 1000.0;
 
 #define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1))
 

selfdrive/car/tests/test_cruise_speed.py

@@ -44,12 +44,12 @@ class TestCruiseSpeed:
     assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s'
 
 
-# TODO: test pcmCruise
-@parameterized_class(('pcm_cruise',), [(False,)])
+# TODO: test pcmCruise and pcmCruiseSpeed
+@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
 class TestVCruiseHelper:
   def setup_method(self):
     self.CP = car.CarParams(pcmCruise=self.pcm_cruise)
-    self.CP_SP = custom.CarParamsSP()
+    self.CP_SP = custom.CarParamsSP(pcmCruiseSpeed=self.pcm_cruise_speed)
     self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP)
     self.reset_cruise_speed_state()
 

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:

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

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 = [

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h

@@ -0,0 +1,47 @@
+/*
+ * 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")
+};
+
+enum class SpeedLimitMode {
+  OFF,
+  INFORMATION,
+  WARNING,
+};
+
+inline const QString SpeedLimitModeTexts[]{
+  QObject::tr("Off"),
+  QObject::tr("Information"),
+  QObject::tr("Warning"),
+};

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();
+}

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);
+  }
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc

@@ -0,0 +1,130 @@
+/*
+ * 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, false);
+
+  speedLimitPolicyScreen = new SpeedLimitPolicy(this);
+
+  std::vector<QString> speed_limit_mode_texts{
+    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::OFF)],
+    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::INFORMATION)],
+    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::WARNING)],
+  };
+  speed_limit_mode_settings = new ButtonParamControlSP(
+    "SpeedLimitMode",
+    tr("Speed Limit Mode"),
+    "",
+    "",
+    speed_limit_mode_texts,
+    385);
+  list->addItem(speed_limit_mode_settings);
+
+  list->addItem(horizontal_line());
+  list->addItem(vertical_space());
+
+  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);
+  list->addItem(speedLimitSource);
+
+  list->addItem(vertical_space(0));
+  list->addItem(horizontal_line());
+
+  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_mode_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
+  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");
+  SpeedLimitMode speed_limit_mode_param = static_cast<SpeedLimitMode>(std::atoi(params.get("SpeedLimitMode").c_str()));
+  SpeedLimitOffsetType offset_type_param = static_cast<SpeedLimitOffsetType>(std::atoi(params.get("SpeedLimitOffsetType").c_str()));
+  QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset"));
+
+  speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param));
+  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();
+  }
+
+  speed_limit_mode_settings->showDescription();
+  speed_limit_offset->showDescription();
+}
+
+void SpeedLimitSettings::showEvent(QShowEvent *event) {
+  refresh();
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h

@@ -0,0 +1,73 @@
+/*
+ * 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;
+  ButtonParamControlSP *speed_limit_mode_settings;
+  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);
+  }
+
+  static QString modeDescription(SpeedLimitMode mode = SpeedLimitMode::OFF) {
+    QString off_str = tr("⦿ Off: Disables the Speed Limit functions.");
+    QString info_str = tr("⦿ Information: Displays the current road's speed limit.");
+    QString warning_str = tr("⦿ Warning: Provides a warning when exceeding the current road's speed limit.");
+
+    if (mode == SpeedLimitMode::WARNING) {
+      warning_str = "<font color='white'><b>" + warning_str + "</b></font>";
+    } else if (mode == SpeedLimitMode::INFORMATION) {
+      info_str = "<font color='white'><b>" + info_str + "</b></font>";
+    } else {
+      off_str = "<font color='white'><b>" + off_str + "</b></font>";
+    }
+
+    return QString("%1<br>%2<br>%3")
+        .arg(off_str)
+        .arg(info_str)
+        .arg(warning_str);
+  }
+};

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);
 }

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;
 };

selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc

@@ -25,21 +25,28 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
       "BlindSpot",
       tr("Show Blind Spot Warnings"),
       tr("Enabling this will display warnings when a vehicle is detected in your blind spot as long as your car has BSM supported."),
-      "../assets/offroad/icon_monitoring.png",
+      "",
       false,
     },
     {
       "RainbowMode",
       tr("Enable Tesla Rainbow Mode"),
       RainbowizeWords(tr("A beautiful rainbow effect on the path the model wants to take.")) + "<br/><i>" + tr("It")+ " <b>" + tr("does not") + "</b> " + tr("affect driving in any way.") + "</i>",
-      "../assets/offroad/icon_monitoring.png",
+      "",
       false,
     },
     {
       "StandstillTimer",
       tr("Enable Standstill Timer"),
       tr("Show a timer on the HUD when the car is at a standstill."),
-      "../assets/offroad/icon_monitoring.png",
+      "",
+      false,
+    },
+    {
+      "RoadName",
+      tr("Display Road Name"),
+      tr("Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name."),
+      "",
       false,
     },
   };

selfdrive/ui/sunnypilot/qt/onroad/hud.cc

@@ -17,7 +17,6 @@ void HudRendererSP::updateState(const UIState &s) {
   HudRenderer::updateState(s);
 
   const SubMaster &sm = *(s.sm);
-  const bool cs_alive = sm.alive("controlsState");
   const auto cs = sm["controlsState"].getControlsState();
   const auto car_state = sm["carState"].getCarState();
   const auto car_control = sm["carControl"].getCarControl();
@@ -27,6 +26,25 @@ void HudRendererSP::updateState(const UIState &s) {
   const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters();
   const auto car_params = sm["carParams"].getCarParams();
   const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+  const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP();
+
+  float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH;
+  speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv;
+  speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv;
+  speedLimitMode = static_cast<SpeedLimitMode>(s.scene.speed_limit_mode);
+  roadName = s.scene.road_name;
+  if (sm.updated("liveMapDataSP")) {
+    roadNameStr = QString::fromStdString(lmd.getRoadName());
+    speedLimitAheadValid = lmd.getSpeedLimitAheadValid();
+    speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv;
+    speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance();
+    if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < 2) {
+      speedLimitAheadValidFrame++;
+    } else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) {
+      speedLimitAheadValidFrame--;
+    }
+  }
+  speedLimitAheadDistancePrev = speedLimitAheadDistance;
 
   static int reverse_delay = 0;
   bool reverse_allowed = false;
@@ -41,13 +59,6 @@ void HudRendererSP::updateState(const UIState &s) {
   }
 
   reversing = reverse_allowed;
-  is_metric = s.scene.is_metric;
-
-  // Handle older routes where vEgoCluster is not set
-  v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0;
-  float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo();
-  speed = cs_alive ? std::max<float>(0.0, v_ego) : 0.0;
-  speed *= is_metric ? MS_TO_KPH : MS_TO_MPH;
 
   latActive = car_control.getLatActive();
   steerOverride = car_state.getSteeringPressed();
@@ -123,6 +134,15 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
     if (standstillTimer) {
       drawStandstillTimer(p, surface_rect.right() / 12 * 10, surface_rect.bottom() / 12 * 1.53);
     }
+
+    // Speed Limit
+    if (speedLimitMode != SpeedLimitMode::OFF) {
+      drawSpeedLimitSigns(p);
+      drawUpcomingSpeedLimit(p);
+    }
+
+    // Road Name
+    drawRoadName(p, surface_rect);
   }
 }
 
@@ -311,3 +331,223 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
     standstillElapsedTime = 0.0;
   }
 }
+
+void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
+  int speedLimitRounded = std::nearbyint(speedLimit);
+  bool overspeed = speedLimitRounded < std::nearbyint(speed) && speedLimitRounded > 0;
+  bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING;
+  QString speedLimitStr = speedLimit > 0 ? QString::number(speedLimitRounded) : "---";
+
+  // Offset display text
+  QString speedLimitSubText = "";
+  if (speedLimitOffset != 0) {
+    speedLimitSubText = (speedLimitOffset > 0 ? "+" : "") + QString::number(std::nearbyint(speedLimitOffset));
+  }
+
+  // Position next to MAX speed box
+  const int sign_width = is_metric ? 200 : 172;
+  const int sign_x = is_metric ? 280 : 272;
+  const int sign_y = 45;
+  const int sign_height = 204;
+  QRect sign_rect(sign_x, sign_y, sign_width, sign_height);
+
+  int alpha = 255;
+  QColor red_color = QColor(255, 0, 0, alpha);
+  QColor speed_color = (speedLimitWarningEnabled && overspeed) ? red_color : QColor(0, 0, 0, alpha);
+
+  if (is_metric) {
+    // EU Vienna Convention style circular sign
+    QRect vienna_rect = sign_rect;
+    int circle_size = std::min(vienna_rect.width(), vienna_rect.height());
+    QRect circle_rect(vienna_rect.x(), vienna_rect.y(), circle_size, circle_size);
+
+    if (vienna_rect.width() > vienna_rect.height()) {
+      circle_rect.moveLeft(vienna_rect.x() + (vienna_rect.width() - circle_size) / 2);
+    } else if (vienna_rect.height() > vienna_rect.width()) {
+      circle_rect.moveTop(vienna_rect.y() + (vienna_rect.height() - circle_size) / 2);
+    }
+
+    // White background circle
+    p.setPen(Qt::NoPen);
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawEllipse(circle_rect);
+
+    // Red border ring with color coding
+    QRect red_ring = circle_rect;
+
+    p.setBrush(red_color);
+    p.drawEllipse(red_ring);
+
+    // Center white circle for text
+    int ring_size = circle_size * 0.12;
+    QRect center_circle = red_ring.adjusted(ring_size, ring_size, -ring_size, -ring_size);
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawEllipse(center_circle);
+
+    // Speed value, smaller font for 3+ digits
+    int font_size = (speedLimitStr.size() >= 3) ? 70 : 85;
+    p.setFont(InterFont(font_size, QFont::Bold));
+
+    p.setPen(speed_color);
+    p.drawText(center_circle, Qt::AlignCenter, speedLimitStr);
+
+    // Offset value in small circular box
+    if (!speedLimitSubText.isEmpty()) {
+      int offset_circle_size = circle_size * 0.4;
+      int overlap = offset_circle_size * 0.25;
+      QRect offset_circle_rect(
+        circle_rect.right() - offset_circle_size/1.25 + overlap,
+        circle_rect.top() - offset_circle_size/1.75 + overlap,
+        offset_circle_size,
+        offset_circle_size
+      );
+
+      p.setPen(QPen(QColor(77, 77, 77, 255), 6));
+      p.setBrush(QColor(0, 0, 0, alpha));
+      p.drawEllipse(offset_circle_rect);
+
+      p.setFont(InterFont(offset_circle_size * 0.45, QFont::Bold));
+      p.setPen(QColor(255, 255, 255, alpha));
+      p.drawText(offset_circle_rect, Qt::AlignCenter, speedLimitSubText);
+    }
+  } else {
+    // US/Canada MUTCD style sign
+    p.setPen(Qt::NoPen);
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawRoundedRect(sign_rect, 32, 32);
+
+    // Inner border with violation color coding
+    QRect inner_rect = sign_rect.adjusted(10, 10, -10, -10);
+    QColor border_color = QColor(0, 0, 0, alpha);
+
+    p.setPen(QPen(border_color, 4));
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawRoundedRect(inner_rect, 22, 22);
+
+    // "SPEED LIMIT" text
+    p.setFont(InterFont(40, QFont::DemiBold));
+    p.setPen(QColor(0, 0, 0, alpha));
+    p.drawText(inner_rect.adjusted(0, 10, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
+    p.drawText(inner_rect.adjusted(0, 50, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+
+    // Speed value with color coding
+    p.setFont(InterFont(90, QFont::Bold));
+
+    p.setPen(speed_color);
+    p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+
+    // Offset value in small box
+    if (!speedLimitSubText.isEmpty()) {
+      int offset_box_size = sign_rect.width() * 0.4;
+      int overlap = offset_box_size * 0.25;
+      QRect offset_box_rect(
+        sign_rect.right() - offset_box_size/1.5 + overlap,
+        sign_rect.top() - offset_box_size/1.25 + overlap,
+        offset_box_size,
+        offset_box_size
+      );
+
+      int corner_radius = offset_box_size * 0.2;
+      p.setPen(QPen(QColor(77, 77, 77, 255), 6));
+      p.setBrush(QColor(0, 0, 0, alpha));
+      p.drawRoundedRect(offset_box_rect, corner_radius, corner_radius);
+
+      p.setFont(InterFont(offset_box_size * 0.45, QFont::Bold));
+      p.setPen(QColor(255, 255, 255, alpha));
+      p.drawText(offset_box_rect, Qt::AlignCenter, speedLimitSubText);
+    }
+  }
+}
+
+void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) {
+  bool speed_limit_ahead = speedLimitAheadValid && speedLimitAhead > 0 && speedLimitAhead != speedLimit && speedLimitAheadValidFrame > 0;
+  if (!speed_limit_ahead) {
+    return;
+  }
+
+  auto roundToInterval = [&](float distance, int interval, int threshold) {
+    int base = static_cast<int>(distance / interval) * interval;
+    return (distance - base >= threshold) ? base + interval : base;
+  };
+
+  auto outputDistance = [&] {
+    if (is_metric) {
+      if (speedLimitAheadDistance < 50) return tr("Near");
+      if (speedLimitAheadDistance >= 1000) return QString::number(speedLimitAheadDistance * METER_TO_KM, 'f', 1) + tr("km");
+
+      int rounded = (speedLimitAheadDistance < 200) ? std::max(10, roundToInterval(speedLimitAheadDistance, 10, 5)) : roundToInterval(speedLimitAheadDistance, 100, 50);
+      return QString::number(rounded) + tr("m");
+    } else {
+      float distance_ft = speedLimitAheadDistance * METER_TO_FOOT;
+      if (distance_ft < 100) return tr("Near");
+      if (distance_ft >= 900) return QString::number(speedLimitAheadDistance * METER_TO_MILE, 'f', 1) + tr("mi");
+
+      int rounded = (distance_ft < 500) ? std::max(50, roundToInterval(distance_ft, 50, 25)) : roundToInterval(distance_ft, 100, 50);
+      return QString::number(rounded) + tr("ft");
+    }
+  };
+
+  QString speedStr = QString::number(std::nearbyint(speedLimitAhead));
+  QString distanceStr = outputDistance();
+
+  // Position below current speed limit sign
+  const int sign_width = is_metric ? 200 : 172;
+  const int sign_x = is_metric ? 280 : 272;
+  const int sign_y = 45;
+  const int sign_height = 204;
+
+  const int ahead_width = 170;
+  const int ahead_height = 160;
+  const int ahead_x = sign_x + (sign_width - ahead_width) / 2;
+  const int ahead_y = sign_y + sign_height + 10;
+
+  QRect ahead_rect(ahead_x, ahead_y, ahead_width, ahead_height);
+  p.setPen(QPen(QColor(255, 255, 255, 100), 3));
+  p.setBrush(QColor(0, 0, 0, 180));
+  p.drawRoundedRect(ahead_rect, 16, 16);
+
+  // "AHEAD" label
+  p.setFont(InterFont(40, QFont::DemiBold));
+  p.setPen(QColor(200, 200, 200, 255));
+  p.drawText(ahead_rect.adjusted(0, 4, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("AHEAD"));
+
+  // Speed value
+  p.setFont(InterFont(70, QFont::Bold));
+  p.setPen(QColor(255, 255, 255, 255));
+  p.drawText(ahead_rect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedStr);
+
+  // Distance
+  p.setFont(InterFont(40, QFont::Normal));
+  p.setPen(QColor(180, 180, 180, 255));
+  p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr);
+}
+
+void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) {
+  if (!roadName || roadNameStr.isEmpty()) return;
+
+  // Measure text to size container
+  p.setFont(InterFont(40, QFont::Normal));
+  QFontMetrics fm(p.font());
+
+  int text_width = fm.horizontalAdvance(roadNameStr);
+  int padding = 40;
+  int rect_width = text_width + padding;
+
+  // Constrain to reasonable bounds
+  int min_width = 200;
+  int max_width = surface_rect.width() - 40;
+  rect_width = std::max(min_width, std::min(rect_width, max_width));
+
+  // Center at top of screen
+  QRect road_rect(surface_rect.width() / 2 - rect_width / 2, -6, rect_width, 60);
+
+  p.setPen(Qt::NoPen);
+  p.setBrush(QColor(0, 0, 0, 120));
+  p.drawRoundedRect(road_rect, 12, 12);
+
+  p.setPen(QColor(255, 255, 255, 200));
+
+  // Truncate if still too long
+  QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20);
+  p.drawText(road_rect, Qt::AlignCenter, truncated);
+}

selfdrive/ui/sunnypilot/qt/onroad/hud.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include "selfdrive/ui/qt/onroad/hud.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
 #include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h"
 
 class HudRendererSP : public HudRenderer {
@@ -28,6 +29,9 @@ private:
   void drawStandstillTimer(QPainter &p, int x, int y);
   bool pulseElement(int frame);
   void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name);
+  void drawSpeedLimitSigns(QPainter &p);
+  void drawUpcomingSpeedLimit(QPainter &p);
+  void drawRoadName(QPainter &p, const QRect &surface_rect);
 
   bool lead_status;
   float lead_d_rel;
@@ -63,4 +67,14 @@ private:
   bool smartCruiseControlVisionEnabled;
   bool smartCruiseControlVisionActive;
   int smartCruiseControlVisionFrame;
+  float speedLimit;
+  float speedLimitOffset;
+  bool speedLimitAheadValid;
+  float speedLimitAhead;
+  float speedLimitAheadDistance;
+  float speedLimitAheadDistancePrev;
+  int speedLimitAheadValidFrame;
+  SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF;
+  bool roadName;
+  QString roadNameStr;
 };

selfdrive/ui/sunnypilot/ui.cc

@@ -20,7 +20,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
     "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
     "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters",
-    "carStateSP", "liveParameters"
+    "carStateSP", "liveParameters", "liveMapDataSP"
   });
 
   // update timer
@@ -53,6 +53,8 @@ void ui_update_params_sp(UIStateSP *s) {
   auto params = Params();
   s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str());
   s->scene.standstill_timer = params.getBool("StandstillTimer");
+  s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str());
+  s->scene.road_name = params.getBool("RoadName");
 }
 
 DeviceSP::DeviceSP(QObject *parent) : Device(parent) {

selfdrive/ui/sunnypilot/ui_scene.h

@@ -10,4 +10,6 @@
 typedef struct UISceneSP : UIScene {
   int dev_ui_info = 0;
   bool standstill_timer = false;
+  int speed_limit_mode = 0;
+  bool road_name = false;
 } UISceneSP;

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():

sunnypilot/mapd/mapd_installer.py

@@ -22,7 +22,7 @@ from openpilot.system.version import is_prebuilt
 from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR
 import openpilot.system.sentry as sentry
 
-VERSION = 'v1.10.0'
+VERSION = 'v1.11.0'
 URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
 
 

sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

@@ -16,7 +16,7 @@ State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManageme
 SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState
 
 ALLOWED_SPEED_THRESHOLD = 1.8  # m/s, ~4 MPH
-HYST_GAP = 0.75
+HYST_GAP = 0.0  # currently disabled; TODO-SP: might need to be brand-specific
 INACTIVE_TIMER = 0.4
 
 

sunnypilot/selfdrive/car/tests/test_custom_cruise.py

@@ -11,7 +11,8 @@ ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type
 
 
-@parameterized_class(('pcm_cruise',), [(False,)])
+# TODO: test pcmCruise and pcmCruiseSpeed
+@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
 class TestCustomAccIncrements(TestVCruiseHelper):
   def setup_method(self):
     TestVCruiseHelper.setup_method(self)

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)

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.

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.

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

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

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.