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

2026-08-04 15:56:05 +08:00 · 2025-10-03 10:42:07 +02:00
parent 41cb97d0b1 09d165a85b
commit d6b4352253
51 changed files with 1535 additions and 81 deletions
@@ -148,6 +148,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
  speedLimit @3 :SpeedLimit;
  vTarget @4 :Float32;
  aTarget @5 :Float32;
+  events @6 :List(OnroadEventSP.Event);
+  e2eAlerts @7 :E2eAlerts;

  struct DynamicExperimentalControl {
    state @0 :DynamicExperimentalControlState;
@@ -201,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {

  struct SpeedLimit {
    resolver @0 :Resolver;
+    assist @1 :Assist;

    struct Resolver {
      speedLimit @0 :Float32;
@@ -214,17 +217,40 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
      speedLimitLastValid @8 :Bool;
    }

+    struct Assist {
+      state @0 :AssistState;
+      enabled @1 :Bool;
+      active @2 :Bool;
+      vTarget @3 :Float32;
+      aTarget @4 :Float32;
+    }
+
    enum Source {
      none @0;
      car @1;
      map @2;
    }
+
+    enum AssistState {
+      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.
+    }
  }

  enum LongitudinalPlanSource {
    cruise @0;
    sccVision @1;
    sccMap @2;
+    speedLimitAssist @3;
+  }
+
+  struct E2eAlerts {
+    greenLightAlert @0 :Bool;
+    leadDepartAlert @1 :Bool;
  }
 }

@@ -267,6 +293,11 @@ struct OnroadEventSP @0xda96579883444c35 {
    pedalPressedAlertOnly @16;
    laneTurnLeft @17;
    laneTurnRight @18;
+    speedLimitPreActive @19;
+    speedLimitActive @20;
+    speedLimitChanged @21;
+    speedLimitPending @22;
+    e2eChime @23;
  }
 }

@@ -275,6 +306,7 @@ struct CarParamsSP @0x80ae746ee2596b11 {
  safetyParam @1 : Int16;  # flags for sunnypilot's custom safety flags
  pcmCruiseSpeed @3 :Bool;
  intelligentCruiseButtonManagementAvailable @4 :Bool;
+  enableGasInterceptor @5 :Bool;

  neuralNetworkLateralControl @2 :NeuralNetworkLateralControl;

@@ -160,15 +160,20 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
    {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
    {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
+    {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
    {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
    {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
    {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
    {"LastGPSPositionLLK", {PERSISTENT, STRING}},
+    {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
    {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
    {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
    {"Offroad_TiciSupport", {CLEAR_ON_MANAGER_START, JSON}},
+    {"OnroadScreenOffBrightness", {PERSISTENT | BACKUP, INT, "100"}},
+    {"OnroadScreenOffControl", {PERSISTENT | BACKUP, BOOL}},
+    {"OnroadScreenOffTimer", {PERSISTENT | BACKUP, INT, "0"}},
    {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -71,7 +71,7 @@ class Car:

  def __init__(self, CI=None, RI=None) -> None:
    self.can_sock = messaging.sub_sock('can', timeout=20)
-    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP'])
+    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP'])
    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP'])

    self.can_rcv_cum_timeout_counter = 0
@@ -124,7 +124,7 @@ class Car:

    self.CP.alternativeExperience = 0
    # mads
-    set_alternative_experience(self.CP, self.params)
+    set_alternative_experience(self.CP, self.CP_SP, self.params)
    set_car_specific_params(self.CP, self.CP_SP, self.params)

    # Dynamic Experimental Control
@@ -216,6 +216,7 @@ class Car:
    if can_rcv_valid and REPLAY:
      self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime

+    self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP'])
    self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric, self.sm['carControl'].cruiseControl.speedLimit, self.sm['carControl'].cruiseControl.speedLimitPredicative)
    if self.sm['carControl'].enabled and not self.CC_prev.enabled:
      # Use CarState w/ buttons from the step selfdrived enables on
@@ -55,6 +55,7 @@ class VCruiseHelper(VCruiseHelperSP):
        # if stock cruise is completely disabled, then we can use our own set speed logic
        self._update_v_speed_limit(CS, _enabled, speed_limit_control, speed_limit_predicative)
        self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
+        self.update_speed_limit_assist_v_cruise_non_pcm()
        self.v_cruise_cluster_kph = self.v_cruise_kph
        self.update_button_timers(CS, enabled)
      else:
@@ -121,6 +122,12 @@ class VCruiseHelper(VCruiseHelperSP):
    if not self.button_change_states[button_type]["enabled"]:
      return

+    # Speed Limit Assist for Non PCM long cars.
+    # True: Disallow set speed changes when user confirmed the target set speed during preActive state
+    # False: Allow set speed changes as SLA is not requesting user confirmation
+    if self.update_speed_limit_assist_pre_active_confirmed(button_type):
+      return
+
    long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
    if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
      self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
@@ -60,7 +60,7 @@ class TestCarInterfaces:
    # Test controller initialization
    # TODO: wait until card refactor is merged to run controller a few times,
    #  hypothesis also slows down significantly with just one more message draw
-    LongControl(car_params)
+    LongControl(car_params, car_params_sp)
    if car_params.steerControlType == CarParams.SteerControlType.angle:
      LatControlAngle(car_params, car_params_sp, car_interface)
    elif car_params.lateralTuning.which() == 'pid':
@@ -70,7 +70,7 @@ class Controls(ControlsExt, ModelStateBase):
    self.pose_calibrator = PoseCalibrator()
    self.calibrated_pose: Pose | None = None

-    self.LoC = LongControl(self.CP)
+    self.LoC = LongControl(self.CP, self.CP_SP)
    self.VM = VehicleModel(self.CP)
    self.LaC: LatControl
    if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or
@@ -10,8 +10,11 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 LongCtrlState = car.CarControl.Actuators.LongControlState


-def long_control_state_trans(CP, active, long_control_state, v_ego,
+def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego,
                             should_stop, brake_pressed, cruise_standstill):
+  # Gas Interceptor
+  cruise_standstill = cruise_standstill and not CP_SP.enableGasInterceptor
+
  stopping_condition = should_stop
  starting_condition = (not should_stop and
                        not cruise_standstill and
@@ -45,8 +48,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
  return long_control_state

 class LongControl:
-  def __init__(self, CP):
+  def __init__(self, CP, CP_SP):
    self.CP = CP
+    self.CP_SP = CP_SP
    self.long_control_state = LongCtrlState.off
    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
@@ -61,7 +65,7 @@ class LongControl:
    self.pid.neg_limit = accel_limits[0]
    self.pid.pos_limit = accel_limits[1]

-    self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
+    self.long_control_state = long_control_state_trans(self.CP, self.CP_SP, active, self.long_control_state, CS.vEgo,
                                                       should_stop, CS.brakePressed,
                                                       CS.cruiseState.standstill)
    if self.long_control_state == LongCtrlState.off:
@@ -155,7 +155,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:
@@ -24,10 +24,11 @@ def main():
  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
                            'liveMapDataSP', 'carStateSP', gps_location_service],
-                           poll='modelV2')
+                           poll='carState')

  while True:
    sm.update()
+    longitudinal_planner.sla.update_car_state(sm['carState'])
    if sm.updated['modelV2']:
      longitudinal_planner.update(sm)
      longitudinal_planner.publish(sm, pm)
@@ -1,4 +1,4 @@
-from cereal import car
+from cereal import car, custom
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans


@@ -8,49 +8,52 @@ class TestLongControlStateTransition:

  def test_stay_stopped(self):
    CP = car.CarParams.new_message()
+    CP_SP = custom.CarParamsSP.new_message()
    active = True
    current_state = LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=True, brake_pressed=False, cruise_standstill=False)
    assert next_state == LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=False, brake_pressed=True, cruise_standstill=False)
    assert next_state == LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=False, brake_pressed=False, cruise_standstill=True)
    assert next_state == LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
+    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
                             should_stop=False, brake_pressed=False, cruise_standstill=False)
    assert next_state == LongCtrlState.pid
    active = False
-    next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
+    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
                             should_stop=False, brake_pressed=False, cruise_standstill=False)
    assert next_state == LongCtrlState.off

 def test_engage():
  CP = car.CarParams.new_message()
+  CP_SP = custom.CarParamsSP.new_message()
  active = True
  current_state = LongCtrlState.off
-  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=True, brake_pressed=False, cruise_standstill=False)
  assert next_state == LongCtrlState.stopping
-  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=False, brake_pressed=True, cruise_standstill=False)
  assert next_state == LongCtrlState.stopping
-  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=False, brake_pressed=False, cruise_standstill=True)
  assert next_state == LongCtrlState.stopping
-  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=False, brake_pressed=False, cruise_standstill=False)
  assert next_state == LongCtrlState.pid

 def test_starting():
  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  CP_SP = custom.CarParamsSP.new_message()
  active = True
  current_state = LongCtrlState.starting
-  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
                             should_stop=False, brake_pressed=False, cruise_standstill=False)
  assert next_state == LongCtrlState.starting
-  next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
+  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
                             should_stop=False, brake_pressed=False, cruise_standstill=False)
  assert next_state == LongCtrlState.pid
@@ -159,7 +159,7 @@ class SelfdriveD(CruiseHelper):
    self.mads = ModularAssistiveDrivingSystem(self)
    self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP)

-    self.car_events_sp = CarSpecificEventsSP(self.CP, self.params)
+    self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)

    CruiseHelper.__init__(self, self.CP)

@@ -205,6 +205,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:
@@ -57,10 +57,11 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) {
  bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0));
  p.fillRect(0, 0, surface_rect.width(), UI_HEADER_HEIGHT, bg);

-
+#ifndef SUNNYPILOT
  if (is_cruise_available) {
    drawSetSpeed(p, surface_rect);
  }
+#endif
  drawCurrentSpeed(p, surface_rect);

  auto params = Params();
@@ -24,6 +24,7 @@ qt_src = [
  "sunnypilot/qt/offroad/offroad_home.cc",
  "sunnypilot/qt/offroad/settings/developer_panel.cc",
  "sunnypilot/qt/offroad/settings/device_panel.cc",
+  "sunnypilot/qt/offroad/settings/display_panel.cc",
  "sunnypilot/qt/offroad/settings/lateral_panel.cc",
  "sunnypilot/qt/offroad/settings/longitudinal_panel.cc",
  "sunnypilot/qt/offroad/settings/max_time_offroad.cc",
@@ -90,9 +91,13 @@ brand_settings_qt_src = [
  "sunnypilot/qt/offroad/settings/vehicle/volkswagen_settings.cc",
 ]

+display_panel_qt_src = [
+  "sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc",
+]

 sp_widgets_src = widgets_src + network_src
-sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + longitudinal_panel_qt_src + osm_panel_qt_src
+sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + \
+            longitudinal_panel_qt_src + osm_panel_qt_src + display_panel_qt_src
 sp_qt_util = qt_util

 Export('sp_widgets_src', 'sp_qt_src', "sp_qt_util")
@@ -0,0 +1,63 @@
+/**
+ * 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/display/onroad_screen_brightness.h"
+
+OnroadScreenBrightnessControl::OnroadScreenBrightnessControl(const QString &param, const QString &title,
+                                                             const QString &description, const QString &icon,
+                                                             QWidget *parent)
+  : ExpandableToggleRow(param, title, description, icon, parent) {
+  auto *mainFrame = new QFrame(this);
+  auto *mainFrameLayout = new QGridLayout();
+  mainFrame->setLayout(mainFrameLayout);
+  mainFrameLayout->setSpacing(0);
+
+  onroadScreenOffTimer = new OptionControlSP(
+    "OnroadScreenOffTimer",
+    "",
+    "",
+    "",
+    {0, 11}, 1, true, &onroadScreenOffTimerOptions);
+
+  onroadScreenBrightness = new OptionControlSP(
+    "OnroadScreenOffBrightness",
+    "",
+    "",
+    "",
+    {0, 100}, 10, true, nullptr, false);
+
+  connect(onroadScreenOffTimer, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh);
+  connect(onroadScreenBrightness, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh);
+  onroadScreenOffTimer->setFixedWidth(280);
+  onroadScreenBrightness->setFixedWidth(280);
+  mainFrameLayout->addWidget(onroadScreenOffTimer, 0, 0, Qt::AlignLeft);
+  mainFrameLayout->addWidget(onroadScreenBrightness, 0, 1, Qt::AlignRight);
+
+  addItem(mainFrame);
+
+  refresh();
+}
+
+void OnroadScreenBrightnessControl::refresh() {
+  // Driving Screen Off Timer
+  int valTimer = std::atoi(params.get("OnroadScreenOffTimer").c_str());
+  std::string labelTimer = "<span style='font-size: 45px; font-weight: 450; color: #FFFFFF;'>";
+  labelTimer += "Delay";
+  labelTimer += " <br><span style='font-size: 40px; font-weight: 450; color:rgb(174, 255, 195);'>";
+  labelTimer += (valTimer < 60 ? std::to_string(valTimer) + "s" : std::to_string(valTimer / 60) + "m");
+  labelTimer += "</span></span>";
+  onroadScreenOffTimer->setLabel(QString::fromStdString(labelTimer));
+
+  // Driving Screen Off Brightness
+  std::string valBrightness = params.get("OnroadScreenOffBrightness");
+  std::string labelBrightness = "<span style='font-size: 45px; font-weight: 450; color: #FFFFFF;'>";
+  labelBrightness += "Brightness";
+  labelBrightness += " <br><span style='font-size: 40px; font-weight: 450; color:rgb(174, 255, 195);'>";
+  labelBrightness += (valBrightness == "0" ? " Screen Off" : valBrightness + "%");
+  labelBrightness += "</span></span>";
+  onroadScreenBrightness->setLabel(QString::fromStdString(labelBrightness));
+}
@@ -0,0 +1,42 @@
+/**
+ * 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/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
+
+static const QMap<QString, QString> onroadScreenOffTimerOptions = {
+  {"0", "15"},
+  {"1", "30"},
+  {"2", "60"},
+  {"3", "120"},
+  {"4", "180"},
+  {"5", "240"},
+  {"6", "300"},
+  {"7", "360"},
+  {"8", "420"},
+  {"9", "480"},
+  {"10", "540"},
+  {"11", "600"}
+};
+
+class OnroadScreenBrightnessControl : public ExpandableToggleRow {
+  Q_OBJECT
+
+public:
+  OnroadScreenBrightnessControl(const QString &param, const QString &title, const QString &desc, const QString &icon,
+                                QWidget *parent = nullptr);
+  void refresh();
+
+private:
+  Params params;
+  OptionControlSP *onroadScreenOffTimer;
+  OptionControlSP *onroadScreenBrightness;
+};
@@ -0,0 +1,40 @@
+/**
+ * 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/display_panel.h"
+
+DisplayPanel::DisplayPanel(QWidget *parent) : QWidget(parent) {
+  main_layout = new QStackedLayout(this);
+  ListWidgetSP *list = new ListWidgetSP(this, false);
+
+  sunnypilotScreen = new QWidget(this);
+  QVBoxLayout* vlayout = new QVBoxLayout(sunnypilotScreen);
+  vlayout->setContentsMargins(50, 20, 50, 20);
+
+  // Onroad Screen Off/Brightness
+  onroadScreenBrightnessControl = new OnroadScreenBrightnessControl(
+    "OnroadScreenOffControl",
+    tr("Driving Screen Off: Non-Critical Events"),
+    tr("Turn off device screen or reduce brightness after driving starts. "
+       "It automatically brightens again when screen is touched or a critical event occurs."),
+    "",
+    this);
+  list->addItem(onroadScreenBrightnessControl);
+
+  sunnypilotScroller = new ScrollViewSP(list, this);
+  vlayout->addWidget(sunnypilotScroller);
+  main_layout->addWidget(sunnypilotScreen);
+}
+
+void DisplayPanel::showEvent(QShowEvent *event) {
+  QWidget::showEvent(event);
+  refresh();
+}
+
+void DisplayPanel::refresh() {
+  onroadScreenBrightnessControl->refresh();
+}
@@ -0,0 +1,28 @@
+/**
+ * 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/qt/offroad/settings/display/onroad_screen_brightness.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
+
+class DisplayPanel : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit DisplayPanel(QWidget *parent = nullptr);
+  void showEvent(QShowEvent *event) override;
+  void refresh();
+
+private:
+  QStackedLayout* main_layout = nullptr;
+  QWidget* sunnypilotScreen = nullptr;
+  ScrollViewSP *sunnypilotScroller = nullptr;
+  Params params;
+  OnroadScreenBrightnessControl *onroadScreenBrightnessControl = nullptr;
+};
@@ -52,12 +52,16 @@ void MadsSettings::updateToggles(bool _offroad) {
  );

  auto cp_bytes = params.get("CarParamsPersistent");
-  if (!cp_bytes.empty()) {
+  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
+  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
    AlignedBuffer aligned_buf;
+    AlignedBuffer aligned_buf_sp;
    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();

-    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
+    if (madsLimitedSettings(CP, CP_SP)) {
      params.remove("MadsMainCruiseAllowed");
      params.putBool("MadsUnifiedEngagementMode", true);
      params.put("MadsSteeringMode", std::to_string(static_cast<int>(MadsSteeringMode::DISENGAGE)));
@@ -12,7 +12,16 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"

-const std::vector<std::string> mads_limited_settings_brands = {"rivian", "tesla"};
+inline bool madsLimitedSettings(const cereal::CarParams::Reader &CP, const cereal::CarParamsSP::Reader &CP_SP) {
+  if (CP.getBrand() == "rivian") {
+    return true;
+  }
+  if (CP.getBrand() == "tesla") {
+    return !(CP_SP.getFlags() & 1);  // 1 == TeslaFlagsSP.HAS_VEHICLE_BUS
+  }
+
+  return false;
+}

 enum class MadsSteeringMode {
  REMAIN_ACTIVE = 0,
@@ -134,12 +134,16 @@ void LateralPanel::updateToggles(bool _offroad) {
  }

  auto cp_bytes = params.get("CarParamsPersistent");
-  if (!cp_bytes.empty()) {
+  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
+  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
    AlignedBuffer aligned_buf;
+    AlignedBuffer aligned_buf_sp;
    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();

-    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
+    if (madsLimitedSettings(CP, CP_SP)) {
      madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_LIMITED_COMPATIBILITY, MADS_BASE_DESC));
    } else {
      madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_FULL_COMPATIBILITY, MADS_BASE_DESC));
@@ -38,10 +38,12 @@ enum class SpeedLimitMode {
  OFF,
  INFORMATION,
  WARNING,
+  ASSIST,
 };

 inline const QString SpeedLimitModeTexts[]{
  QObject::tr("Off"),
  QObject::tr("Information"),
  QObject::tr("Warning"),
+  QObject::tr("Assist"),
 };
@@ -28,6 +28,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
    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",
@@ -35,7 +36,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
    "",
    "",
    speed_limit_mode_texts,
-    385);
+    380);
  list->addItem(speed_limit_mode_settings);

  list->addItem(horizontal_line());
@@ -107,6 +108,25 @@ void SpeedLimitSettings::refresh() {
  SpeedLimitOffsetType offset_type_param = static_cast<SpeedLimitOffsetType>(std::atoi(params.get("SpeedLimitOffsetType").c_str()));
  QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset"));

+  bool has_longitudinal_control;
+  bool intelligent_cruise_button_management_available;
+  auto cp_bytes = params.get("CarParamsPersistent");
+  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
+  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
+    AlignedBuffer aligned_buf;
+    AlignedBuffer aligned_buf_sp;
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
+    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
+
+    has_longitudinal_control = hasLongitudinalControl(CP);
+    intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
+  } else {
+    has_longitudinal_control = false;
+    intelligent_cruise_button_management_available = false;
+  }
+
  speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param));
  speed_limit_offset->setDescription(offsetDescription(offset_type_param));

@@ -124,6 +144,13 @@ void SpeedLimitSettings::refresh() {
    speed_limit_offset->showDescription();
  }

+  if (has_longitudinal_control || intelligent_cruise_button_management_available) {
+    speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(getSpeedLimitModeValues()));
+  } else {
+    speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(
+      {SpeedLimitMode::OFF,SpeedLimitMode::INFORMATION, SpeedLimitMode::WARNING}));
+  }
+
  speed_limit_mode_settings->showDescription();
  speed_limit_offset->showDescription();
 }
@@ -58,8 +58,11 @@ private:
    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.");
+    QString assist_str = tr("⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons.");

-    if (mode == SpeedLimitMode::WARNING) {
+    if (mode == SpeedLimitMode::ASSIST) {
+      assist_str = "<font color='white'><b>" + assist_str + "</b></font>";
+    } else 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>";
@@ -67,9 +70,27 @@ private:
      off_str = "<font color='white'><b>" + off_str + "</b></font>";
    }

-    return QString("%1<br>%2<br>%3")
+    return QString("%1<br>%2<br>%3<br>%4")
        .arg(off_str)
        .arg(info_str)
-        .arg(warning_str);
+        .arg(warning_str)
+        .arg(assist_str);
+  }
+
+  static std::vector<SpeedLimitMode> getSpeedLimitModeValues() {
+    std::vector<SpeedLimitMode> values;
+    for (int i = static_cast<int>(SpeedLimitMode::OFF);
+         i <= static_cast<int>(SpeedLimitMode::ASSIST); ++i) {
+      values.push_back(static_cast<SpeedLimitMode>(i));
+    }
+    return values;
+  }
+
+  static std::vector<int> convertSpeedLimitModeValues(const std::vector<SpeedLimitMode> &modes) {
+    std::vector<int> values;
+    for (const auto& mode : modes) {
+      values.push_back(static_cast<int>(mode));
+    }
+    return values;
  }
 };
@@ -13,6 +13,7 @@

 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/developer_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h"
@@ -88,6 +89,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) {
    PanelInfo("   " + tr("Steering"), new LateralPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_lateral.png"),
    PanelInfo("   " + tr("Cruise"), new LongitudinalPanel(this), "../assets/icons/speed_limit.png"),
    PanelInfo("   " + tr("Visuals"), new VisualsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_visuals.png"),
+    PanelInfo("   " + tr("Display"), new DisplayPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_display.png"),
    PanelInfo("   " + tr("OSM"), new OsmPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_map.png"),
    PanelInfo("   " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"),
    PanelInfo("   " + tr("Vehicle"), new VehiclePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_vehicle.png"),
@@ -49,6 +49,26 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
      "",
      false,
    },
+    {
+      "GreenLightAlert",
+      tr("Green Traffic Light Alert (Beta)"),
+      QString("%1<br>"
+        "<h4>%2</h4><br>")
+        .arg(tr("A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you."))
+        .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")),
+      "",
+      false,
+    },
+    {
+      "LeadDepartAlert",
+      tr("Lead Departure Alert (Beta)"),
+      QString("%1<br>"
+        "<h4>%2</h4><br>")
+        .arg(tr("A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving."))
+        .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")),
+      "",
+      false,
+    },
  };

  // Add regular toggles first
@@ -11,7 +11,17 @@
 #include "selfdrive/ui/qt/util.h"


-HudRendererSP::HudRendererSP() {}
+HudRendererSP::HudRendererSP() {
+  plus_arrow_up_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_plus_arrow_up", {105, 105});
+  minus_arrow_down_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_minus_arrow_down", {105, 105});
+
+  int small_max = e2e_alert_small * 2 - 40;
+  int large_max = e2e_alert_large * 2 - 40;
+  green_light_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {small_max, small_max});
+  green_light_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {large_max, large_max});
+  lead_depart_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {small_max, small_max});
+  lead_depart_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {large_max, large_max});
+}

 void HudRendererSP::updateState(const UIState &s) {
  HudRenderer::updateState(s);
@@ -36,6 +46,8 @@ void HudRendererSP::updateState(const UIState &s) {
  speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid();
  speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast() * speedConv;
  speedLimitMode = static_cast<SpeedLimitMode>(s.scene.speed_limit_mode);
+  speedLimitAssistState = lp_sp.getSpeedLimit().getAssist().getState();
+  speedLimitAssistActive = lp_sp.getSpeedLimit().getAssist().getActive();
  roadName = s.scene.road_name;
  if (sm.updated("liveMapDataSP")) {
    roadNameStr = QString::fromStdString(lmd.getRoadName());
@@ -100,10 +112,22 @@ void HudRendererSP::updateState(const UIState &s) {
  smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
  smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled();
  smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive();
+
+  greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert();
+  leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert();
 }

 void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
  HudRenderer::draw(p, surface_rect);
+
+  e2eAlertDisplayTimer = std::max(0, e2eAlertDisplayTimer - 1);
+
+  p.save();
+
+  if (is_cruise_available) {
+    drawSetSpeedSP(p, surface_rect);
+  }
+
  if (!reversing) {
    // Smart Cruise Control
    int x_offset = -260;
@@ -152,14 +176,61 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
    }

    // Speed Limit
-    if (speedLimitMode != SpeedLimitMode::OFF) {
-      drawSpeedLimitSigns(p);
-      drawUpcomingSpeedLimit(p);
+    bool showSpeedLimit;
+    bool speed_limit_assist_pre_active_pulse = pulseElement(speedLimitAssistFrame);
+
+    // Position speed limit sign next to set 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);
+
+    if (speedLimitAssistState == cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
+      speedLimitAssistFrame++;
+      showSpeedLimit = speed_limit_assist_pre_active_pulse;
+      drawSpeedLimitPreActiveArrow(p, sign_rect);
+    } else {
+      speedLimitAssistFrame = 0;
+      showSpeedLimit = speedLimitMode != SpeedLimitMode::OFF;
+    }
+
+    if (showSpeedLimit) {
+      drawSpeedLimitSigns(p, sign_rect);
+
+      // do not show during SLA's preActive state
+      if (speedLimitAssistState != cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
+        drawUpcomingSpeedLimit(p);
+      }
    }

    // Road Name
    drawRoadName(p, surface_rect);
+
+    // Green Light & Lead Depart Alerts
+    if (greenLightAlert or leadDepartAlert) {
+      e2eAlertDisplayTimer = 3 * UI_FREQ;
+      // reset onroad sleep timer for e2e alerts
+      uiStateSP()->reset_onroad_sleep_timer();
+    }
+
+    if (e2eAlertDisplayTimer > 0) {
+      e2eAlertFrame++;
+      if (greenLightAlert) {
+        alert_text = tr("GREEN\nLIGHT");
+        alert_img = devUiInfo > 0 ? green_light_alert_small_img : green_light_alert_large_img;
+      }
+      else if (leadDepartAlert) {
+        alert_text = tr("LEAD VEHICLE\nDEPARTING");
+        alert_img = devUiInfo > 0 ? lead_depart_alert_small_img : lead_depart_alert_large_img;
+      }
+      drawE2eAlert(p, surface_rect);
+    } else {
+      e2eAlertFrame = 0;
+    }
  }
+
+  p.restore();
 }

 void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QColor color) {
@@ -348,10 +419,10 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
  }
 }

-void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
-  bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING;  // TODO-SP: update to include SpeedLimitMode::ASSIST
+void HudRendererSP::drawSpeedLimitSigns(QPainter &p, QRect &sign_rect) {
+  bool speedLimitWarningEnabled = speedLimitMode >= SpeedLimitMode::WARNING;  // TODO-SP: update to include SpeedLimitMode::ASSIST
  bool hasSpeedLimit = speedLimitValid || speedLimitLastValid;
-  bool overspeed = hasSpeedLimit && speedLimitFinalLast < std::nearbyint(speed);
+  bool overspeed = hasSpeedLimit && std::nearbyint(speedLimitFinalLast) < std::nearbyint(speed);
  QString speedLimitStr = hasSpeedLimit ? QString::number(std::nearbyint(speedLimitLast)) : "---";

  // Offset display text
@@ -365,13 +436,6 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
    speedLimitSubTextFactor = 0.475;
  }

-  // 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 :
@@ -573,3 +637,97 @@ void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) {
  QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20);
  p.drawText(road_rect, Qt::AlignCenter, truncated);
 }
+
+void HudRendererSP::drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect) {
+  const int sign_margin = 12;
+  const int arrow_spacing = sign_margin * 3;
+  int arrow_x = sign_rect.right() + arrow_spacing;
+
+  int _set_speed = std::nearbyint(set_speed);
+  int _speed_limit_final_last = std::nearbyint(speedLimitFinalLast);
+
+  // Calculate the vertical offset using a sinusoidal function for smooth bouncing
+  double bounce_frequency = 2.0 * M_PI / UI_FREQ;  // 20 frames for one full oscillation
+  int bounce_offset = 20 * sin(speedLimitAssistFrame * bounce_frequency);  // Adjust the amplitude (20 pixels) as needed
+
+  if (_set_speed < _speed_limit_final_last) {
+    QPoint iconPosition(arrow_x, sign_rect.center().y() - plus_arrow_up_img.height() / 2 + bounce_offset);
+    p.drawPixmap(iconPosition, plus_arrow_up_img);
+  } else if (_set_speed > _speed_limit_final_last) {
+    QPoint iconPosition(arrow_x, sign_rect.center().y() - minus_arrow_down_img.height() / 2 - bounce_offset);
+    p.drawPixmap(iconPosition, minus_arrow_down_img);
+  }
+}
+
+void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) {
+  // Draw outer box + border to contain set speed
+  const QSize default_size = {172, 204};
+  QSize set_speed_size = is_metric ? QSize(200, 204) : default_size;
+  QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
+
+  // Draw set speed box
+  p.setPen(QPen(QColor(255, 255, 255, 75), 6));
+  p.setBrush(QColor(0, 0, 0, 166));
+  p.drawRoundedRect(set_speed_rect, 32, 32);
+
+  // Colors based on status
+  QColor max_color = QColor(0xa6, 0xa6, 0xa6, 0xff);
+  QColor set_speed_color = QColor(0x72, 0x72, 0x72, 0xff);
+  if (is_cruise_set) {
+    set_speed_color = QColor(255, 255, 255);
+    if (speedLimitAssistActive) {
+      set_speed_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 0xff, 0, 0xff);
+      max_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0x80, 0xd8, 0xa6, 0xff);
+    } else {
+      if (status == STATUS_DISENGAGED) {
+        max_color = QColor(255, 255, 255);
+      } else if (status == STATUS_OVERRIDE) {
+        max_color = QColor(0x91, 0x9b, 0x95, 0xff);
+      } else {
+        max_color = QColor(0x80, 0xd8, 0xa6, 0xff);
+      }
+    }
+  }
+
+  // Draw "MAX" text
+  p.setFont(InterFont(40, QFont::DemiBold));
+  p.setPen(max_color);
+  p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("MAX"));
+
+  // Draw set speed
+  QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(set_speed)) : "–";
+  p.setFont(InterFont(90, QFont::Bold));
+  p.setPen(set_speed_color);
+  p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr);
+}
+
+void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect) {
+  int size = devUiInfo > 0 ? e2e_alert_small : e2e_alert_large;
+  int x = surface_rect.center().x() + surface_rect.width() / 4;
+  int y = surface_rect.center().y() + 40;
+  x += devUiInfo > 0 ? 0 : 50;
+  y += devUiInfo > 0 ? 0 : 80;
+  QRect alertRect(x - size, y - size, size * 2, size * 2);
+
+  // Alert Circle
+  QPoint center = alertRect.center();
+  QColor frameColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 75) : QColor(0, 255, 0, 75);
+  p.setPen(QPen(frameColor, 15));
+  p.setBrush(QColor(0, 0, 0, 190));
+  p.drawEllipse(center, size, size);
+
+  // Alert Text
+  QColor txtColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 255) : QColor(0, 255, 0, 255);
+  p.setFont(InterFont(48, QFont::Bold));
+  p.setPen(txtColor);
+  QFontMetrics fm(p.font());
+  QRect textRect = fm.boundingRect(alertRect, Qt::TextWordWrap, alert_text);
+  textRect.moveCenter({alertRect.center().x(), alertRect.center().y()});
+  textRect.moveBottom(alertRect.bottom() - alertRect.height() / 7);
+  p.drawText(textRect, Qt::AlignCenter, alert_text);
+
+  // Alert Image
+  QPointF pixmapCenterOffset = QPointF(alert_img.width() / 2.0, alert_img.height() / 2.0);
+  QPointF drawPoint = center - pixmapCenterOffset;
+  p.drawPixmap(drawPoint, alert_img);
+}
@@ -31,9 +31,12 @@ 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 drawSpeedLimitSigns(QPainter &p, QRect &sign_rect);
  void drawUpcomingSpeedLimit(QPainter &p);
  void drawRoadName(QPainter &p, const QRect &surface_rect);
+  void drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect);
+  void drawSetSpeedSP(QPainter &p, const QRect &surface_rect);
+  void drawE2eAlert(QPainter &p, const QRect &surface_rect);

  bool lead_status;
  float lead_d_rel;
@@ -86,4 +89,21 @@ private:
  SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF;
  bool roadName;
  QString roadNameStr;
+  cereal::LongitudinalPlanSP::SpeedLimit::AssistState speedLimitAssistState;
+  bool speedLimitAssistActive;
+  int speedLimitAssistFrame;
+  QPixmap plus_arrow_up_img;
+  QPixmap minus_arrow_down_img;
+  int e2e_alert_small = 250;
+  int e2e_alert_large = 300;
+  QPixmap green_light_alert_small_img;
+  QPixmap green_light_alert_large_img;
+  bool greenLightAlert;
+  int e2eAlertFrame;
+  int e2eAlertDisplayTimer = 0;
+  bool leadDepartAlert;
+  QPixmap lead_depart_alert_small_img;
+  QPixmap lead_depart_alert_large_img;
+  QString alert_text;
+  QPixmap alert_img;
 };
@@ -25,8 +25,10 @@ void OnroadWindowSP::updateState(const UIStateSP &s) {

 void OnroadWindowSP::mousePressEvent(QMouseEvent *e) {
  OnroadWindow::mousePressEvent(e);
+  uiStateSP()->reset_onroad_sleep_timer();
 }

 void OnroadWindowSP::offroadTransition(bool offroad) {
  OnroadWindow::offroadTransition(offroad);
+  uiStateSP()->reset_onroad_sleep_timer();
 }
@@ -11,6 +11,15 @@

 void UIStateSP::updateStatus() {
  UIState::updateStatus();
+
+  if (scene.started && scene.onroadScreenOffControl) {
+    auto selfdriveState = (*sm)["selfdriveState"].getSelfdriveState();
+    if (selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE) {
+      reset_onroad_sleep_timer();
+    } else if (scene.onroadScreenOffTimer > 0) {
+      scene.onroadScreenOffTimer--;
+    }
+  }
 }

 UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
@@ -55,6 +64,20 @@ void ui_update_params_sp(UIStateSP *s) {
  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("RoadNameToggle");
+
+  // Onroad Screen Brightness
+  s->scene.onroadScreenOffBrightness = std::atoi(params.get("OnroadScreenOffBrightness").c_str());
+  s->scene.onroadScreenOffControl = params.getBool("OnroadScreenOffControl");
+  s->scene.onroadScreenOffTimerParam = std::atoi(params.get("OnroadScreenOffTimer").c_str());
+  s->reset_onroad_sleep_timer();
+}
+
+void UIStateSP::reset_onroad_sleep_timer() {
+  if (scene.onroadScreenOffTimerParam >= 0 and scene.onroadScreenOffControl) {
+    scene.onroadScreenOffTimer = scene.onroadScreenOffTimerParam * UI_FREQ;
+  } else {
+    scene.onroadScreenOffTimer = -1;
+  }
 }

 DeviceSP::DeviceSP(QObject *parent) : Device(parent) {
@@ -61,6 +61,7 @@ public:
      return user.user_id.toLower() != "unregisteredsponsor" && user.user_id.toLower() != "temporarysponsor";
    });
  }
+  void reset_onroad_sleep_timer();

 signals:
  void sunnylinkRoleChanged(bool subscriber);
@@ -12,4 +12,7 @@ typedef struct UISceneSP : UIScene {
  bool standstill_timer = false;
  int speed_limit_mode = 0;
  bool road_name = false;
+  int onroadScreenOffBrightness, onroadScreenOffTimer = 0;
+  bool onroadScreenOffControl;
+  int onroadScreenOffTimerParam;
 } UISceneSP;
@@ -210,6 +210,13 @@ void Device::updateBrightness(const UIState &s) {
    brightness = 0;
  }

+  // Onroad Brightness Control
+#ifdef SUNNYPILOT
+  if (awake && s.scene.started && s.scene.onroadScreenOffControl && s.scene.onroadScreenOffTimer == 0) {
+    brightness = s.scene.onroadScreenOffBrightness * 0.01 * brightness;
+  }
+#endif
+
  if (brightness != last_brightness) {
    if (!brightness_future.isRunning()) {
      brightness_future = QtConcurrent::run(Hardware::set_brightness, brightness);
@@ -9,6 +9,10 @@ from openpilot.common.params import Params
 from opendbc.car import structs
 from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP
+from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
+
+
+MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla")


 class MadsSteeringModeOnBrake:
@@ -17,20 +21,25 @@ class MadsSteeringModeOnBrake:
  DISENGAGE = 2


-def get_mads_limited_brands(CP: structs.CarParams) -> bool:
-  return CP.brand in ("rivian", "tesla")
+def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool:
+  if CP.brand == 'rivian':
+    return True
+  if CP.brand == 'tesla':
+    return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS
+
+  return False


-def read_steering_mode_param(CP: structs.CarParams, params: Params):
-  if get_mads_limited_brands(CP):
+def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
+  if get_mads_limited_brands(CP, CP_SP):
    return MadsSteeringModeOnBrake.DISENGAGE

  return params.get("MadsSteeringMode", return_default=True)


-def set_alternative_experience(CP: structs.CarParams, params: Params):
+def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
  enabled = params.get_bool("Mads")
-  steering_mode = read_steering_mode_param(CP, params)
+  steering_mode = read_steering_mode_param(CP, CP_SP, params)

  if enabled:
    CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
@@ -53,9 +62,12 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p
  # MADS Partial Support
  # MADS is currently partially supported for these platforms due to lack of consistent states to engage controls
  # Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms
-  # TODO-SP: To enable MADS full support for Rivian/Tesla, identify consistent signals for MADS toggling
-  mads_partial_support = get_mads_limited_brands(CP)
+  # TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling
+  mads_partial_support = get_mads_limited_brands(CP, CP_SP)
  if mads_partial_support:
    params.put("MadsSteeringMode", 2)
    params.put_bool("MadsUnifiedEngagementMode", True)
+
+  # no ACC MAIN button for these brands
+  if CP.brand in MADS_NO_ACC_MAIN_BUTTON:
    params.remove("MadsMainCruiseAllowed")
@@ -10,7 +10,7 @@ from cereal import log, custom
 from opendbc.car import structs
 from opendbc.car.hyundai.values import HyundaiFlags
 from openpilot.common.params import Params
-from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, get_mads_limited_brands
+from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON
 from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT

 State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
@@ -27,6 +27,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 class ModularAssistiveDrivingSystem:
  def __init__(self, selfdrive):
    self.CP = selfdrive.CP
+    self.CP_SP = selfdrive.CP_SP
    self.params = selfdrive.params

    self.enabled = False
@@ -43,14 +44,16 @@ class ModularAssistiveDrivingSystem:
    if self.CP.brand == "hyundai":
      if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
        self.allow_always = True
+    if self.CP.brand == "tesla":
+      self.allow_always = True

-    if get_mads_limited_brands(self.CP):
+    if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON:
      self.no_main_cruise = True

    # read params on init
    self.enabled_toggle = self.params.get_bool("Mads")
    self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
-    self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.params)
+    self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params)
    self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")

  def read_params(self):
@@ -102,12 +105,13 @@ class ModularAssistiveDrivingSystem:

  def update_events(self, CS: structs.CarState):
    if not self.selfdrive.enabled and self.enabled:
-      if self.events.has(EventName.doorOpen):
-        self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
-        self.transition_paused_state()
-      if self.events.has(EventName.seatbeltNotLatched):
-        self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
-        self.transition_paused_state()
+      if CS.standstill:
+        if self.events.has(EventName.doorOpen):
+          self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
+          self.transition_paused_state()
+        if self.events.has(EventName.seatbeltNotLatched):
+          self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
+          self.transition_paused_state()
      if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse):
        self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
        self.transition_paused_state()
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3aa5ec9ac1daee6a549e62647d90bcaa66d2485f7df7f386ff902fcfb04c1716
+size 6583
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:087db35bd469e85aefe3b45636f11ab3e8b55ceb7bc94ea059cfd9a69c2f338f
+size 8914
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0f4a0bf26cfd2c64939759d43bee839aadfd017a6f74065095a27b03615e55a4
+size 26627
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3e451ff31e9b3f144822ab282b8e47cd5a603ca59ff94bd1849271181b86c6b1
+size 29220
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:614767308d435d165a9ab78e3eac22ee15697d94066df8200ef32afb33ef8d60
+size 5698
@@ -18,9 +18,9 @@ GearShifter = structs.CarState.GearShifter


 class CarSpecificEventsSP:
-  def __init__(self, CP: structs.CarParams, params):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
    self.CP = CP
-    self.params = params
+    self.CP_SP = CP_SP

    self.low_speed_alert = False

@@ -42,4 +42,10 @@ class CarSpecificEventsSP:
      if self.low_speed_alert:
        events.add(EventName.belowSteerSpeed)

+    elif self.CP.brand == 'toyota':
+      if self.CP.openpilotLongitudinalControl:
+        if CS.cruiseState.standstill and not CS.brakePressed and self.CP_SP.enableGasInterceptor:
+          if events.has(EventName.resumeRequired):
+            events.remove(EventName.resumeRequired)
+
    return events_sp
@@ -6,18 +6,24 @@ See the LICENSE.md file in the root directory for more details.
 """
 import numpy as np

-from cereal import car
+from cereal import car, custom
 from opendbc.car import structs
+from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target

 ButtonType = car.CarState.ButtonEvent.Type
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState

 CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
                       ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
                       ButtonType.cancel: 0, ButtonType.mainCruise: 0}

 V_CRUISE_MIN = 8
+V_CRUISE_MAX = 145
+V_CRUISE_UNSET = 255


 def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
@@ -32,11 +38,12 @@ def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarSta
      button_timers[b.type.raw] = 1 if b.pressed else 0


-
 class VCruiseHelperSP:
  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
    self.CP = CP
    self.CP_SP = CP_SP
+    self.v_cruise_kph = V_CRUISE_UNSET
+    self.v_cruise_cluster_kph = V_CRUISE_UNSET
    self.params = Params()
    self.v_cruise_min = 0
    self.enabled_prev = False
@@ -47,6 +54,16 @@ class VCruiseHelperSP:

    self.enable_button_timers = CRUISE_BUTTON_TIMER

+    # Speed Limit Assist
+    self.sla_state = SpeedLimitAssistState.disabled
+    self.prev_sla_state = SpeedLimitAssistState.disabled
+    self.has_speed_limit = False
+    self.speed_limit_final_last = 0.
+    self.speed_limit_final_last_kph = 0.
+    self.prev_speed_limit_final_last_kph = 0.
+    self.req_plus = False
+    self.req_minus = False
+
  def read_custom_set_speed_params(self) -> None:
    self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
    self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
@@ -89,3 +106,33 @@ class VCruiseHelperSP:
      return enabled and self.enabled_prev

    return enabled
+
+  def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None:
+    resolver = LP_SP.speedLimit.resolver
+    self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
+    self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast
+    self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
+    self.sla_state = LP_SP.speedLimit.assist.state
+    self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
+                                                           self.speed_limit_final_last, is_metric)
+
+  @property
+  def update_speed_limit_final_last_changed(self) -> bool:
+    return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
+
+  def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool:
+    if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive:
+      if button_type == ButtonType.decelCruise and self.req_minus:
+        return True
+      if button_type == ButtonType.accelCruise and self.req_plus:
+        return True
+
+    return False
+
+  def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
+    if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or
+                                                self.update_speed_limit_final_last_changed):
+      self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
+
+    self.prev_sla_state = self.sla_state
+    self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph
@@ -0,0 +1,58 @@
+"""
+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 cereal import messaging, custom
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+TRIGGER_THRESHOLD = 30
+
+
+class E2EAlertsHelper:
+  def __init__(self):
+    self._params = Params()
+    self._frame = -1
+
+    self.green_light_alert = False
+    self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
+    self.lead_depart_alert = False
+    self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
+
+  def _read_params(self) -> None:
+    if self._frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
+      self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
+
+    self._frame += 1
+
+  def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None:
+    self._read_params()
+
+    if not (self.green_light_alert_enabled or self.lead_depart_alert_enabled):
+      return
+
+    CS = sm['carState']
+    CC = sm['carControl']
+
+    model_x = sm['modelV2'].position.x
+    max_idx = len(model_x) - 1
+    has_lead = sm['radarState'].leadOne.status
+    lead_vRel: float = sm['radarState'].leadOne.vRel
+
+    # Green light alert
+    self.green_light_alert = (self.green_light_alert_enabled and model_x[max_idx] > TRIGGER_THRESHOLD
+                              and not has_lead and CS.standstill and not CS.gasPressed and not CC.enabled)
+
+    # Lead Departure Alert
+    self.lead_depart_alert = (self.lead_depart_alert_enabled and CS.standstill and model_x[max_idx] > 30
+                              and has_lead and lead_vRel > 1 and not CS.gasPressed)
+
+    if self.green_light_alert or self.green_light_alert:
+      events_sp.add(custom.OnroadEventSP.EventName.e2eChime)
@@ -7,22 +7,31 @@ See the LICENSE.md file in the root directory for more details.

 from cereal import messaging, custom
 from opendbc.car import structs
+from openpilot.common.constants import CV
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.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
-Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
+LongitudinalPlanSource = 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.resolver = SpeedLimitResolver()
+    self.sla = SpeedLimitAssist(CP)
    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
-    self.source = Source.cruise
+    self.source = LongitudinalPlanSource.cruise
+    self.e2e_alerts_helper = E2EAlertsHelper()

    self.output_v_target = 0.
    self.output_a_target = 0.
@@ -39,15 +48,29 @@ 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)
+    CS = sm['carState']
+    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
+    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
+
+    long_enabled = sm['carControl'].enabled
+    long_override = sm['carControl'].cruiseControl.override
+
+    # Smart Cruise Control
+    self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

    # Speed Limit Resolver
    self.resolver.update(v_ego, sm)

+    # Speed Limit Assist
+    has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid
+    self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit,
+                    self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, 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.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target),
+      LongitudinalPlanSource.cruise: (v_cruise, a_ego),
+      LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
+      LongitudinalPlanSource.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target),
+      LongitudinalPlanSource.speedLimitAssist: (self.sla.output_v_target, self.sla.output_a_target),
    }

    self.source = min(targets, key=lambda k: targets[k][0])
@@ -55,7 +78,9 @@ class LongitudinalPlannerSP:
    return self.output_v_target, self.output_a_target

  def update(self, sm: messaging.SubMaster) -> None:
+    self.events_sp.clear()
    self.dec.update(sm)
+    self.e2e_alerts_helper.update(sm, self.events_sp)

  def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
    plan_sp_send = messaging.new_message('longitudinalPlanSP')
@@ -66,6 +91,7 @@ class LongitudinalPlannerSP:
    longitudinalPlanSP.longitudinalPlanSource = self.source
    longitudinalPlanSP.vTarget = float(self.output_v_target)
    longitudinalPlanSP.aTarget = float(self.output_a_target)
+    longitudinalPlanSP.events = self.events_sp.to_msg()

    # Dynamic Experimental Control
    dec = longitudinalPlanSP.dec
@@ -104,5 +130,16 @@ class LongitudinalPlannerSP:
    resolver.speedLimitOffset = float(self.resolver.speed_limit_offset)
    resolver.distToSpeedLimit = float(self.resolver.distance)
    resolver.source = self.resolver.source
+    assist = speedLimit.assist
+    assist.state = self.sla.state
+    assist.enabled = self.sla.is_enabled
+    assist.active = self.sla.is_active
+    assist.vTarget = float(self.sla.output_v_target)
+    assist.aTarget = float(self.sla.output_a_target)
+
+    # E2E Alerts
+    e2eAlerts = longitudinalPlanSP.e2eAlerts
+    e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert
+    e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert

    pm.send('longitudinalPlanSP', plan_sp_send)
@@ -14,9 +14,6 @@ class SmartCruiseControl:
    self.vision = SmartCruiseControlVision()
    self.map = SmartCruiseControlMap()

-  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.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
    self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
@@ -6,3 +6,14 @@ 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.
+
+# Speed Limit Assist constants
+PCM_LONG_REQUIRED_MAX_SET_SPEED = {
+  True: (33.3333, 36.1111),  # km/h, (120, 130)
+  False: (31.2928, 35.7632),  # mph, (70, 80)
+}
+
+CONFIRM_SPEED_THRESHOLD = {
+  True: 80,   # km/h
+  False: 50,  # mph
+}
@@ -19,3 +19,10 @@ class OffsetType(IntEnum):
  off = 0
  fixed = 1
  percentage = 2
+
+
+class Mode(IntEnum):
+  off = 0
+  information = 1
+  warning = 2
+  assist = 3
@@ -0,0 +1,19 @@
+"""
+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 openpilot.common.constants import CV
+
+
+def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
+  speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
+  v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv)
+  target_set_speed_conv = round(target_set_speed * speed_conv)
+
+  req_plus = v_cruise_cluster_conv < target_set_speed_conv
+  req_minus = v_cruise_cluster_conv > target_set_speed_conv
+
+  return req_plus, req_minus
@@ -0,0 +1,395 @@
+"""
+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
+
+from cereal import custom, car
+from openpilot.common.params import Params
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+ButtonType = car.CarState.ButtonEvent.Type
+EventNameSP = custom.OnroadEventSP.EventName
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+
+ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
+ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
+
+DISABLED_GUARD_PERIOD = 0.5  # secs.
+PRE_ACTIVE_GUARD_PERIOD = 15  # secs. Time to wait after activation before considering temp deactivation signal.
+SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1  # secs. Time to wait after speed limit change before switching to preActive.
+
+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.
+V_CRUISE_UNSET = 255
+
+CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
+CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
+CRUISE_BUTTON_CONFIRM_HOLD = 0.5  # secs.
+
+
+class SpeedLimitAssist:
+  _speed_limit_final_last: float
+  _distance: float
+  v_ego: float
+  a_ego: float
+  v_offset: 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("SpeedLimitMode", return_default=True) == Mode.assist
+    self.long_enabled = False
+    self.long_enabled_prev = False
+    self.is_enabled = False
+    self.is_active = False
+    self.output_v_target = V_CRUISE_UNSET
+    self.output_a_target = 0.
+    self.v_ego = 0.
+    self.a_ego = 0.
+    self.v_offset = 0.
+    self.target_set_speed_conv = 0
+    self.prev_target_set_speed_conv = 0
+    self.v_cruise_cluster = 0.
+    self.v_cruise_cluster_prev = 0.
+    self.v_cruise_cluster_conv = 0
+    self.prev_v_cruise_cluster_conv = 0
+    self._has_speed_limit = False
+    self._speed_limit = 0.
+    self._speed_limit_final_last = 0.
+    self.speed_limit_prev = 0.
+    self.speed_limit_final_last_conv = 0
+    self.prev_speed_limit_final_last_conv = 0
+    self._distance = 0.
+    self.state = SpeedLimitAssistState.disabled
+    self._state_prev = SpeedLimitAssistState.disabled
+    self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
+
+    self._plus_hold = 0.
+    self._minus_hold = 0.
+    self._last_carstate_ts = 0.
+
+    # TODO-SP: SLA's own output_a_target for planner
+    # 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_changed(self) -> bool:
+    return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev)
+
+  @property
+  def v_cruise_cluster_changed(self) -> bool:
+    return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv)
+
+  @property
+  def target_set_speed_confirmed(self) -> bool:
+    return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
+
+  def get_v_target_from_control(self) -> float:
+    if self._has_speed_limit:
+      if self.pcm_op_long and self.is_enabled:
+        return self._speed_limit_final_last
+      if not self.pcm_op_long and self.is_active:
+        return self._speed_limit_final_last
+
+    # Fallback
+    return V_CRUISE_UNSET
+
+  # TODO-SP: SLA's own output_a_target for planner
+  def get_a_target_from_control(self) -> float:
+    return self.a_ego
+
+  def update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.is_metric = self.params.get_bool("IsMetric")
+      self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
+
+  def update_car_state(self, CS: car.CarState) -> None:
+    now = time.monotonic()
+    self._last_carstate_ts = now
+
+    for b in CS.buttonEvents:
+      if not b.pressed:
+        if b.type in CRUISE_BUTTONS_PLUS:
+          self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
+        elif b.type in CRUISE_BUTTONS_MINUS:
+          self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
+
+  def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
+    now = time.monotonic()
+    if req_plus and now <= self._plus_hold:
+      self._plus_hold = 0.
+      return True
+    elif req_minus and now <= self._minus_hold:
+      self._minus_hold = 0.
+      return True
+
+    # expired
+    if now > self._plus_hold:
+      self._plus_hold = 0.
+    if now > self._minus_hold:
+      self._minus_hold = 0.
+    return False
+
+  def update_calculations(self, v_cruise_cluster: float) -> None:
+    speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
+    self.v_cruise_cluster = v_cruise_cluster
+
+    # Update current velocity offset (error)
+    self.v_offset = self._speed_limit_final_last - self.v_ego
+
+    self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
+    self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
+
+    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
+    pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \
+                            cst_high
+    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
+
+    self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
+
+  @property
+  def apply_confirm_speed_threshold(self) -> bool:
+    # below CST: always require user confirmation
+    if self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]:
+      return True
+
+    # at/above CST:
+    # - new speed limit >= CST: auto change
+    # - new speed limit < CST: user confirmation required
+    return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
+
+  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_last ** 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_confirmed_state(self):
+    if self._has_speed_limit:
+      if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+        self.state = SpeedLimitAssistState.adapting
+      else:
+        self.state = SpeedLimitAssistState.active
+    else:
+      self.state = SpeedLimitAssistState.pending
+
+  def _update_non_pcm_long_confirmed_state(self) -> bool:
+    if self.target_set_speed_confirmed:
+      return True
+
+    if self.state != SpeedLimitAssistState.preActive:
+      return False
+
+    req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric)
+
+    return self._get_button_release(req_plus, req_minus)
+
+  def update_state_machine_pcm_op_long(self):
+    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
+    if self.state != SpeedLimitAssistState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = SpeedLimitAssistState.disabled
+
+      else:
+        # ACTIVE
+        if self.state == SpeedLimitAssistState.active:
+          if self.v_cruise_cluster_changed:
+            self.state = SpeedLimitAssistState.inactive
+          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+          elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.adapting
+
+        # ADAPTING
+        elif self.state == SpeedLimitAssistState.adapting:
+          if self.v_cruise_cluster_changed:
+            self.state = SpeedLimitAssistState.inactive
+          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+          elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.active
+
+        # PENDING
+        elif self.state == SpeedLimitAssistState.pending:
+          if self._has_speed_limit:
+            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.target_set_speed_confirmed:
+            self._update_confirmed_state()
+          elif self.pre_active_timer <= 0:
+            # Timeout - session ended
+            self.state = SpeedLimitAssistState.inactive
+
+        # INACTIVE
+        elif self.state == SpeedLimitAssistState.inactive:
+          pass
+
+    # DISABLED
+    elif self.state == SpeedLimitAssistState.disabled:
+      if self.long_enabled and self.enabled:
+        # start or reset preActive timer if initially enabled or manual set speed change detected
+        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
+          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
+
+        elif self.long_engaged_timer <= 0:
+          if self.target_set_speed_confirmed:
+            self._update_confirmed_state()
+          else:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def update_state_machine_non_pcm_long(self):
+    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
+    if self.state != SpeedLimitAssistState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = SpeedLimitAssistState.disabled
+
+      else:
+        # ACTIVE
+        if self.state == SpeedLimitAssistState.active:
+          if self.v_cruise_cluster_changed:
+            self.state = SpeedLimitAssistState.inactive
+
+          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+
+        # PRE_ACTIVE
+        elif self.state == SpeedLimitAssistState.preActive:
+          if self._update_non_pcm_long_confirmed_state():
+            self.state = SpeedLimitAssistState.active
+          elif self.pre_active_timer <= 0:
+            # Timeout - session ended
+            self.state = SpeedLimitAssistState.inactive
+
+        # INACTIVE
+        elif self.state == SpeedLimitAssistState.inactive:
+          if self.speed_limit_changed:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+          elif self._update_non_pcm_long_confirmed_state():
+            self.state = SpeedLimitAssistState.active
+
+    # DISABLED
+    elif self.state == SpeedLimitAssistState.disabled:
+      if self.long_enabled and self.enabled:
+        # start or reset preActive timer if initially enabled or manual set speed change detected
+        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
+          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
+
+        elif self.long_engaged_timer <= 0:
+          if self._update_non_pcm_long_confirmed_state():
+            self.state = SpeedLimitAssistState.active
+          elif self._has_speed_limit:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+          else:
+            self.state = SpeedLimitAssistState.inactive
+
+    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:
+      events_sp.add(EventNameSP.speedLimitPreActive)
+
+    if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending:
+      events_sp.add(EventNameSP.speedLimitPending)
+
+    if self.is_active:
+      if self._state_prev not in ACTIVE_STATES:
+        events_sp.add(EventNameSP.speedLimitActive)
+
+      # only notify if we acquire a valid speed limit
+      # do not check has_speed_limit here
+      elif self._speed_limit != self.speed_limit_prev:
+        if self.speed_limit_prev <= 0:
+          events_sp.add(EventNameSP.speedLimitActive)
+        elif self.speed_limit_prev > 0 and self._speed_limit > 0:
+          events_sp.add(EventNameSP.speedLimitChanged)
+
+  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
+             speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
+    self.long_enabled = long_enabled
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+
+    self._has_speed_limit = has_speed_limit
+    self._speed_limit = speed_limit
+    self._speed_limit_final_last = speed_limit_final_last
+    self._distance = distance
+
+    self.update_params()
+    self.update_calculations(v_cruise_cluster)
+
+    self._state_prev = self.state
+    if self.pcm_op_long:
+      self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
+    else:
+      self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
+
+    self.update_events(events_sp)
+
+    # Update change tracking variables
+    self.speed_limit_prev = self._speed_limit
+    self.v_cruise_cluster_prev = self.v_cruise_cluster
+    self.long_enabled_prev = self.long_enabled
+    self.prev_target_set_speed_conv = self.target_set_speed_conv
+    self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
+    self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
+
+    self.output_v_target = self.get_v_target_from_control()
+    self.output_a_target = self.get_a_target_from_control()
+
+    self.frame += 1
@@ -0,0 +1,227 @@
+"""
+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 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.common import Mode
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
+  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
+
+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)
+    self.sla = SpeedLimitAssist(CI.CP)
+    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+    self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1]  # use 80 MPH for now
+    self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
+
+  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("SpeedLimitMode", int(Mode.assist))
+    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._speed_limit = 0.
+    self.sla.speed_limit_prev = 0.
+    self.sla.last_valid_speed_limit_offsetted = 0.
+    self.sla._distance = 0.
+    self.events_sp.clear()
+
+  def initialize_active_state(self, initialize_v_cruise):
+    self.sla.state = SpeedLimitAssistState.active
+    self.sla.v_cruise_cluster = initialize_v_cruise
+    self.sla.v_cruise_cluster_prev = initialize_v_cruise
+    self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv)
+
+  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("SpeedLimitMode", int(Mode.off))
+    for _ in range(int(10. / DT_MDL)):
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, 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'], SPEED_LIMITS['city'], True, 0, 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
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
+                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+    assert self.sla.is_enabled and self.sla.is_active
+    assert self.sla.output_v_target == SPEED_LIMITS['highway']
+
+  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'], SPEED_LIMITS['city'], True, 0, 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'], SPEED_LIMITS['city'], True, 0, 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['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, 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, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, 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, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, 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(self.pcm_long_max_set_speed)
+
+    self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
+                    SPEED_LIMITS['highway'], True, 0, 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_cluster_prev = self.pcm_long_max_set_speed
+    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
+
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
+                    SPEED_LIMITS['highway'], True, 0, 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_cluster_prev = expected_cruise
+
+    different_cruise = SPEED_LIMITS['highway'] + 5
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.inactive
+
+  # TODO-SP: test lower CST cases
+  def test_rapid_speed_limit_changes(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+    speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']]
+
+    for _, speed_limit in enumerate(speed_limits):
+      self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+
+  def test_invalid_speed_limits_handling(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+
+    invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
+
+    for invalid_limit in invalid_limits:
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp)
+      assert isinstance(self.sla.output_v_target, (int, float))
+      assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0
+
+  def test_stale_data_handling(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+    old_speed_limit = SPEED_LIMITS['city']
+
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+    assert self.sla.output_v_target == old_speed_limit
+
+  def test_distance_based_adapting(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
+    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
+
+    distance = 100.0
+    current_speed = SPEED_LIMITS['freeway']
+    target_speed = SPEED_LIMITS['highway']
+
+    self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+    assert self.sla.output_v_target == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
+
+  def test_long_disengaged_to_disabled(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+
+    self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'],
+                    SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.disabled
+    assert self.sla.output_v_target == 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
+
+      initial_state = state
+
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, 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
@@ -1,6 +1,10 @@
+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
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target


 AlertSize = log.SelfdriveState.AlertSize
@@ -14,6 +18,49 @@ 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'].speedLimit.resolver.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.)
+
+
+def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
+  speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
+  speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
+
+  if CP.openpilotLongitudinalControl and CP.pcmCruise:
+    # PCM long
+    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
+    pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
+    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
+    speed_unit = "km/h" if metric else "mph"
+    alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
+  else:
+    # Non PCM long
+    v_cruise_cluster = CS.vCruiseCluster * CV.KPH_TO_MS
+
+    req_plus, req_minus = compare_cluster_target(v_cruise_cluster, speed_limit_final_last, metric)
+    arrow_str = ""
+    if req_plus:
+      arrow_str = "RES/+"
+    elif req_minus:
+      arrow_str = "SET/-"
+
+    alert_2_str = f"Operate the {arrow_str} cruise control button to activate"
+
+  return Alert(
+    "Speed Limit Assist: Activation Required",
+    alert_2_str,
+    AlertStatus.normal, AlertSize.mid,
+    Priority.LOW, VisualAlert.none, AudibleAlert.none, .1)
+
+
 class EventsSP(EventsBase):
  def __init__(self):
    super().__init__()
@@ -148,5 +195,41 @@ 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: speed_limit_pre_active_alert,
+  },
+
+  EventNameSP.speedLimitPending: {
+    ET.WARNING: Alert(
+      "Automatically adjusting to the last speed limit",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+  },
+
+  EventNameSP.e2eChime: {
+    ET.PERMANENT: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 0.1),
+  },
 }