Merge branch 'feature/slc' into ui/slc-ui

common/params_keys.h

@@ -131,12 +131,12 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"CarParamsSPCache", CLEAR_ON_MANAGER_START},
     {"CarParamsSPPersistent", PERSISTENT},
     {"CarPlatformBundle", PERSISTENT},
+    {"DeviceBootMode", PERSISTENT | BACKUP},
     {"EnableGithubRunner", PERSISTENT | BACKUP},
     {"MaxTimeOffroad", PERSISTENT | BACKUP},
     {"Brightness", PERSISTENT | BACKUP},
     {"ModelRunnerTypeCache", CLEAR_ON_ONROAD_TRANSITION},
     {"OffroadMode", CLEAR_ON_MANAGER_START},
-    {"OffroadMode_Status", CLEAR_ON_MANAGER_START},
     {"QuietMode", PERSISTENT | BACKUP},
 
     // MADS params

selfdrive/selfdrived/alerts_offroad.json

@@ -45,10 +45,6 @@
     "text": "sunnypilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
     "severity": 0
   },
-  "OffroadMode_Status": {
-    "text": "sunnypilot is now in Always Offroad mode. sunnypilot won't start until Always Offroad mode is disabled. Go to \"Settings\" -> \"Device\" to exit Always Offroad mode.",
-    "severity": 1
-  },
   "Offroad_OSMUpdateRequired": {
     "text": "OpenStreetMap database is out of date. New maps must be downloaded if you wish to continue using OpenStreetMap data for Enhanced Speed Control and road name display.\n\n%1",
     "severity": 0

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -67,6 +67,9 @@ class Plant:
     lp = messaging.new_message('liveParameters')
     car_control = messaging.new_message('carControl')
     model = messaging.new_message('modelV2')
+    car_state_sp = messaging.new_message('carStateSP')
+    live_map_data_sp = messaging.new_message('liveMapDataSP')
+    gps_data = messaging.new_message('gpsLocation')
     a_lead = (v_lead - self.v_lead_prev)/self.ts
     self.v_lead_prev = v_lead
 
@@ -133,7 +136,10 @@ class Plant:
           'controlsState': control.controlsState,
           'selfdriveState': ss.selfdriveState,
           'liveParameters': lp.liveParameters,
-          'modelV2': model.modelV2}
+          'modelV2': model.modelV2,
+          'carStateSP': car_state_sp.carStateSP,
+          'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+          'gpsLocation': gps_data.gpsLocation}
     self.planner.update(sm)
     self.acceleration = self.planner.output_a_target
     self.speed = self.speed + self.acceleration * self.ts

selfdrive/ui/qt/offroad/offroad_home.cc

@@ -18,7 +18,7 @@ OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) {
   main_layout->setContentsMargins(40, 40, 40, 40);
 
   // top header
-  QHBoxLayout* header_layout = new QHBoxLayout();
+  header_layout = new QHBoxLayout();
   header_layout->setContentsMargins(0, 0, 0, 0);
   header_layout->setSpacing(16);
 

selfdrive/ui/qt/offroad/offroad_home.h

@@ -39,11 +39,13 @@ public:
 
 protected:
   QHBoxLayout *home_layout;
+  QHBoxLayout *header_layout;
+
+  void showEvent(QShowEvent *event) override;
+  void refresh();
 
 private:
-  void showEvent(QShowEvent *event) override;
   void hideEvent(QHideEvent *event) override;
-  void refresh();
 
   Params params;
 

selfdrive/ui/sunnypilot/SConscript

@@ -19,6 +19,7 @@ qt_src = [
   "sunnypilot/qt/sidebar.cc",
   "sunnypilot/qt/window.cc",
   "sunnypilot/qt/home.cc",
+  "sunnypilot/qt/offroad/exit_offroad_button.cc",
   "sunnypilot/qt/offroad/offroad_home.cc",
   "sunnypilot/qt/offroad/settings/device_panel.cc",
   "sunnypilot/qt/offroad/settings/lateral_panel.cc",
@@ -87,6 +88,7 @@ brand_settings_qt_src = [
   "sunnypilot/qt/offroad/settings/vehicle/volkswagen_settings.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 + osm_panel_qt_src + longitudinal_panel_qt_src
 sp_qt_util = qt_util

selfdrive/ui/sunnypilot/qt/offroad/exit_offroad_button.cc

@@ -0,0 +1,100 @@
+#include <QDebug>
+#include <QHBoxLayout>
+#include <QPainter>
+#include <QPainterPath>
+#include <QStyle>
+
+#include "selfdrive/ui/ui.h"
+#include "selfdrive/ui/qt/widgets/input.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/exit_offroad_button.h"
+
+
+ExitOffroadButton::ExitOffroadButton(QWidget *parent) : QPushButton(parent), glowTimer(new QTimer(this)) {
+  setMouseTracking(true);
+
+  connect(glowTimer, &QTimer::timeout, this, [this]() {
+    // Pulse alpha up and down
+    glowAlpha += glowDelta;
+    if (glowAlpha > 220 || glowAlpha < 10) {
+      glowDelta *= -1;
+    }
+    update(); // trigger repaint
+  });
+
+  glowTimer->start(45);
+  
+  pixmap = QPixmap("../../sunnypilot/selfdrive/assets/offroad/icon_exit_offroad.png").scaledToWidth(img_width, Qt::SmoothTransformation);
+
+  // go to toggles and expand experimental mode description
+  connect(this, &QPushButton::clicked, [=]() {
+    if (ConfirmationDialog::confirm(tr("Are you sure you want to exit Always Offroad mode?"), tr("Confirm"), this)) {
+      params.remove("OffroadMode");
+    }
+  });
+
+  setFixedHeight(125);
+  QHBoxLayout *main_layout = new QHBoxLayout;
+  main_layout->setContentsMargins(horizontal_padding, 0, horizontal_padding, 0);
+
+  mode_label = new QLabel(tr("EXIT ALWAYS OFFROAD MODE"));
+  mode_icon = new QLabel;
+  mode_icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
+  mode_icon->setPixmap(pixmap);
+
+  main_layout->addWidget(mode_label, 1, Qt::AlignLeft);
+  main_layout->addWidget(mode_icon, 0, Qt::AlignRight);
+
+  setLayout(main_layout);
+
+  setStyleSheet(R"(
+    QPushButton {
+      border: none;
+    }
+
+    QLabel {
+      font-size: 45px;
+      font-weight: 300;
+      text-align: left;
+      font-family: JetBrainsMono;
+      color: #000000;
+    }
+  )");
+}
+
+void drawPulsingGlowOverlay(QPainter &p, QPainterPath path, int glowAlpha) {
+  // Draw pulsing glow effect clipped to button area
+  p.save();
+  p.setClipPath(path);
+  p.setCompositionMode(QPainter::CompositionMode_HardLight);
+
+  const QColor animatedGlowColor(255, 255, 255, std::min(255, glowAlpha));
+  QPen glowPen(animatedGlowColor, 8);
+  glowPen.setJoinStyle(Qt::RoundJoin);
+  p.setPen(glowPen);
+  p.drawPath(path);
+  
+  p.restore();
+}
+
+void ExitOffroadButton::paintEvent(QPaintEvent *event) {
+  QPainter p(this);
+  p.setRenderHint(QPainter::Antialiasing);
+
+  QPainterPath path;
+  path.addRoundedRect(rect(), 10, 10);
+
+  // gradient
+  bool pressed = isDown();
+  QLinearGradient gradient(rect().left(), 0, rect().right(), 0);
+  gradient.setColorAt(0, QColor(35, 149, 255, pressed ? 0xcc : 0xff));
+  gradient.setColorAt(0.3, QColor(35, 149, 255, pressed ? 0xcc : 0xff));
+  gradient.setColorAt(1, QColor(20, 255, 171, pressed ? 0xcc : 0xff));
+  p.fillPath(path, gradient);
+
+  drawPulsingGlowOverlay(p, path, glowAlpha);
+
+  // vertical line
+  p.setPen(QPen(QColor(0, 0, 0, 0x4d), 3, Qt::SolidLine));
+  int line_x = rect().right() - img_width - (2 * horizontal_padding);
+  p.drawLine(line_x, rect().bottom(), line_x, rect().top());
+}

selfdrive/ui/sunnypilot/qt/offroad/exit_offroad_button.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <QLabel>
+#include <QPushButton>
+
+#include "common/params.h"
+
+class ExitOffroadButton : public QPushButton {
+  Q_OBJECT
+
+private:
+  QTimer *glowTimer;
+  int glowAlpha = 100;  // Current alpha of glow
+  int glowDelta = 10;    // Change per tick
+  
+public:
+  explicit ExitOffroadButton(QWidget* parent = 0);
+
+  Params params;
+  bool offroad_mode;
+  int img_width = 100;
+  int horizontal_padding = 30;
+  QPixmap pixmap;
+  QLabel *mode_label;
+  QLabel *mode_icon;
+
+protected:
+  void paintEvent(QPaintEvent *event) override;
+};

selfdrive/ui/sunnypilot/qt/offroad/offroad_home.cc

@@ -5,16 +5,45 @@
  * See the LICENSE.md file in the root directory for more details.
  */
 
-#include "selfdrive/ui/sunnypilot/qt/offroad/offroad_home.h"
-
 #include <QStackedWidget>
 
+#include "selfdrive/ui/sunnypilot/qt/offroad/offroad_home.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/drive_stats.h"
 
 OffroadHomeSP::OffroadHomeSP(QWidget *parent) : OffroadHome(parent) {
-  QStackedWidget *left_widget = new QStackedWidget(this);
-  left_widget->addWidget(new DriveStats(this));
+  QFrame *left_widget = new QFrame(this);
+  QVBoxLayout *left_layout = new QVBoxLayout(left_widget);
+  left_layout->setContentsMargins(0, 0, 0, 0);
+  left_layout->setSpacing(30);
+
+  btn_exit_offroad = new ExitOffroadButton(this);
+  QObject::connect(btn_exit_offroad, &ExitOffroadButton::clicked, [=]() {
+    refreshOffroadStatus();
+  });
+  left_layout->addWidget(btn_exit_offroad);
+
+  left_layout->addWidget(new DriveStats(this));
   left_widget->setStyleSheet("border-radius: 10px;");
 
   home_layout->insertWidget(0, left_widget);
+
+  offroad_notif = new QPushButton(tr("ALWAYS OFFROAD ACTIVE"));
+  offroad_notif->setVisible(false);
+  offroad_notif->setStyleSheet("background-color: #E22C2C;");
+  header_layout->insertWidget(0, offroad_notif, 0, Qt::AlignHCenter | Qt::AlignLeft);
+
+  QObject::connect(deviceSP(), &DeviceSP::displayPowerChanged, this, &OffroadHomeSP::refreshOffroadStatus);
+
+}
+
+void OffroadHomeSP::showEvent(QShowEvent *event) {
+  refreshOffroadStatus();
+  OffroadHome::showEvent(event);
+}
+
+void OffroadHomeSP::refreshOffroadStatus() {
+  bool is_offroad = params.getBool("OffroadMode");
+  btn_exit_offroad->setVisible(is_offroad);
+  offroad_notif->setVisible(is_offroad);
+  OffroadHome::refresh();
 }

selfdrive/ui/sunnypilot/qt/offroad/offroad_home.h

@@ -8,10 +8,19 @@
 #pragma once
 
 #include "selfdrive/ui/qt/offroad/offroad_home.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/exit_offroad_button.h"
 
 class OffroadHomeSP : public OffroadHome {
   Q_OBJECT
 
 public:
   explicit OffroadHomeSP(QWidget *parent = 0);
+
+private:
+  ExitOffroadButton *btn_exit_offroad;
+  QPushButton *offroad_notif;
+  Params params;
+
+  void showEvent(QShowEvent *event) override;
+  void refreshOffroadStatus();
 };

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

@@ -80,6 +80,14 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) {
   connect(maxTimeOffroad, &OptionControlSP::updateLabels, maxTimeOffroad, &MaxTimeOffroad::refresh);
   addItem(maxTimeOffroad);
 
+    toggleDeviceBootMode = new ButtonParamControlSP("DeviceBootMode", tr("Wake-Up Behavior"), "", "", {"Default", "Offroad"}, 375, true);
+  addItem(toggleDeviceBootMode);
+
+  connect(toggleDeviceBootMode, &ButtonParamControlSP::buttonClicked, this, [=](int index) {
+    params.put("DeviceBootMode", QString::number(index).toStdString());
+    updateState();
+  });
+  
   // Brightness
   brightness = new Brightness();
   connect(brightness, &OptionControlSP::updateLabels, brightness, &Brightness::refresh);
@@ -184,4 +192,10 @@ void DevicePanelSP::updateState() {
   bool offroad_mode_param = params.getBool("OffroadMode");
   offroadBtn->setText(offroad_mode_param ? tr("Exit Always Offroad") : tr("Always Offroad"));
   offroadBtn->setStyleSheet(offroad_mode_param ? alwaysOffroadStyle : autoOffroadStyle);
+
+  DeviceSleepModeStatus currStatus = DeviceSleepModeStatus::DEFAULT;
+  if (params.get("DeviceBootMode") == "1") {
+    currStatus = DeviceSleepModeStatus::OFFROAD;
+  }
+  toggleDeviceBootMode->setDescription(deviceSleepModeDescription(currStatus));
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h

@@ -12,6 +12,11 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
 
+enum class DeviceSleepModeStatus {
+  DEFAULT,
+  OFFROAD,
+};
+
 class DevicePanelSP : public DevicePanel {
   Q_OBJECT
 
@@ -26,6 +31,7 @@ private:
   std::map<QString, PushButtonSP*> buttons;
   PushButtonSP *offroadBtn;
   MaxTimeOffroad *maxTimeOffroad;
+  ButtonParamControlSP *toggleDeviceBootMode;
   Brightness *brightness;
 
   const QString alwaysOffroadStyle = R"(
@@ -87,4 +93,20 @@ private:
       background-color: #FF2424;
     }
   )";
+
+  static QString deviceSleepModeDescription(DeviceSleepModeStatus status = DeviceSleepModeStatus::DEFAULT) {
+    QString def_str = tr("⁍ Default: Device will boot/wake-up normally & will be ready to engage.");
+    QString offrd_str = tr("⁍ Offroad: Device will be in Always Offroad mode after boot/wake-up.");
+
+    if (status == DeviceSleepModeStatus::DEFAULT) {
+      def_str = "<font color='white'><b>" + def_str + "</b></font>";
+    } else if (status == DeviceSleepModeStatus::OFFROAD) {
+      offrd_str = "<font color='white'><b>" + offrd_str + "</b></font>";
+    }
+
+    return QString("%1<br><br>%2<br>%3")
+             .arg(tr("Controls state of the device after boot/sleep."))
+             .arg(def_str)
+             .arg(offrd_str);
+  }
 };

selfdrive/ui/sunnypilot/qt/widgets/controls.h

@@ -247,14 +247,11 @@ public:
     if (inline_layout) {
       button_param_layout->setMargin(0);
       button_param_layout->setSpacing(0);
+      spacingItem = nullptr;
       if (!title.isEmpty()) {
         main_layout->removeWidget(title_label);
         hlayout->addWidget(title_label, 1);
       }
-      if (spacingItem != nullptr && main_layout->indexOf(spacingItem) != -1) {
-        main_layout->removeItem(spacingItem);
-        spacingItem = nullptr;
-      }
     }
 
     button_group = new QButtonGroup(this);

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

@@ -34,7 +34,7 @@ DriveStats::DriveStats(QWidget* parent) : QFrame(parent) {
 
     int row = 0;
     grid_layout->addWidget(newLabel(title, "title"), row++, 0, 1, 3);
-    grid_layout->addItem(new QSpacerItem(0, 50), row++, 0, 1, 1);
+    grid_layout->addItem(new QSpacerItem(0, 30), row++, 0, 1, 1);
 
     grid_layout->addWidget(labels.routes = newLabel("0", "number"), row, 0, Qt::AlignLeft);
     grid_layout->addWidget(labels.distance = newLabel("0", "number"), row, 1, Qt::AlignLeft);

selfdrive/ui/sunnypilot/ui.cc

@@ -41,6 +41,7 @@ void UIStateSP::update() {
 
 DeviceSP::DeviceSP(QObject *parent) : Device(parent) {
   QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &DeviceSP::update);
+  QObject::connect(this, &Device::displayPowerChanged, this, &DeviceSP::handleDisplayPowerChanged);
 }
 
 UIStateSP *uiStateSP() {
@@ -62,3 +63,10 @@ DeviceSP *deviceSP() {
   static DeviceSP _device;
   return &_device;
 }
+
+void DeviceSP::handleDisplayPowerChanged(bool on) {
+  // if enabled, trigger offroad mode when device goes to sleep
+  if (params.get("DeviceBootMode") == "1" && not on) {
+    params.putBool("OffroadMode", true);
+  }
+}

selfdrive/ui/sunnypilot/ui.h

@@ -84,6 +84,10 @@ class DeviceSP : public Device {
 
 public:
   DeviceSP(QObject *parent = 0);
+
+private:
+  Params params;
+  void handleDisplayPowerChanged(bool on);
 };
 
 DeviceSP *deviceSP();

sunnypilot/selfdrive/assets/offroad/icon_exit_offroad.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d44f71f7603b106986fff7835f35c4c18c4c619a29e6292f8626ae3eedd85e57
+size 7811

sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py

@@ -332,7 +332,7 @@ class SpeedLimitController:
     self._op_engaged = enabled and self._CP.openpilotLongitudinalControl
     self._v_ego = v_ego
     self._a_ego = a_ego
-    self._v_cruise_setpoint = v_cruise_setpoint
+    self._v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
     self._gas_pressed = _car_state.gasPressed
     self._current_time = time.monotonic()
 

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

@@ -1,7 +1,7 @@
 import time
 import numpy as np
 
-from cereal import messaging, custom
+from cereal import messaging
 from openpilot.common.gps import get_gps_location_service
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
@@ -51,16 +51,13 @@ class SpeedLimitResolver:
     self._get_from_map_data(sm)
 
   def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
-    if sm.updated['carStateSP']:
-      self._reset_limit_sources(Source.car_state)
-      self._limit_solutions[Source.car_state] = sm['carStateSP'].speedLimit
-      self._distance_solutions[Source.car_state] = 0.
+    self._reset_limit_sources(Source.car_state)
+    self._limit_solutions[Source.car_state] = sm['carStateSP'].speedLimit
+    self._distance_solutions[Source.car_state] = 0.
 
   def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
-    # Load limits from map_data
-    if sm.updated['liveMapDataSP']:
-      self._reset_limit_sources(Source.map_data)
-      self._process_map_data(sm)
+    self._reset_limit_sources(Source.map_data)
+    self._process_map_data(sm)
 
   def _process_map_data(self, sm: messaging.SubMaster) -> None:
     gps_data = sm[self._gps_location_service]

system/hardware/hardwared.py

@@ -323,7 +323,6 @@ def hardware_thread(end_event, hw_queue) -> None:
     offroad_mode = params.get_bool("OffroadMode")
     startup_conditions["not_always_offroad"] = not offroad_mode
     onroad_conditions["not_always_offroad"] = not offroad_mode
-    set_offroad_alert("OffroadMode_Status", offroad_mode)
 
     # if the temperature enters the danger zone, go offroad to cool down
     onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger

system/manager/manager.py

@@ -49,6 +49,7 @@ def manager_init() -> None:
     ("BlindSpot", "0"),
     ("BlinkerMinLateralControlSpeed", "20"),  # MPH or km/h
     ("BlinkerPauseLateralControl", "0"),
+    ("DeviceBootMode", "0"),
     ("DynamicExperimentalControl", "0"),
     ("HyundaiLongitudinalTuning", "0"),
     ("LagdToggle", "1"),
@@ -74,6 +75,10 @@ def manager_init() -> None:
     ("SpeedLimitWarningValueOffset", "0"),
   ]
 
+  # device boot mode
+  if params.get("DeviceBootMode") == b"1": # start in always offroad mode
+    params.put_bool("OffroadMode", True)
+
   if params.get_bool("RecordFrontLock"):
     params.put_bool("RecordFront", True)