Merge branch 'feature/slc' into ui-sla-control-only

# Conflicts:
#	selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h
#	sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py
#	sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py
#	sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py

cereal/custom.capnp

@@ -145,6 +145,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
   longitudinalPlanSource @1 :LongitudinalPlanSource;
   smartCruiseControl @2 :SmartCruiseControl;
+  speedLimitAssist @3 :SpeedLimitAssist;
+  events @4 :List(OnroadEventSP.Event);
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -180,9 +182,36 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     }
   }
 
+  struct SpeedLimitAssist {
+    state @0 :SpeedLimitAssistState;
+    enabled @1 :Bool;
+    active @2 :Bool;
+    speedLimit @3 :Float32;
+    speedLimitOffset @4 :Float32;
+    distToSpeedLimit @5 :Float32;
+    source @6 :SpeedLimitSource;
+  }
+
   enum LongitudinalPlanSource {
     cruise @0;
     sccVision @1;
+    speedLimitAssist @2;
+  }
+
+  enum SpeedLimitAssistState {
+    disabled @0;
+    inactive @1; # No speed limit set or not enabled by parameter.
+    preActive @2;
+    pending @3; # Awaiting new speed limit.
+    adapting @4; # Reducing speed to match new speed limit.
+    active @5; # Cruising at speed limit.
+    overriding @6; # System overriding with manual control.
+  }
+
+  enum SpeedLimitSource {
+    none @0;
+    car @1;
+    map @2;
   }
 }
 
@@ -225,6 +254,10 @@ struct OnroadEventSP @0xda96579883444c35 {
     pedalPressedAlertOnly @16;
     laneTurnLeft @17;
     laneTurnRight @18;
+    speedLimitPreActive @19;
+    speedLimitActive @20;
+    speedLimitConfirmed @21;
+    speedLimitChanged @22;
   }
 }
 
@@ -316,6 +349,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 }
 
 struct CarStateSP @0xb86e6369214c01c8 {
+  speedLimit @0 :Float32;  # m/s
 }
 
 struct LiveMapDataSP @0xf416ec09499d9d19 {

common/params_keys.h

@@ -225,4 +225,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"OsmStateTitle", {PERSISTENT, STRING}},
     {"OsmWayTest", {PERSISTENT, STRING}},
     {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
+
+    // Speed Limit Control
+    {"SpeedLimitAssist", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
+    {"SpeedLimitEngageType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitWarningType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitWarningOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitWarningValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
 };

selfdrive/controls/lib/longitudinal_planner.py

@@ -146,7 +146,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
 
-    # Get new v_cruise and a_desired from Smart Cruise Control
+    # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
 
     if force_slow_decel:

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/selfdrived/selfdrived.py

@@ -95,9 +95,8 @@ class SelfdriveD(CruiseHelper):
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
-                                   'modelDataV2SP'] + \
-                                   self.camera_packets + self.sensor_packets + self.gps_packets,
+                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \
+                                   self.camera_packets + self.sensor_packets + self.gps_packets + ['modelDataV2SP', 'longitudinalPlanSP'],
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
 
@@ -205,6 +204,7 @@ class SelfdriveD(CruiseHelper):
 
     if not self.CP.notCar:
       self.events.add_from_msg(self.sm['driverMonitoringState'].events)
+      self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events)
 
     # Add car events, ignore if CAN isn't valid
     if CS.canValid:

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,49 @@
+/*
+ * 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 SpeedLimitMode {
+  OFF,
+  INFORMATION,
+  WARNING,
+  ASSIST,
+};
+
+inline const char *SpeedLimitModeTexts[]{
+  QT_TR_NOOP("Off"),
+  QT_TR_NOOP("Information"),
+  QT_TR_NOOP("Warning"),
+  QT_TR_NOOP("Assist"),
+};
+
+enum class SpeedLimitOffsetType {
+  NONE,
+  FIXED,
+  PERCENT,
+};
+
+inline const char *SpeedLimitOffsetTypeTexts[]{
+  QT_TR_NOOP("None"),
+  QT_TR_NOOP("Fixed"),
+  QT_TR_NOOP("Percent"),
+};
+
+enum class SpeedLimitSourcePolicy {
+  CAR_ONLY,
+  MAP_ONLY,
+  CAR_FIRST,
+  MAP_FIRST,
+  COMBINED
+};
+
+inline const char *SpeedLimitSourcePolicyTexts[]{
+  QT_TR_NOOP("Car\nOnly"),
+  QT_TR_NOOP("Map\nOnly"),
+  QT_TR_NOOP("Car\nFirst"),
+  QT_TR_NOOP("Map\nFirst"),
+  QT_TR_NOOP("Combined\nData")
+};

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, true);
+
+  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,57 @@
+/*
+ * 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/widgets/controls.h"
+
+class SpeedLimitPolicy : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit SpeedLimitPolicy(QWidget *parent = nullptr);
+  void refresh();
+  void showEvent(QShowEvent *event) override;
+
+signals:
+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,138 @@
+/*
+ * 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, true);
+
+  // Speed Limit Assist
+  // TODO-SP: OptionControlSP with Speed Limit Information/Warning as a single option
+  speedLimitAssistToggle = new ParamControlSP(
+    "SpeedLimitAssist",
+    tr("Speed Limit Assist (SLA)"),
+    tr("When you engage ACC, you will be prompted to set the cruising speed to the speed limit of the road adjusted by the Offset and Source Policy specified, or the current driving speed. "
+      "The maximum cruising speed will always be the MAX set speed."),
+    "",
+    this);
+  list->addItem(speedLimitAssistToggle);
+
+  // TODO-SP: Combine Assist/Warning/Information
+  // std::vector<QString> speed_limit_texts{
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::OFF)],
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::INFORMATION)],
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::WARNING)],
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::ASSIST)],
+  // };
+  // speed_limit_mode_settings = new ButtonParamControlSP(
+  //   "SpeedLimitMode",
+  //   tr("Speed Limit Mode"),
+  //   tr("When you engage ACC, you will be prompted to set the cruising speed to the speed limit of the road adjusted by the Offset and Source Policy specified, or the current driving speed. "
+  //     "The maximum cruising speed will always be the MAX set speed."),
+  //   "",
+  //   speed_limit_texts,
+  //   250);
+  // list->addItem(speed_limit_mode_settings);
+
+  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_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() {
+  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 += "%";
+  }
+
+  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();
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.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/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;
+  ParamControlSP *speedLimitAssistToggle;
+  // ButtonParamControlSP *speed_limit_mode_settings;
+  PushButtonSP *speedLimitSource;
+  ButtonParamControlSP *speed_limit_offset_settings;
+  OptionControlSP *speed_limit_offset;
+  SpeedLimitPolicy *speedLimitPolicyScreen;
+
+  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);
+  }
+};

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,8 +8,10 @@
 #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"
+#include "selfdrive/ui/sunnypilot/ui.h"
 
 class LongitudinalPanel : public QWidget {
   Q_OBJECT
@@ -32,4 +34,6 @@ private:
   CustomAccIncrement *customAccIncrement = nullptr;
   ParamControl *SmartCruiseControlVision;
   ParamControl *intelligentCruiseButtonManagement = nullptr;
+  SpeedLimitSettings *speedLimitScreen;
+  PushButtonSP *speedLimitSettings;
 };

selfdrive/ui/sunnypilot/qt/widgets/expandable_row.cc

@@ -18,6 +18,7 @@ ExpandableToggleRow::ExpandableToggleRow(const QString &param, const QString &ti
   collapsibleWidget->setVisible(false);
   QVBoxLayout *collapsible_layout = new QVBoxLayout();
   collapsibleWidget->setLayout(collapsible_layout);
+  collapsible_layout->setContentsMargins(0, 20, 0, 20);
 
   list = new ListWidgetSP(this, false);
 

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/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,9 @@ 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_assist.speed_limit_assist import SpeedLimitAssist
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
@@ -17,8 +20,13 @@ Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
+    self.events_sp = EventsSP()
+
+    self.resolver = SpeedLimitResolver()
+
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
+    self.sla = SpeedLimitAssist(CP)
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = Source.cruise
 
@@ -34,11 +42,22 @@ class LongitudinalPlannerSP:
     return self.dec.mode()
 
   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)
+    long_enabled = sm['carControl'].enabled
+    long_override = sm['carControl'].cruiseControl.override
+
+    self.events_sp.clear()
+
+    self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
+
+    # Speed Limit Assist
+    self.resolver.update(v_ego, sm)
+    v_cruise_sla = self.sla.update(long_enabled, long_override, v_ego, a_ego, sm['carState'].vCruiseCluster,
+                                   self.resolver.speed_limit, self.resolver.distance, self.resolver.source, self.events_sp)
 
     targets = {
       Source.cruise: (v_cruise, a_ego),
-      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target)
+      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
+      Source.speedLimitAssist: (v_cruise_sla, a_ego),
     }
 
     self.source = min(targets, key=lambda k: targets[k][0])
@@ -56,6 +75,7 @@ class LongitudinalPlannerSP:
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
     longitudinalPlanSP.longitudinalPlanSource = self.source
+    longitudinalPlanSP.events = self.events_sp.to_msg()
 
     # Dynamic Experimental Control
     dec = longitudinalPlanSP.dec
@@ -75,4 +95,14 @@ class LongitudinalPlannerSP:
     sccVision.enabled = self.scc.vision.is_enabled
     sccVision.active = self.scc.vision.is_active
 
+    # Speed Limit Assist
+    speedLimitAssist = longitudinalPlanSP.speedLimitAssist
+    speedLimitAssist.state = self.sla.state
+    speedLimitAssist.enabled = self.sla.is_enabled
+    speedLimitAssist.active = self.sla.is_active
+    speedLimitAssist.speedLimit = float(self.resolver.speed_limit)
+    speedLimitAssist.speedLimitOffset = float(self.sla.speed_limit_offset)
+    speedLimitAssist.distToSpeedLimit = float(self.resolver.distance)
+    speedLimitAssist.source = self.resolver.source
+
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py

@@ -12,8 +12,5 @@ class SmartCruiseControl:
   def __init__(self):
     self.vision = SmartCruiseControlVision()
 
-  def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> None:
-    long_enabled = sm['carControl'].enabled
-    long_override = sm['carControl'].cruiseControl.override
-
+  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
     self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

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_assist/__init__.py

@@ -0,0 +1,20 @@
+from cereal import custom
+
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimitAssistState
+
+PARAMS_UPDATE_PERIOD = 3.  # secs. Time between parameter updates.
+DISABLED_GUARD_PERIOD = 2  # secs.
+PRE_ACTIVE_GUARD_PERIOD = 5  # secs. Time to wait after activation before considering temp deactivation signal.
+
+# Constants for Limit controllers.
+LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
+LIMIT_MIN_ACC = -1.5  # m/s^2 Maximum deceleration allowed for limit controllers to provide.
+LIMIT_MAX_ACC = 1.0   # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
+LIMIT_MIN_SPEED = 8.33  # m/s, Minimum speed limit to provide as solution on limit controllers.
+LIMIT_SPEED_OFFSET_TH = -1.  # m/s Maximum offset between speed limit and current speed for adapting state.
+LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
+
+# Speed Limit Assist Auto mode constants
+REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632  # m/s 80 MPH  # TODO-SP: customizable with params
+CRUISE_SPEED_TOLERANCE = 0.44704  # m/s ±1 MPH tolerance  # TODO-SP: metric vs imperial
+FALLBACK_CRUISE_SPEED = 255.0  # m/s fallback when no speed limit available

sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py

@@ -0,0 +1,15 @@
+from enum import IntEnum
+
+
+class Policy(IntEnum):
+  map_data_only = 0
+  car_state_only = 1
+  map_data_priority = 2
+  car_state_priority = 3
+  combined = 4
+
+
+class OffsetType(IntEnum):
+  off = 0
+  fixed = 1
+  percentage = 2

sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py

@@ -0,0 +1,267 @@
+"""
+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 numpy as np
+
+from cereal import custom
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, \
+  SpeedLimitAssistState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE, DISABLED_GUARD_PERIOD
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+EventNameSP = custom.OnroadEventSP.EventName
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
+ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, SpeedLimitAssistState.overriding, *ACTIVE_STATES)
+
+
+class SpeedLimitAssist:
+  _speed_limit: float
+  _distance: float
+  _source: custom.LongitudinalPlanSP.SpeedLimitSource
+  v_ego: float
+  a_ego: float
+  v_offset: float
+  last_valid_speed_limit_final: float
+
+  def __init__(self, CP):
+    self.params = Params()
+    self.CP = CP
+    self.frame = -1
+    self.long_engaged_timer = 0
+    self.pre_active_timer = 0
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.enabled = self.params.get_bool("SpeedLimitAssist")
+    self.long_enabled = False
+    self.long_enabled_prev = False
+    self.long_override = False
+    self.is_enabled = False
+    self.is_active = False
+    self.v_ego = 0.
+    self.a_ego = 0.
+    self.v_offset = 0.
+    self.v_cruise_setpoint = 0.
+    self.v_cruise_setpoint_prev = 0.
+    self.initial_max_set = False
+    self._speed_limit = 0.
+    self.speed_limit_prev = 0.
+    self.last_valid_speed_limit_final = 0.
+    self._distance = 0.
+    self._source = SpeedLimitSource.none
+    self.state = SpeedLimitAssistState.disabled
+    self._state_prev = SpeedLimitAssistState.disabled
+    self.pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
+
+    self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True))
+    self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+
+    # Solution functions mapped to respective states
+    self.acceleration_solutions = {
+      SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
+      SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
+    }
+
+  @property
+  def speed_limit_final(self) -> float:
+    return self._speed_limit + self.speed_limit_offset
+
+  @property
+  def speed_limit_changed(self) -> bool:
+    return bool(self._speed_limit != self.speed_limit_prev)
+
+  @property
+  def speed_limit_offset(self) -> float:
+    return self.get_offset(self.offset_type, self.offset_value)
+
+  @property
+  def v_cruise_setpoint_changed(self) -> bool:
+    return bool(self.v_cruise_setpoint != self.v_cruise_setpoint_prev)
+
+  def get_v_target_from_control(self) -> float:
+    if self.is_enabled:
+      # If we have a current valid speed limit, use it
+      if self._speed_limit > 0:
+        self.last_valid_speed_limit_final = self.speed_limit_final
+        return self.speed_limit_final
+
+      # If no current speed limit but we have a last valid one, use that
+      if self.last_valid_speed_limit_final > 0:
+        return self.last_valid_speed_limit_final
+
+    # Fallback
+    return V_CRUISE_UNSET
+
+  def get_offset(self, offset_type: OffsetType, offset_value: int) -> float:
+    if offset_type == OffsetType.off:
+      return 0
+    elif offset_type == OffsetType.fixed:
+      return offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)
+    elif offset_type == OffsetType.percentage:
+      return offset_value * 0.01 * self._speed_limit
+    else:
+      raise NotImplementedError("Offset not supported")
+
+  def update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("SpeedLimitAssist")
+      self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True))
+      self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+      self.is_metric = self.params.get_bool("IsMetric")
+
+  def initial_max_set_confirmed(self) -> bool:
+    return bool(abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE)
+
+  def detect_manual_cruise_change(self) -> bool:
+    # If cruise speed changed and it's not what SLA would set
+    if self.v_cruise_setpoint_changed:
+      expected_cruise = self.speed_limit_final
+      return bool(abs(self.v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE)
+
+    return False
+
+  def update_calculations(self, v_cruise_setpoint: float) -> None:
+    self.v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
+
+    # Update current velocity offset (error)
+    self.v_offset = self.speed_limit_final - self.v_ego
+
+  def get_current_acceleration_as_target(self) -> float:
+    return self.a_ego
+
+  def get_adapting_state_target_acceleration(self) -> float:
+    if self._distance > 0:
+      return (self.speed_limit_final ** 2 - self.v_ego ** 2) / (2. * self._distance)
+
+    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+  def get_active_state_target_acceleration(self) -> float:
+    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+  def update_state_machine(self):
+    self._state_prev = self.state
+
+    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
+    self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE, OVERRIDING
+    if self.state != SpeedLimitAssistState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = SpeedLimitAssistState.disabled
+        self.initial_max_set = False
+      elif self.long_override:
+        self.state = SpeedLimitAssistState.overriding
+
+      else:
+        # ACTIVE
+        if self.state == SpeedLimitAssistState.active:
+          if self.detect_manual_cruise_change():
+            self.state = SpeedLimitAssistState.inactive
+          elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.adapting
+
+        # ADAPTING
+        elif self.state == SpeedLimitAssistState.adapting:
+          if self.detect_manual_cruise_change():
+            self.state = SpeedLimitAssistState.inactive
+          elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.active
+
+        # PENDING
+        elif self.state == SpeedLimitAssistState.pending:
+          if self._speed_limit > 0:
+            if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+              self.state = SpeedLimitAssistState.adapting
+            else:
+              self.state = SpeedLimitAssistState.active
+
+        # PRE_ACTIVE
+        elif self.state == SpeedLimitAssistState.preActive:
+          if self.initial_max_set_confirmed():
+            self.initial_max_set = True
+            if self._speed_limit > 0:
+              if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+                self.state = SpeedLimitAssistState.adapting
+              else:
+                self.state = SpeedLimitAssistState.active
+            else:
+              self.state = SpeedLimitAssistState.pending
+          elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD:
+            # Timeout - session ended
+            self.state = SpeedLimitAssistState.inactive
+
+        # OVERRIDING
+        elif self.state == SpeedLimitAssistState.overriding:
+          if not self.long_override:
+            self.state = SpeedLimitAssistState.preActive
+
+        # INACTIVE
+        elif self.state == SpeedLimitAssistState.inactive:
+          pass
+
+    # DISABLED
+    elif self.state == SpeedLimitAssistState.disabled:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = SpeedLimitAssistState.overriding
+
+        elif not self.long_enabled_prev:
+          self.pre_active_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
+
+        elif self.pre_active_timer <= 0:
+          self.state = SpeedLimitAssistState.preActive
+          self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+          self.initial_max_set = False
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def update_events(self, events_sp: EventsSP) -> None:
+    if self.state == SpeedLimitAssistState.preActive and self._state_prev != SpeedLimitAssistState.preActive:
+      events_sp.add(EventNameSP.speedLimitPreActive)
+    elif self.is_active:
+      if self._state_prev not in ACTIVE_STATES:
+        events_sp.add(EventNameSP.speedLimitActive)
+      elif self.speed_limit_changed:
+        events_sp.add(EventNameSP.speedLimitChanged)
+
+  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float,
+             speed_limit: float, distance: float, source: custom.LongitudinalPlanSP.SpeedLimitSource, events_sp: EventsSP) -> float:
+    self.long_enabled = long_enabled
+    self.long_override = long_override
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+
+    self._speed_limit = speed_limit
+    self._distance = distance
+    self._source = source
+
+    self.update_params()
+    self.update_calculations(v_cruise_setpoint)
+    self.is_enabled, self.is_active = self.update_state_machine()
+    self.update_events(events_sp)
+
+    # Update change tracking variables
+    self.speed_limit_prev = self._speed_limit
+    self.v_cruise_setpoint_prev = self.v_cruise_setpoint
+    self.long_enabled_prev = self.long_enabled
+    self.frame += 1
+
+    v_target = self.get_v_target_from_control()
+
+    return v_target

sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py

@@ -0,0 +1,133 @@
+import time
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom
+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.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
+
+
+class SpeedLimitResolver:
+  _limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float]
+  _distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float]
+  _v_ego: float
+  speed_limit: float
+  distance: float
+  source: custom.LongitudinalPlanSP.SpeedLimitSource
+
+  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.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
+      Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
+      Policy.map_data_only: [SpeedLimitSource.map],
+      Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
+    }
+    self.source = SpeedLimitSource.none
+    for source in ALL_SOURCES:
+      self._reset_limit_sources(source)
+
+  def update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.policy = Policy(self.params.get("SpeedLimitPolicy", return_default=True))
+      self.change_policy(self.policy)
+
+  def change_policy(self, policy: Policy) -> None:
+    self.policy = policy
+
+  def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimitSource) -> None:
+    self._limit_solutions[source] = 0.
+    self._distance_solutions[source] = 0.
+
+  def _resolve_limit_sources(self, sm: messaging.SubMaster) -> None:
+    """Get limit solutions from each data source"""
+    self._get_from_car_state(sm)
+    self._get_from_map_data(sm)
+
+  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 _consolidate(self) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimitSource]:
+    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 _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimitSource:
+    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:
+        return source if self._limit_solutions[source] > 0. else SpeedLimitSource.none
+
+    limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float)
+    sources = np.array(sources_for_policy, dtype=int)
+
+    if len(limits) > 0:
+      min_idx = np.argmin(limits)
+      return SpeedLimitSource(int(sources[min_idx]))
+
+    return SpeedLimitSource.none
+
+  def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
+    self._v_ego = v_ego
+    self.update_params()
+    self._resolve_limit_sources(sm)
+
+    self.speed_limit, self.distance, self.source = self._consolidate()
+
+    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py

@@ -0,0 +1,247 @@
+"""
+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 pytest
+
+from cereal import custom
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.toyota.values import CAR as TOYOTA
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import SpeedLimitAssistState, REQUIRED_INITIAL_MAX_SET_SPEED, \
+  PRE_ACTIVE_GUARD_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist, ACTIVE_STATES
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
+
+SPEED_LIMITS = {
+  'residential': 25 * CV.MPH_TO_MS,  # 25 mph
+  'city': 35 * CV.MPH_TO_MS,         # 35 mph
+  'highway': 65 * CV.MPH_TO_MS,      # 65 mph
+  'freeway': 80 * CV.MPH_TO_MS,      # 80 mph
+}
+
+
+class TestSpeedLimitAssist:
+
+  def setup_method(self):
+    self.params = Params()
+    self.reset_custom_params()
+    self.events_sp = EventsSP()
+    CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2_2022)
+    self.sla = SpeedLimitAssist(CI.CP)
+    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+
+  def teardown_method(self, method):
+    self.reset_state()
+
+  def _setup_platform(self, car_name):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+    CI = CarInterface(CP, CP_SP)
+    sunnypilot_interfaces.setup_interfaces(CI, self.params)
+    return CI
+
+  def reset_custom_params(self):
+    self.params.put_bool("SpeedLimitAssist", True)
+    self.params.put_bool("IsMetric", False)
+    self.params.put("SpeedLimitOffsetType", 0)
+    self.params.put("SpeedLimitValueOffset", 0)
+
+  def reset_state(self):
+    self.sla.state = SpeedLimitAssistState.disabled
+    self.sla.frame = -1
+    self.sla.last_op_engaged_frame = 0
+    self.sla.op_engaged = False
+    self.sla.op_engaged_prev = False
+    self.sla.initial_max_set = False
+    self.sla._speed_limit = 0.
+    self.sla.speed_limit_prev = 0.
+    self.sla.last_valid_speed_limit_offsetted = 0.
+    self.sla._distance = 0.
+    self.sla._source = SpeedLimitSource.none
+    self.sla.v_cruise_setpoint = 0.
+    self.sla.v_cruise_setpoint_prev = 0.
+    self.events_sp.clear()
+
+  def initialize_active_state(self, v_cruise_setpoint):
+    self.sla.state = SpeedLimitAssistState.active
+    self.sla.v_cruise_setpoint = v_cruise_setpoint
+    self.sla.v_cruise_setpoint_prev = v_cruise_setpoint
+
+  def test_initial_state(self):
+    assert self.sla.state == SpeedLimitAssistState.disabled
+    assert not self.sla.is_enabled
+    assert not self.sla.is_active
+    assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
+
+  def test_disabled(self):
+    self.params.put_bool("SpeedLimitAssist", False)
+    for _ in range(int(10. / DT_MDL)):
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.disabled
+
+  def test_transition_disabled_to_preactive(self):
+    for _ in range(int(3. / DT_MDL)):
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.preActive
+    assert self.sla.is_enabled and not self.sla.is_active
+
+  def test_preactive_to_active_with_max_speed_confirmation(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+    assert self.sla.is_enabled and self.sla.is_active, f"enabled: {self.sla.is_enabled}, active: {self.sla.is_active}"
+    assert v_cruise_sla == SPEED_LIMITS['city']
+
+  def test_preactive_timeout_to_inactive(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+
+    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.inactive
+
+  def test_preactive_to_pending_no_speed_limit(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.pending
+    assert self.sla.is_enabled and not self.sla.is_active
+
+  def test_pending_to_active_when_speed_limit_available(self):
+    self.sla.state = SpeedLimitAssistState.pending
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+
+  def test_pending_to_adapting_when_below_speed_limit(self):
+    self.sla.state = SpeedLimitAssistState.pending
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+    assert self.sla.is_enabled and self.sla.is_active
+
+  def test_active_to_adapting_transition(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+
+  def test_adapting_to_active_transition(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
+
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+
+  def test_manual_cruise_change_detection(self):
+    self.sla.state = SpeedLimitAssistState.active
+    expected_cruise = SPEED_LIMITS['highway']
+    self.sla.v_cruise_setpoint_prev = expected_cruise
+
+    different_cruise = SPEED_LIMITS['highway'] + 5
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.inactive
+
+  @pytest.mark.parametrize("offset_type, offset_value, speed_limit, expected_offset", [
+    (OffsetType.fixed, 5, SPEED_LIMITS['city'], 5 * CV.MPH_TO_MS),  # 5 MPH fixed offset
+    (OffsetType.percentage, 10, SPEED_LIMITS['city'], 0.1 * SPEED_LIMITS['city']),  # 10% offset
+    (OffsetType.off, 0, SPEED_LIMITS['city'], 0),  # Off
+    (OffsetType.fixed, 10, SPEED_LIMITS['highway'], 10 * CV.MPH_TO_MS),  # Different speed, fixed offset
+    (OffsetType.percentage, 5, SPEED_LIMITS['highway'], 0.05 * SPEED_LIMITS['highway']),  # Different speed, percentage
+  ])
+  def test_offset_calculations(self, offset_type, offset_value, speed_limit, expected_offset):
+    self.sla._speed_limit = speed_limit
+    actual_offset = self.sla.get_offset(offset_type, offset_value)
+    assert actual_offset == pytest.approx(expected_offset, rel=0.01)
+
+  def test_rapid_speed_limit_changes(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+    speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']]
+
+    for _, speed_limit in enumerate(speed_limits):
+      _ = self.sla.update(True, False, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+
+  def test_invalid_speed_limits_handling(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+    self.sla.last_valid_speed_limit_final = SPEED_LIMITS['city']
+
+    invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
+
+    for invalid_limit in invalid_limits:
+      v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp)
+      assert isinstance(v_cruise_sla, (int, float))
+      assert v_cruise_sla == V_CRUISE_UNSET or v_cruise_sla > 0
+
+  def test_stale_data_handling(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+    old_speed_limit = SPEED_LIMITS['city']
+    self.sla.last_valid_speed_limit_final = old_speed_limit
+
+    v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+    assert v_cruise_sla == old_speed_limit
+
+  def test_different_speed_limit_sources(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+    for source in (SpeedLimitSource.car, SpeedLimitSource.map):
+      v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp)
+      assert v_cruise_sla != V_CRUISE_UNSET
+
+  def test_distance_based_adapting(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
+
+    distance = 100.0
+    current_speed = SPEED_LIMITS['highway']
+    target_speed = SPEED_LIMITS['city']
+
+    v_cruise_sla = self.sla.update(True, False, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+    assert v_cruise_sla == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
+
+  def test_long_disengaged_to_disabled(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+    v_cruise_sla = self.sla.update(False, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'],
+                                   0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.disabled
+    assert v_cruise_sla == V_CRUISE_UNSET
+
+  def test_maintain_states_with_no_changes(self):
+    """Test that states are maintained when no significant changes occur"""
+    test_states = [
+      SpeedLimitAssistState.preActive,
+      SpeedLimitAssistState.pending,
+      SpeedLimitAssistState.active,
+      SpeedLimitAssistState.adapting
+    ]
+
+    for state in test_states:
+      self.sla.state = state
+      self.sla.op_engaged = True
+      if state in [SpeedLimitAssistState.pending, SpeedLimitAssistState.active, SpeedLimitAssistState.adapting]:
+        self.sla.initial_max_set = True
+
+      initial_state = state
+
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+
+      assert self.sla.state in ALL_STATES  # Sanity check
+
+      if initial_state == SpeedLimitAssistState.preActive:
+        assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
+      elif initial_state in ACTIVE_STATES:
+        assert self.sla.state in ACTIVE_STATES

sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py

@@ -0,0 +1,138 @@
+import random
+import time
+
+import pytest
+from pytest_mock import MockerFixture
+
+from cereal import custom
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE
+
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+
+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.

sunnypilot/selfdrive/selfdrived/events.py

@@ -1,4 +1,6 @@
+import cereal.messaging as messaging
 from cereal import log, car, custom
+from openpilot.common.constants import CV
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
 
@@ -14,6 +16,17 @@ EventNameSP = custom.OnroadEventSP.EventName
 EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
 
 
+def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  speedLimit = sm['longitudinalPlanSP'].sla.speedLimit
+  speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
+  message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
+  return Alert(
+    message,
+    "",
+    AlertStatus.normal, AlertSize.small,
+    Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
+
+
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -148,5 +161,29 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       "",
       AlertStatus.normal, AlertSize.small,
       Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
-  }
+  },
+
+  EventNameSP.speedLimitActive: {
+    ET.WARNING: Alert(
+      "Automatically adjusting",
+      "to the posted speed limit",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+  },
+
+  EventNameSP.speedLimitChanged: {
+    ET.WARNING: Alert(
+      "Set speed changed",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+  },
+
+  EventNameSP.speedLimitPreActive: {
+    ET.WARNING: Alert(
+      "Auto Speed Limit Assist: Activation Required",
+      "Manually change set speed to 80 MPH to activate",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+  },
 }