Revert "Refine lateral control limits and simplify safety model handling"

This reverts commit e2ec8a7b13.

.codespellignore

@@ -2,4 +2,5 @@ Wen
 REGIST
 PullRequest
 cancelled
+indeces
 FOF

.idea/tools/External Tools.xml

@@ -2,7 +2,7 @@
   <tool name="uv Scons Build Debug" showInMainMenu="false" showInEditor="false" showInProject="false" showInSearchPopup="false" disabled="false" useConsole="true" showConsoleOnStdOut="false" showConsoleOnStdErr="false" synchronizeAfterRun="true">
     <exec>
       <option name="COMMAND" value="bash" />
-      <option name="PARAMETERS" value="-c &quot;source .venv/bin/activate &amp;&amp; scons -u -j$(nproc) --compile_db --ccflags=\&quot;-fno-inline\&quot;&quot;" />
+      <option name="PARAMETERS" value="-c &quot;source .venv/bin/activate &amp;&amp; scons -u -j$(nproc) --ccflags=\&quot;-fno-inline\&quot;&quot;" />
       <option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
     </exec>
   </tool>
@@ -16,7 +16,7 @@
   <tool name="uv Scons Build Release" showInMainMenu="false" showInEditor="false" showInProject="false" showInSearchPopup="false" disabled="false" useConsole="true" showConsoleOnStdOut="false" showConsoleOnStdErr="false" synchronizeAfterRun="true">
     <exec>
       <option name="COMMAND" value="bash" />
-      <option name="PARAMETERS" value="-c &quot;source .venv/bin/activate &amp;&amp; scons -u -j$(nproc) --compile_db&quot; " />
+      <option name="PARAMETERS" value="-c &quot;source .venv/bin/activate &amp;&amp; scons -u -j$(nproc)&quot; " />
       <option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
     </exec>
   </tool>

common/params_keys.h

@@ -5,8 +5,8 @@
 
 inline static std::unordered_map<std::string, uint32_t> keys = {
     {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
-    {"AdbEnabled", PERSISTENT},
+    {"AdbEnabled", PERSISTENT | BACKUP},
-    {"AlwaysOnDM", PERSISTENT},
+    {"AlwaysOnDM", PERSISTENT | BACKUP},
     {"ApiCache_Device", PERSISTENT},
     {"ApiCache_FirehoseStats", PERSISTENT},
     {"AssistNowToken", PERSISTENT},
@@ -78,7 +78,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"LocationFilterInitialState", PERSISTENT},
     {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"LongitudinalPersonality", PERSISTENT | BACKUP},
-    {"NetworkMetered", PERSISTENT},
+    {"NetworkMetered", PERSISTENT | BACKUP},
     {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"Offroad_BadNvme", CLEAR_ON_MANAGER_START},
@@ -124,25 +124,29 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
 
     // --- sunnypilot params --- //
     {"ApiCache_DriveStats", PERSISTENT},
-    {"AutoLaneChangeBsmDelay", PERSISTENT},
+    {"AutoLaneChangeBsmDelay", PERSISTENT | BACKUP},
-    {"AutoLaneChangeTimer", PERSISTENT},
+    {"AutoLaneChangeTimer", PERSISTENT | BACKUP},
     {"BlinkerMinLateralControlSpeed", PERSISTENT | BACKUP},
     {"BlinkerPauseLateralControl", PERSISTENT | BACKUP},
+    {"Brightness", PERSISTENT | BACKUP},
     {"CarParamsSP", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CarParamsSPCache", CLEAR_ON_MANAGER_START},
     {"CarParamsSPPersistent", PERSISTENT},
-    {"CarPlatformBundle", PERSISTENT},
+    {"CarPlatformBundle", PERSISTENT | BACKUP},
+    {"ChevronInfo", PERSISTENT | BACKUP},
     {"CustomAccIncrementsEnabled", PERSISTENT | BACKUP},
     {"CustomAccLongPressIncrement", PERSISTENT | BACKUP},
     {"CustomAccShortPressIncrement", PERSISTENT | BACKUP},
     {"DeviceBootMode", PERSISTENT | BACKUP},
     {"EnableGithubRunner", PERSISTENT | BACKUP},
     {"InteractivityTimeout", PERSISTENT | BACKUP},
+    {"IsDevelopmentBranch", CLEAR_ON_MANAGER_START},
     {"MaxTimeOffroad", PERSISTENT | BACKUP},
-    {"Brightness", PERSISTENT | BACKUP},
     {"ModelRunnerTypeCache", CLEAR_ON_ONROAD_TRANSITION},
     {"OffroadMode", CLEAR_ON_MANAGER_START},
+    {"QuickBootToggle", PERSISTENT | BACKUP},
     {"QuietMode", PERSISTENT | BACKUP},
+    {"ShowAdvancedControls", PERSISTENT | BACKUP},
 
     // MADS params
     {"Mads", PERSISTENT | BACKUP},
@@ -152,6 +156,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
 
     // Model Manager params
     {"ModelManager_ActiveBundle", PERSISTENT},
+    {"ModelManager_ClearCache", CLEAR_ON_MANAGER_START},
     {"ModelManager_DownloadIndex", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"ModelManager_LastSyncTime", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"ModelManager_ModelsCache", PERSISTENT | BACKUP},
@@ -173,9 +178,9 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"BackupManager_RestoreVersion", PERSISTENT},
 
     // sunnypilot car specific params
-    {"HyundaiLongitudinalTuning", PERSISTENT},
+    {"HyundaiLongitudinalTuning", PERSISTENT | BACKUP},
 
-    {"DynamicExperimentalControl", PERSISTENT},
+    {"DynamicExperimentalControl", PERSISTENT | BACKUP},
     {"BlindSpot", PERSISTENT | BACKUP},
 
     // model panel params
@@ -202,4 +207,13 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"OsmStateTitle", PERSISTENT},
     {"OsmWayTest", PERSISTENT},
     {"RoadName", CLEAR_ON_ONROAD_TRANSITION},
+
+    // Tuning keys
+    {"EnableHkgTuningAngleSmoothingFactor", PERSISTENT | BACKUP},
+    {"HkgTuningAngleMinTorqueReductionGain", PERSISTENT | BACKUP},
+    {"HkgTuningAngleMaxTorqueReductionGain", PERSISTENT | BACKUP},
+    {"HkgTuningAngleActiveTorqueReductionGain", PERSISTENT | BACKUP},
+    {"HkgTuningOverridingCycles", PERSISTENT | BACKUP},
+    {"HkgAngleLiveTuning", CLEAR_ON_MANAGER_START}
+    
 };

pyproject.toml

@@ -59,7 +59,7 @@ dependencies = [
   "future-fstrings",
 
   # joystickd
-  "inputs",
+  "pygame",
 
   # these should be removed
   "psutil",
@@ -108,7 +108,6 @@ dev = [
   "opencv-python-headless",
   "parameterized >=0.8, <0.9",
   "pyautogui",
-  "pygame",
   "pyopencl; platform_machine != 'aarch64'", # broken on arm64
   "pytools < 2024.1.11; platform_machine != 'aarch64'", # pyopencl use a broken version
   "pywinctl",

scripts/lint/lint.sh

@@ -13,7 +13,7 @@ cd $ROOT
 
 FAILED=0
 
-IGNORED_FILES="uv\.lock|docs\/CARS.md|LICENSE\.md"
+IGNORED_FILES="uv\.lock|docs\/CARS.md|LICENSE\.md|layouts\/.*\.xml"
 IGNORED_DIRS="^third_party.*|^msgq.*|^msgq_repo.*|^opendbc.*|^opendbc_repo.*|^cereal.*|^panda.*|^rednose.*|^rednose_repo.*|^tinygrad.*|^tinygrad_repo.*|^teleoprtc.*|^teleoprtc_repo.*"
 
 function run() {

selfdrive/controls/controlsd.py

@@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+from openpilot.selfdrive.controls.lib.latcontrol_angle_torque import LatControlAngleTorque
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
@@ -58,7 +59,9 @@ class Controls(ControlsExt):
     self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
-    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle and self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlAngleTorque(self.CP, self.CP_SP, self.CI)
+    elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
     elif self.CP.lateralTuning.which() == 'pid':
       self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)

selfdrive/controls/lib/latcontrol_angle_torque.py

@@ -0,0 +1,13 @@
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+
+
+class LatControlAngleTorque(LatControlTorque, LatControlAngle):
+  def __init__(self, CP, CP_SP, CI):
+    LatControlTorque.__init__(self, CP, CP_SP, CI)
+    LatControlAngle.__init__(self, CP, CP_SP, CI)
+
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+    torque, _, _ = LatControlTorque.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
+    _, angle, angle_log = LatControlAngle.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
+    return torque, angle, angle_log

selfdrive/modeld/modeld.py

@@ -90,7 +90,7 @@ class ModelState:
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
 
   def __init__(self, context: CLContext):
-    self.LAT_SMOOTH_SECONDS = 0.0
+    self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS
     with open(VISION_METADATA_PATH, 'rb') as f:
       vision_metadata = pickle.load(f)
       self.vision_input_shapes =  vision_metadata['input_shapes']

selfdrive/ui/qt/offroad/developer_panel.cc

@@ -45,17 +45,6 @@ DeveloperPanel::DeveloperPanel(SettingsWindow *parent) : ListWidget(parent) {
   });
   addItem(experimentalLongitudinalToggle);
 
-  enableGithubRunner = new ParamControl("EnableGithubRunner", tr("Enable GitHub runner service"), tr("Enables or disables the github runner service."), "");
-  addItem(enableGithubRunner);
-
-  // error log button
-  errorLogBtn = new ButtonControl(tr("Error Log"), tr("VIEW"), tr("View the error log for sunnypilot crashes."));
-  connect(errorLogBtn, &ButtonControl::clicked, [=]() {
-    std::string txt = util::read_file("/data/community/crashes/error.log");
-    ConfirmationDialog::rich(QString::fromStdString(txt), this);
-  });
-  addItem(errorLogBtn);
-
   // Joystick and longitudinal maneuvers should be hidden on release branches
   is_release = params.getBool("IsReleaseBranch");
 
@@ -104,8 +93,6 @@ void DeveloperPanel::updateToggles(bool _offroad) {
   experimentalLongitudinalToggle->refresh();
 
   // Handle specific controls visibility for release branches
-  enableGithubRunner->setVisible(!is_release);
-  errorLogBtn->setVisible(!is_release);
   joystickToggle->setVisible(!is_release);
 
   offroad = _offroad;

selfdrive/ui/qt/offroad/developer_panel.h

@@ -16,10 +16,8 @@ private:
   Params params;
   ParamControl* adbToggle;
   ParamControl* joystickToggle;
-  ButtonControl* errorLogBtn;
   ParamControl* longManeuverToggle;
   ParamControl* experimentalLongitudinalToggle;
-  ParamControl* enableGithubRunner;
   bool is_release;
   bool offroad = false;
 

selfdrive/ui/qt/onroad/model.cc

@@ -34,6 +34,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
       drawLead(painter, lead_two, lead_vertices[1], surface_rect);
     }
   }
+  drawLeadStatus(painter, surface_rect.height(), surface_rect.width());
 
   painter.restore();
 }
@@ -173,6 +174,173 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float
       (1 - t) * start.alphaF() + t * end.alphaF());
 }
 
+
+void ModelRenderer::drawLeadStatus(QPainter &painter, int height, int width) {
+    auto *s = uiState();
+    auto &sm = *(s->sm);
+
+    if (!sm.alive("radarState")) return;
+
+    const auto &radar_state = sm["radarState"].getRadarState();
+    const auto &lead_one = radar_state.getLeadOne();
+    const auto &lead_two = radar_state.getLeadTwo();
+
+    // Check if we have any active leads
+    bool has_lead_one = lead_one.getStatus();
+    bool has_lead_two = lead_two.getStatus();
+
+    if (!has_lead_one && !has_lead_two) {
+        // Fade out status display
+        lead_status_alpha = std::max(0.0f, lead_status_alpha - 0.05f);
+        if (lead_status_alpha <= 0.0f) return;
+    } else {
+        // Fade in status display
+        lead_status_alpha = std::min(1.0f, lead_status_alpha + 0.1f);
+    }
+
+    // Draw status for each lead vehicle under its chevron
+    if (true) {
+        drawLeadStatusAtPosition(painter, lead_one, lead_vertices[0], height, width, "L1");
+    }
+
+    if (has_lead_two && std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0) {
+        drawLeadStatusAtPosition(painter, lead_two, lead_vertices[1], height, width, "L2");
+    }
+}
+
+void ModelRenderer::drawLeadStatusAtPosition(QPainter &painter,
+                                           const cereal::RadarState::LeadData::Reader &lead_data,
+                                           const QPointF &chevron_pos,
+                                           int height, int width,
+                                           const QString &label) {
+
+    float d_rel = lead_data.getDRel();
+    float v_rel = lead_data.getVRel();
+    auto *s = uiState();
+    auto &sm = *(s->sm);
+    float v_ego = sm["carState"].getCarState().getVEgo();
+
+    int chevron_data = std::atoi(Params().get("ChevronInfo").c_str());
+
+    // Calculate chevron size (same logic as drawLead)
+    float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35;
+
+    QFont content_font = painter.font();
+    content_font.setPixelSize(35);
+    content_font.setBold(true);
+    painter.setFont(content_font);
+
+    QFontMetrics fm(content_font);
+    bool is_metric = s->scene.is_metric;
+
+    QStringList text_lines;
+
+    const int chevron_types = 3;
+    const int chevron_all = chevron_types + 1;  // All metrics (value 4)
+    QStringList chevron_text[chevron_types];
+    int position;
+    float val;
+
+    // Distance display (chevron_data == 1 or all)
+    if (chevron_data == 1 || chevron_data == chevron_all) {
+        position = 0;
+        val = std::max(0.0f, d_rel);
+        QString distance_unit = is_metric ? "m" : "ft";
+        if (!is_metric) {
+            val *= 3.28084f; // Convert meters to feet
+        }
+        chevron_text[position].append(QString::number(val, 'f', 0) + " " + distance_unit);
+    }
+
+    // Absolute velocity display (chevron_data == 2 or all)
+    if (chevron_data == 2 || chevron_data == chevron_all) {
+        position = (chevron_data == 2) ? 0 : 1;
+        val = std::max(0.0f, (v_rel + v_ego) * (is_metric ? static_cast<float>(MS_TO_KPH) : static_cast<float>(MS_TO_MPH)));
+        chevron_text[position].append(QString::number(val, 'f', 0) + " " + (is_metric ? "km/h" : "mph"));
+    }
+
+    // Time-to-contact display (chevron_data == 3 or all)
+    if (chevron_data == 3 || chevron_data == chevron_all) {
+        position = (chevron_data == 3) ? 0 : 2;
+        val = (d_rel > 0 && v_ego > 0) ? std::max(0.0f, d_rel / v_ego) : 0.0f;
+        QString ttc_str = (val > 0 && val < 200) ? QString::number(val, 'f', 1) + "s" : "---";
+        chevron_text[position].append(ttc_str);
+    }
+
+    // Collect all non-empty text lines
+    for (int i = 0; i < chevron_types; ++i) {
+        if (!chevron_text[i].isEmpty()) {
+            text_lines.append(chevron_text[i]);
+        }
+    }
+
+    // If no text to display, return early
+    if (text_lines.isEmpty()) {
+        return;
+    }
+
+    // Text box dimensions
+    float str_w = 150;  // Width of text area
+    float str_h = 45;   // Height per line
+
+    // Position text below chevron, centered horizontally
+    float text_x = chevron_pos.x() - str_w / 2;
+    float text_y = chevron_pos.y() + sz + 15;
+
+    // Clamp to screen bounds
+    text_x = std::clamp(text_x, 10.0f, (float)width - str_w - 10);
+
+    // Shadow offset
+    QPoint shadow_offset(2, 2);
+
+    // Draw each line of text with shadow
+    for (int i = 0; i < text_lines.size(); ++i) {
+        if (!text_lines[i].isEmpty()) {
+            QRect textRect(text_x, text_y + (i * str_h), str_w, str_h);
+
+            // Draw shadow
+            painter.setPen(QColor(0x0, 0x0, 0x0, (int)(200 * lead_status_alpha)));
+            painter.drawText(textRect.translated(shadow_offset.x(), shadow_offset.y()),
+                           Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]);
+
+            // Determine text color based on content and danger level
+            QColor text_color;
+
+            // Check if this is a distance line (contains 'm' or 'ft')
+            if (text_lines[i].contains("m") || text_lines[i].contains("ft")) {
+                if (d_rel < 20.0f) {
+                    text_color = QColor(255, 80, 80, (int)(255 * lead_status_alpha)); // Red - danger
+                } else if (d_rel < 40.0f) {
+                    text_color = QColor(255, 200, 80, (int)(255 * lead_status_alpha)); // Yellow - caution
+                } else {
+                    text_color = QColor(80, 255, 120, (int)(255 * lead_status_alpha)); // Green - safe
+                }
+            }
+            // Enhanced color coding for time-to-contact
+            else if (text_lines[i].contains("s") && !text_lines[i].contains("---")) {
+                float ttc_val = text_lines[i].left(text_lines[i].length() - 1).toFloat();
+                if (ttc_val < 3.0f) {
+                    text_color = QColor(255, 80, 80, (int)(255 * lead_status_alpha)); // Red - urgent
+                } else if (ttc_val < 6.0f) {
+                    text_color = QColor(255, 200, 80, (int)(255 * lead_status_alpha)); // Yellow - caution
+                } else {
+                    text_color = QColor(0xff, 0xff, 0xff, (int)(255 * lead_status_alpha)); // White - safe
+                }
+            }
+            else {
+                text_color = QColor(0xff, 0xff, 0xff, (int)(255 * lead_status_alpha)); // White for other lines
+            }
+
+            // Draw main text
+            painter.setPen(text_color);
+            painter.drawText(textRect, Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]);
+        }
+    }
+
+    // Reset pen
+    painter.setPen(Qt::NoPen);
+}
+
 void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data,
                              const QPointF &vd, const QRect &surface_rect) {
   const float speedBuff = 10.;

selfdrive/ui/qt/onroad/model.h

@@ -34,6 +34,12 @@ protected:
   bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out);
   void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off,
                         QPolygonF *pvd, int max_idx, bool allow_invert = true);
+  void drawLeadStatus(QPainter &painter, int height, int width);
+  void drawLeadStatusAtPosition(QPainter &painter,
+                             const cereal::RadarState::LeadData::Reader &lead_data,
+                             const QPointF &chevron_pos,
+                             int height, int width,
+                             const QString &label);
   void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect);
   void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
   virtual void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead);
@@ -58,4 +64,9 @@ protected:
   QPointF lead_vertices[2] = {};
   Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero();
   QRectF clip_region;
+
+  float lead_status_alpha = 0.0f;
+  QPointF lead_status_pos;
+  QString lead_status_text;
+  QColor lead_status_color;
 };

selfdrive/ui/qt/widgets/input.cc

@@ -2,6 +2,7 @@
 
 #include <QPushButton>
 #include <QButtonGroup>
+#include <QScroller>
 
 #include "system/hardware/hw.h"
 #include "selfdrive/ui/qt/util.h"
@@ -334,3 +335,141 @@ QString MultiOptionDialog::getSelection(const QString &prompt_text, const QStrin
   }
   return "";
 }
+
+TreeOptionDialog::TreeOptionDialog(const QString &prompt_text, const QList<QPair<QString, QStringList>> &items,
+                                   const QString &current, QWidget *parent) : DialogBase(parent) {
+  QFrame *container = new QFrame(this);
+  container->setStyleSheet(R"(
+    QFrame { background-color: #1B1B1B; }
+    #confirm_btn[enabled="false"] { background-color: #2B2B2B; }
+    #confirm_btn:enabled { background-color: #465BEA; }
+    #confirm_btn:enabled:pressed { background-color: #3049F4; }
+    QTreeWidget {
+      background-color: transparent;
+      border: none;
+    }
+    QTreeWidget::item {
+      height: 135;
+      padding: 0px 50px;
+      margin: 5px;
+      text-align: left;
+      font-size: 55px;
+      font-weight: 300;
+      border-radius: 10px;
+      background-color: #4F4F4F;
+      color: white;
+    }
+    QTreeWidget::item:selected {
+      background-color: #465BEA;
+    }
+    QTreeWidget::branch {
+      background-color: transparent;
+    }
+  )");
+
+  QVBoxLayout *main_layout = new QVBoxLayout(container);
+  main_layout->setContentsMargins(55, 50, 55, 50);
+
+  QLabel *title = new QLabel(prompt_text, this);
+  title->setStyleSheet("font-size: 70px; font-weight: 500;");
+  main_layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop);
+  main_layout->addSpacing(25);
+
+  treeWidget = new QTreeWidget(this);
+  treeWidget->setHeaderHidden(true);
+  treeWidget->setIndentation(50);
+  treeWidget->setExpandsOnDoubleClick(false); // Disable double-click expansion
+  treeWidget->setAnimated(true);
+  treeWidget->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
+  treeWidget->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
+  treeWidget->setVerticalScrollMode(QAbstractItemView::ScrollPerPixel);
+  treeWidget->setDragEnabled(false);
+  treeWidget->setMouseTracking(true);
+
+  // Connect single-click to expand/collapse
+  QObject::connect(treeWidget, &QTreeWidget::itemClicked, [=](QTreeWidgetItem *item, int) {
+    if (item->childCount() > 0) {
+      item->setExpanded(!item->isExpanded());
+      treeWidget->scrollToItem(item->child(0), QAbstractItemView::EnsureVisible);
+    }
+  });
+
+  QScroller::grabGesture(treeWidget->viewport(), QScroller::LeftMouseButtonGesture);
+
+  // Populate tree
+  QListIterator<QPair<QString, QStringList>> iter(items);
+  while (iter.hasNext()) {
+    QPair currItem = iter.next();
+    if (currItem.first.isEmpty()) {
+      for (const QString &item : currItem.second) {
+        QTreeWidgetItem *topLevel = new QTreeWidgetItem();
+        topLevel->setText(0, item);
+        topLevel->setFlags(topLevel->flags() | Qt::ItemIsSelectable);
+        treeWidget->addTopLevelItem(topLevel);
+        if (item == current) {
+          topLevel->setSelected(true);
+        }
+      }
+    } else {
+      QTreeWidgetItem *folderItem = new QTreeWidgetItem(treeWidget);
+      folderItem->setIcon(0, QIcon(QPixmap("../assets/icons/menu.png")));
+      folderItem->setText(0, "  " + currItem.first);
+      folderItem->setFlags(folderItem->flags() | Qt::ItemIsAutoTristate);
+      folderItem->setFlags(folderItem->flags() & ~Qt::ItemIsSelectable);
+
+      for (const QString &item : currItem.second)
+      {
+        QTreeWidgetItem *childItem = new QTreeWidgetItem(folderItem);
+        childItem->setText(0, item);
+        childItem->setFlags(childItem->flags() | Qt::ItemIsSelectable);
+
+        if (item == current) {
+          childItem->setSelected(true);
+          folderItem->setExpanded(true);
+        }
+      }
+    }
+  }
+
+  confirm_btn = new QPushButton(tr("Select"));
+  confirm_btn->setObjectName("confirm_btn");
+  confirm_btn->setEnabled(false);
+
+  QObject::connect(treeWidget, &QTreeWidget::itemSelectionChanged, [=]() {
+    QList<QTreeWidgetItem*> selectedItems = treeWidget->selectedItems();
+    if (!selectedItems.isEmpty()) {
+      selection = selectedItems.first()->text(0);
+      confirm_btn->setEnabled(selection != current);
+    }
+  });
+
+  ScrollView *scroll_view = new ScrollView(treeWidget, this);
+  scroll_view->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded);
+
+  main_layout->addWidget(scroll_view);
+  main_layout->addSpacing(35);
+
+  // cancel + confirm buttons
+  QHBoxLayout *blayout = new QHBoxLayout;
+  main_layout->addLayout(blayout);
+  blayout->setSpacing(50);
+
+  QPushButton *cancel_btn = new QPushButton(tr("Cancel"));
+  QObject::connect(cancel_btn, &QPushButton::clicked, this, &ConfirmationDialog::reject);
+  QObject::connect(confirm_btn, &QPushButton::clicked, this, &ConfirmationDialog::accept);
+  blayout->addWidget(cancel_btn);
+  blayout->addWidget(confirm_btn);
+
+  QVBoxLayout *outer_layout = new QVBoxLayout(this);
+  outer_layout->setContentsMargins(50, 50, 50, 50);
+  outer_layout->addWidget(container);
+}
+
+QString TreeOptionDialog::getSelection(const QString &prompt_text, const QList<QPair<QString, QStringList>> &items,
+                                     const QString &current, QWidget *parent) {
+  TreeOptionDialog d(prompt_text, items, current, parent);
+  if (d.exec()) {
+    return d.selection;
+  }
+  return "";
+}

selfdrive/ui/qt/widgets/input.h

@@ -6,6 +6,7 @@
 #include <QString>
 #include <QVBoxLayout>
 #include <QWidget>
+#include <QTreeWidget>
 
 #include "selfdrive/ui/qt/widgets/keyboard.h"
 
@@ -69,3 +70,16 @@ public:
   static QString getSelection(const QString &prompt_text, const QStringList &l, const QString &current, QWidget *parent);
   QString selection;
 };
+
+class TreeOptionDialog : public DialogBase {
+  Q_OBJECT
+
+public:
+  explicit TreeOptionDialog(const QString &prompt_text, const QList<QPair<QString, QStringList>> &items, const QString &current, QWidget *parent = nullptr);
+  static QString getSelection(const QString &prompt_text, const QList<QPair<QString, QStringList>> &items, const QString &current, QWidget *parent = nullptr);
+  QString selection;
+
+private:
+  QTreeWidget *treeWidget;
+  QPushButton *confirm_btn;
+};

selfdrive/ui/sunnypilot/SConscript

@@ -21,6 +21,7 @@ qt_src = [
   "sunnypilot/qt/home.cc",
   "sunnypilot/qt/offroad/exit_offroad_button.cc",
   "sunnypilot/qt/offroad/offroad_home.cc",
+  "sunnypilot/qt/offroad/settings/developer_panel.cc",
   "sunnypilot/qt/offroad/settings/device_panel.cc",
   "sunnypilot/qt/offroad/settings/lateral_panel.cc",
   "sunnypilot/qt/offroad/settings/longitudinal_panel.cc",
@@ -46,6 +47,7 @@ lateral_panel_qt_src = [
   "sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
+  "sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
 ]
 

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

@@ -0,0 +1,80 @@
+/**
+ * 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/developer_panel.h"
+
+DeveloperPanelSP::DeveloperPanelSP(SettingsWindow *parent) : DeveloperPanel(parent) {
+
+  // Advanced Controls Toggle
+  showAdvancedControls = new ParamControlSP("ShowAdvancedControls", tr("Show Advanced Controls"), tr("Toggle visibility of advanced sunnypilot controls.\nThis only toggles the visibility of the controls; it does not toggle the actual control enabled/disabled state."), "");
+  addItem(showAdvancedControls);
+
+  QObject::connect(showAdvancedControls, &ParamControlSP::toggleFlipped, this, [=](bool) {
+    AbstractControlSP::UpdateAllAdvancedControls();
+    updateToggles(!uiState()->scene.started);
+  });
+  showAdvancedControls->showDescription();
+
+  // Github Runner Toggle
+  enableGithubRunner = new ParamControlSP("EnableGithubRunner", tr("Enable GitHub runner service"), tr("Enables or disables the github runner service."), "", this, true);
+  addItem(enableGithubRunner);
+
+  // Quickboot Mode Toggle
+  prebuiltToggle = new ParamControlSP("QuickBootToggle", tr("Enable Quickboot Mode"), tr(""), "", this, true);
+  addItem(prebuiltToggle);
+
+  QObject::connect(prebuiltToggle, &ParamControl::toggleFlipped, [=](bool state) {
+    QString prebuiltPath = "/data/openpilot/prebuilt";
+    state ? QFile(prebuiltPath).open(QIODevice::WriteOnly) : QFile::remove(prebuiltPath);
+    prebuiltToggle->refresh();
+  });
+  prebuiltToggle->setVisible(false);
+
+  // Error log button
+  errorLogBtn = new ButtonControlSP(tr("Error Log"), tr("VIEW"), tr("View the error log for sunnypilot crashes."));
+  connect(errorLogBtn, &ButtonControlSP::clicked, [=]() {
+    QFileInfo file("/data/community/crashes/error.log");
+    QString text;
+    if (file.exists()) {
+      text = "<b>" + file.lastModified().toString("dd-MMM-yyyy hh:mm:ss ").toUpper() + "</b><br><br>";
+    }
+    text += QString::fromStdString(util::read_file("/data/community/crashes/error.log"));
+    ConfirmationDialog::rich(text, this);
+  });
+  addItem(errorLogBtn);
+
+  QObject::connect(uiState(), &UIState::offroadTransition, this, &DeveloperPanelSP::updateToggles);
+}
+
+void DeveloperPanelSP::updateToggles(bool offroad) {
+  bool is_release = params.getBool("IsReleaseBranch");
+  bool is_tested = params.getBool("IsTestedBranch");
+  bool is_development = params.getBool("IsDevelopmentBranch");
+  bool disable_updates = params.getBool("DisableUpdates");
+
+  prebuiltToggle->setVisible(!is_release && !is_tested && !is_development);
+  prebuiltToggle->setEnabled(disable_updates);
+
+  params.putBool("QuickBootToggle", QFile::exists("/data/openpilot/prebuilt"));
+  prebuiltToggle->refresh();
+
+  prebuiltToggle->setDescription(disable_updates
+    ? tr("When toggled on, this creates a prebuilt file to allow accelerated boot times. When toggled off, "
+         "it immediately removes the prebuilt file so compilation of locally edited cpp files can be made. "
+         "<br><br><b>To edit C++ files locally on device, you MUST first turn off this toggle so the changes can recompile.</b>")
+    : tr("Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first."));
+
+  enableGithubRunner->setVisible(!is_release);
+  errorLogBtn->setVisible(!is_release);
+  showAdvancedControls->setEnabled(true);
+}
+
+void DeveloperPanelSP::showEvent(QShowEvent *event) {
+  DeveloperPanel::showEvent(event);
+  updateToggles(!uiState()->scene.started);
+  AbstractControlSP::UpdateAllAdvancedControls();
+  prebuiltToggle->showDescription();
+}

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

@@ -0,0 +1,30 @@
+/**
+ * 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 <QFile>
+#include <QFileInfo>
+
+#include "selfdrive/ui/qt/offroad/developer_panel.h"
+
+class DeveloperPanelSP : public DeveloperPanel {
+  Q_OBJECT
+public:
+  explicit DeveloperPanelSP(SettingsWindow *parent);
+
+private:
+  ParamControlSP *enableGithubRunner;
+  ButtonControlSP *errorLogBtn;
+  ParamControlSP *prebuiltToggle;
+  Params params;
+  ParamControlSP *showAdvancedControls;
+
+private slots:
+  void updateToggles(bool offroad);
+
+protected:
+  void showEvent(QShowEvent *event) override;
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.cc

@@ -0,0 +1,87 @@
+/**
+ * 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/lateral/angle_tuning_settings.h"
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
+
+AngleTunningSettings::AngleTunningSettings(QWidget *parent) : QWidget(parent) {
+  QVBoxLayout *main_layout = new QVBoxLayout(this);
+  main_layout->setContentsMargins(50, 20, 50, 20);
+  main_layout->setSpacing(20);
+
+  // Back button
+  PanelBackButton *back = new PanelBackButton();
+  connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
+  main_layout->addWidget(back, 0, Qt::AlignLeft);
+
+  auto *list = new ListWidgetSP(this, false);
+
+  main_layout->addWidget(new QWidget());
+
+  enableHkgAngleSmoothingFactor = new ExpandableToggleRow("EnableHkgTuningAngleSmoothingFactor", tr("HKG Angle Smoothing Factor"), tr("Applies EMA (Exponential Moving Average) to the desired angle steering and avoid overcorrections."), "../assets/offroad/icon_blank.png");
+  list->addItem(enableHkgAngleSmoothingFactor);
+
+  auto first_row = new QHBoxLayout();
+  hkgTuningOverridingCycles = new OptionControlSP("HkgTuningOverridingCycles", tr("Override Ramp-Down Cycles"), tr("Number of cycles to ramp down the current amount of torque on the steering wheel.<br/>A smaller value means a faster override by the user (less effort)"), "../assets/offroad/icon_blank.png", {10, 30}, 1);
+  connect(hkgTuningOverridingCycles, &OptionControlSP::updateLabels, hkgTuningOverridingCycles, [=]() {
+    this->updateToggles(offroad);
+  });
+  first_row->addWidget(hkgTuningOverridingCycles);
+
+  hkgAngleMinTorque = new OptionControlSP("HkgTuningAngleMinTorqueReductionGain", tr("Override Steering Effort"), tr("Sets the steering effort percentage used when the driver is overriding lateral control.<br/>Higher values increase resistance and make the wheel feel stiffer."), "../assets/offroad/icon_blank.png", {5, 60}, 1);
+  connect(hkgAngleMinTorque, &OptionControlSP::updateLabels, hkgAngleMinTorque, [=]() {
+    this->updateToggles(offroad);
+  });
+  first_row->addWidget(hkgAngleMinTorque);
+  list->addItem(first_row);
+
+  auto second_row = new QHBoxLayout();
+  hkgAngleActiveTorque = new OptionControlSP("HkgTuningAngleActiveTorqueReductionGain", tr("Min Active Torque"), tr("Torque applied when lateral control is active but the vehicle is not turning.<br/>Used to maintain lane centering on straight paths when no user input is detected."), "../assets/offroad/icon_blank.png", {10, 100}, 1);
+  connect(hkgAngleActiveTorque, &OptionControlSP::updateLabels, hkgAngleActiveTorque, [=]() {
+    this->updateToggles(offroad);
+  });
+  second_row->addWidget(hkgAngleActiveTorque);
+
+  hkgAngleMaxTorque = new OptionControlSP("HkgTuningAngleMaxTorqueReductionGain", tr("Max Torque Allowance"), tr("Sets the maximum torque reduction percentage the controller can apply during normal lateral control.<br/>"), "../assets/offroad/icon_blank.png", {10, 100}, 1);
+  connect(hkgAngleMaxTorque, &OptionControlSP::updateLabels, hkgAngleMaxTorque, [=]() {
+    this->updateToggles(offroad);
+  });
+  second_row->addWidget(hkgAngleMaxTorque);
+  list->addItem(second_row);
+  
+  QObject::connect(uiState(), &UIState::offroadTransition, this, &AngleTunningSettings::updateToggles);
+
+  main_layout->addWidget(new ScrollViewSP(list, this));
+
+  auto *warning = new QLabel(tr("Reboot required for settings to apply; Tap on each setting to see more details."));
+  warning->setStyleSheet("font-size: 30px; font-weight: 500; font-family: 'Noto Color Emoji'; color: orange;");
+  main_layout->addWidget(warning, 0, Qt::AlignCenter);
+}
+
+void AngleTunningSettings::showEvent(QShowEvent *event) {
+  updateToggles(offroad);
+}
+
+void AngleTunningSettings::updateToggles(bool _offroad) {
+  auto HkgAngleSmoothingFactorValue = params.getBool("EnableHkgTuningAngleSmoothingFactor");
+  enableHkgAngleSmoothingFactor->toggleFlipped(HkgAngleSmoothingFactorValue);
+
+  auto HkgAngleMinTorqueValue = QString::fromStdString(params.get("HkgTuningAngleMinTorqueReductionGain")).toInt();
+  hkgAngleMinTorque->setLabel(QString::number(HkgAngleMinTorqueValue)+"%");
+
+  auto HkgAngleActiveTorqueValue = QString::fromStdString(params.get("HkgTuningAngleActiveTorqueReductionGain")).toInt();
+  hkgAngleActiveTorque->setLabel(QString::number(HkgAngleActiveTorqueValue)+"%");
+
+  auto HkgAngleMaxTorqueValue = QString::fromStdString(params.get("HkgTuningAngleMaxTorqueReductionGain")).toInt();
+  hkgAngleMaxTorque->setLabel(QString::number(HkgAngleMaxTorqueValue)+"%");
+
+  auto HkgTuningOverridingCyclesValue = QString::fromStdString(params.get("HkgTuningOverridingCycles")).toInt();
+  hkgTuningOverridingCycles->setLabel(QString::number(HkgTuningOverridingCyclesValue));
+
+  offroad = _offroad;
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.h

@@ -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.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/qt/util.h"
+#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"
+
+class AngleTunningSettings : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit AngleTunningSettings(QWidget *parent = nullptr);
+
+  void showEvent(QShowEvent *event) override;
+
+signals:
+  void backPress();
+
+public slots:
+  void updateToggles(bool _offroad);
+
+private:
+  Params params;
+  bool offroad;
+
+  ExpandableToggleRow* enableHkgAngleSmoothingFactor;
+  OptionControlSP* hkgAngleMinTorque;
+  OptionControlSP* hkgAngleActiveTorque;
+  OptionControlSP* hkgAngleMaxTorque;
+  OptionControlSP* hkgTuningOverridingCycles;
+  ParamControlSP* hkgAngleLiveTuning;
+};

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

@@ -89,6 +89,36 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
     nnlcToggle->updateToggle();
   });
 
+#pragma region hkg angle tuning
+  list->addItem(vertical_space());
+  list->addItem(horizontal_line());
+  list->addItem(vertical_space());
+
+  // HKG Angle Tuning
+  // angleTuningToggle = new ParamControl(
+  //   "AngleTuning",
+  //   tr("Modular Assistive Driving System (MADS)"),
+  //   tr("Enable the beloved MADS feature. Disable toggle to revert back to stock sunnypilot engagement/disengagement."),
+  //   "");
+  // angleTuningToggle->setConfirmation(true, false);
+  // list->addItem(angleTuningToggle);
+
+  angleTuningSettingsButton = new PushButtonSP(tr("Customize ANGLE Tuning"));
+  angleTuningSettingsButton->setObjectName("angle_btn");
+  connect(angleTuningSettingsButton, &QPushButton::clicked, [=]() {
+    sunnypilotScroller->setLastScrollPosition();
+    main_layout->setCurrentWidget(angleTuningWidget);
+  });
+  // QObject::connect(angleTuningToggle, &ToggleControl::toggleFlipped, angleTuningSettingsButton, &PushButtonSP::setEnabled);
+
+  angleTuningWidget = new AngleTunningSettings(this);
+  connect(angleTuningWidget, &AngleTunningSettings::backPress, [=]() {
+    sunnypilotScroller->restoreScrollPosition();
+    main_layout->setCurrentWidget(sunnypilotScreen);
+  });
+  list->addItem(angleTuningSettingsButton);
+#pragma endregion
+
   toggleOffroadOnly = {
     madsToggle, nnlcToggle,
   };
@@ -99,6 +129,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
 
   main_layout->addWidget(sunnypilotScreen);
   main_layout->addWidget(madsWidget);
+  main_layout->addWidget(angleTuningWidget);
   main_layout->addWidget(laneChangeWidget);
 
   setStyleSheet(R"(
@@ -149,6 +180,7 @@ void LateralPanel::updateToggles(bool _offroad) {
   }
 
   madsSettingsButton->setEnabled(madsToggle->isToggled());
+  // angleTuningSettingsButton->setEnabled(angleTuningToggle->isToggled());
 
   blinkerPauseLateralSettings->refresh();
 

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

@@ -13,6 +13,7 @@
 #include "selfdrive/ui/sunnypilot/ui.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h"
 #include "selfdrive/ui/qt/util.h"
@@ -39,8 +40,11 @@ private:
   bool offroad;
 
   ParamControl *madsToggle;
+  // ParamControl *angleTuningToggle;
   PushButtonSP *madsSettingsButton;
+  PushButtonSP *angleTuningSettingsButton;
   MadsSettings *madsWidget = nullptr;
+  AngleTunningSettings *angleTuningWidget = nullptr;
   PushButtonSP *laneChangeSettingsButton;
   LaneChangeSettings *laneChangeWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;

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

@@ -8,6 +8,8 @@
 #include <algorithm>
 #include <QJsonDocument>
 #include <QStyle>
+#include <QtConcurrent/QtConcurrent>
+#include <QDir>
 
 #include "common/model.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h"
@@ -66,6 +68,11 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   connect(uiStateSP(), &UIStateSP::uiUpdate, this, &ModelsPanel::updateLabels);
   list->addItem(currentModelLblBtn);
 
+  clearModelCacheBtn = new ButtonControlSP(tr("Clear Model Cache"), tr("CLEAR"), "", this);
+  connect(clearModelCacheBtn, &ButtonControlSP::clicked, this, &ModelsPanel::clearModelCache);
+
+  list->addItem(clearModelCacheBtn);
+
   // Create progress bars for downloads
   supercomboProgressBar = createProgressBar(this);
   QString supercomboType = tr("Driving Model");
@@ -98,7 +105,7 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   delay_control = new OptionControlSP("LagdToggledelay", tr("Adjust Software Delay"),
                                      tr("Adjust the software delay when Live Learning Steer Delay is toggled off."
                                         "\nThe default software delay value is 0.2"),
-                                     "", {10, 30}, 1, false, nullptr, true);
+                                     "", {5, 30}, 1, false, nullptr, true, true);
 
   connect(delay_control, &OptionControlSP::updateLabels, [=]() {
     float value = QString::fromStdString(params.get("LagdToggledelay")).toFloat();
@@ -244,28 +251,73 @@ void ModelsPanel::handleCurrentModelLblBtnClicked() {
   currentModelLblBtn->setEnabled(false);
   currentModelLblBtn->setValue(tr("Fetching models..."));
 
-  // Create mapping of bundle indices to display names
+  struct ModelEntry {
-  QMap<uint32_t, QString> index_to_bundle;
+    QString folder;
+    QString displayName;
+    int index;
+  };
+  QList<ModelEntry> sortedModels;
+  QSet<QString> modelFolders;
   const auto bundles = model_manager.getAvailableBundles();
-  for (const auto &bundle: bundles) {
+
-    index_to_bundle.insert(bundle.getIndex(), QString::fromStdString(bundle.getDisplayName()));
+  for (const auto &bundle : bundles) {
+    auto overrides = bundle.getOverrides();
+    QString gen;
+    for (const auto &override : overrides) {
+      if (override.getKey() == "folder") {
+        gen = QString::fromStdString(override.getValue().cStr());
+      }
+    }
+
+    modelFolders.insert(gen);
+    sortedModels.append(ModelEntry{
+      gen,
+      QString::fromStdString(bundle.getDisplayName()),
+      static_cast<int>(bundle.getIndex())
+    });
   }
 
-  // Sort bundles by index in descending order
+  std::sort(sortedModels.begin(), sortedModels.end(),
-  QStringList bundleNames;
+    [](const ModelEntry &a, const ModelEntry &b) {
-  // Add "Default" as the first option
+      return a.index > b.index;
-  bundleNames.append(DEFAULT_MODEL);
+    });
 
-  auto indices = index_to_bundle.keys();
+  // Create a list of folder-maxIndex pairs for sorting
-  std::sort(indices.begin(), indices.end(), std::greater<uint32_t>());
+  QList<QPair<QString, int>> folderMaxIndices;
-  for (const auto &index: indices) {
+  for (const auto &folder : modelFolders) {
-    bundleNames.append(index_to_bundle[index]);
+    int maxIndex = -1;
+    for (const auto &model : sortedModels) {
+      if (model.folder == folder) {
+        maxIndex = std::max(maxIndex, model.index);
+      }
+    }
+    folderMaxIndices.append(qMakePair(folder, maxIndex));
   }
 
+  // Sort folders by their highest model index
+  std::sort(folderMaxIndices.begin(), folderMaxIndices.end(),
+      [](const QPair<QString, int> &a, const QPair<QString, int> &b) {
+          return a.second > b.second;
+      });
+
+  // Create the final items list using sorted folders
+  QList<QPair<QString, QStringList>> items;
+  for (const auto &folderPair : folderMaxIndices) {
+    QStringList folderModels;
+    for (const auto &model : sortedModels) {
+      if (model.folder == folderPair.first) {
+        folderModels.append(model.displayName);
+      }
+    }
+    items.append(qMakePair(folderPair.first, folderModels));
+  }
+
+  items.insert(0, qMakePair(QString(""), QStringList{DEFAULT_MODEL}));
+
   currentModelLblBtn->setValue(GetActiveModelInternalName());
 
-  const QString selectedBundleName = MultiOptionDialog::getSelection(
+  const QString selectedBundleName = TreeOptionDialog::getSelection(
-    tr("Select a Model"), bundleNames, GetActiveModelName(), this);
+    tr("Select a Model"), items, GetActiveModelName(), this);
 
   if (selectedBundleName.isEmpty() || !canContinueOnMeteredDialog()) {
     return;
@@ -322,6 +374,8 @@ void ModelsPanel::updateLabels() {
     delay_control->setLabel(QString::number(value, 'f', 2) + "s");
     delay_control->showDescription();
   }
+
+  clearModelCacheBtn->setValue(QString::number(calculateCacheSize(), 'f', 2) + " MB");
 }
 
 /**
@@ -342,3 +396,32 @@ void ModelsPanel::showResetParamsDialog() {
     params.remove("LiveTorqueParameters");
   }
 }
+
+void ModelsPanel::clearModelCache() {
+  QString confirmMsg = tr("This will delete ALL downloaded models from the cache"
+                            "<br/><u>except the currently active model</u>."
+                            "<br/><br/>Are you sure you want to continue?");
+  QString content("<body><h2 style=\"text-align: center;\">" + tr("Driving Model Selector") + "</h2><br>"
+                "<p style=\"text-align: center; margin: 0 128px; font-size: 50px;\">" + confirmMsg + "</p></body>");
+  if (showConfirmationDialog(
+    content,
+    tr("Clear Cache"))) {
+      params.putBool("ModelManager_ClearCache", true);
+    }
+}
+
+double ModelsPanel::calculateCacheSize() {
+  QFuture<qint64> future_ModelCacheSize = QtConcurrent::run([=]() {
+
+    QDir model_dir(QString::fromStdString(Path::model_root()));
+    QFileInfoList model_files = model_dir.entryInfoList(QDir::Files | QDir::NoDotAndDotDot);
+    qint64 totalSize = 0;
+    for (const QFileInfo &model_file : model_files) {
+        if (model_file.isFile()) {
+            totalSize += model_file.size();
+        }
+    }
+    return totalSize;
+  });
+  return static_cast<double>(future_ModelCacheSize) / (1024.0 * 1024.0);
+}

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

@@ -41,6 +41,8 @@ private:
   cereal::ModelManagerSP::Reader model_manager;
   cereal::ModelManagerSP::DownloadStatus download_status{};
   cereal::ModelManagerSP::DownloadStatus prev_download_status{};
+  void clearModelCache();
+  double calculateCacheSize();
 
   bool canContinueOnMeteredDialog() {
     if (!is_metered) return true;
@@ -75,5 +77,6 @@ private:
   QProgressBar *policyProgressBar;
   QFrame *policyFrame;
   Params params;
+  ButtonControlSP *clearModelCacheBtn;
 
 };

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

@@ -8,10 +8,10 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
-#include "selfdrive/ui/qt/offroad/developer_panel.h"
 #include "selfdrive/ui/qt/offroad/firehose.h"
 #include "selfdrive/ui/sunnypilot/qt/network/networking.h"
 
+#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/models_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h"
@@ -91,7 +91,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) {
     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"),
     PanelInfo("   " + tr("Firehose"), new FirehosePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_firehose.svg"),
-    PanelInfo("   " + tr("Developer"), new DeveloperPanel(this), "../assets/icons/shell.png"),
+    PanelInfo("   " + tr("Developer"), new DeveloperPanelSP(this), "../assets/icons/shell.png"),
   };
 
   nav_btns = new QButtonGroup(this);

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

@@ -18,6 +18,18 @@ SoftwarePanelSP::SoftwarePanelSP(QWidget *parent) : SoftwarePanel(parent) {
         searchBranches(d.text());
       }
   });
+
+  // Disable Updates toggle
+  disableUpdatesToggle = new ParamControl("DisableUpdates",
+    tr("Disable Updates"),
+    tr("When enabled, software updates will be disabled. <b>This requires a reboot to take effect.</b>"),
+    "../assets/icons/icon_warning.png",
+    this, true);
+  disableUpdatesToggle->showDescription();
+  addItem(disableUpdatesToggle);
+  connect(disableUpdatesToggle, &ParamControl::toggleFlipped, this, &SoftwarePanelSP::handleDisableUpdatesToggled);
+  connect(uiState(), &UIState::offroadTransition, this, &SoftwarePanelSP::updateDisableUpdatesToggle);
+  updateDisableUpdatesToggle(!uiState()->scene.started);
 }
 
 /**
@@ -49,3 +61,27 @@ void SoftwarePanelSP::searchBranches(const QString &query) {
     checkForUpdates();
   }
 }
+
+void SoftwarePanelSP::handleDisableUpdatesToggled(bool state) {
+  if (ConfirmationDialog::confirm(tr("%1 updates requires a reboot.<br>Reboot now?")
+      .arg(state ? "Disabling" : "Enabling"), tr("Reboot"), this)) {
+    params.putBool("DoReboot", true);
+  } else {
+    params.putBool("DisableUpdates", !state);
+    disableUpdatesToggle->refresh();
+  }
+}
+
+void SoftwarePanelSP::updateDisableUpdatesToggle(bool offroad) {
+  bool enabled = offroad;
+  disableUpdatesToggle->setEnabled(enabled);
+  disableUpdatesToggle->setDescription(enabled
+    ? tr("When enabled, software updates will be disabled.<br><b>This requires a reboot to take effect.</b>")
+    : tr("Please enable always offroad mode or turn off vehicle to adjust these toggles"));
+}
+
+void SoftwarePanelSP::showEvent(QShowEvent *event) {
+  SoftwarePanel::showEvent(event);
+  updateDisableUpdatesToggle(!uiState()->scene.started);
+  disableUpdatesToggle->showDescription();
+}

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

@@ -19,4 +19,10 @@ public:
 
 private:
   void searchBranches(const QString &query);
+  ParamControl *disableUpdatesToggle = nullptr;
+  void handleDisableUpdatesToggled(bool state);
+private slots:
+  void updateDisableUpdatesToggle(bool offroad);
+protected:
+  void showEvent(QShowEvent *event) override;
 };

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

@@ -30,6 +30,7 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
     },
   };
 
+  // Add regular toggles first
   for (auto &[param, title, desc, icon, needs_restart] : toggle_defs) {
     auto toggle = new ParamControlSP(param, title, desc, icon, this);
 
@@ -53,6 +54,17 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
     param_watcher->addParam(param);
   }
 
+  // Visuals: Display Metrics below Chevron
+  std::vector<QString> chevron_info_settings_texts{tr("Off"), tr("Distance"), tr("Speed"), tr("Time"), tr("All")};
+  chevron_info_settings = new ButtonParamControlSP(
+    "ChevronInfo", tr("Display Metrics Below Chevron"), tr("Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control)."),
+    "",
+    chevron_info_settings_texts,
+    200);
+  chevron_info_settings->showDescription();
+  list->addItem(chevron_info_settings);
+  param_watcher->addParam("ChevronInfo");
+
   sunnypilotScroller = new ScrollViewSP(list, this);
   vlayout->addWidget(sunnypilotScroller);
 
@@ -67,4 +79,8 @@ void VisualsPanel::paramsRefresh() {
   for (auto toggle : toggles) {
     toggle.second->refresh();
   }
+
+  if (chevron_info_settings) {
+    chevron_info_settings->refresh();
+  }
 }

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

@@ -27,4 +27,5 @@ protected:
  Params params;
  std::map<std::string, ParamControlSP*> toggles;
  ParamWatcher * param_watcher;
+ ButtonParamControlSP *chevron_info_settings;
 };

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

@@ -30,9 +30,24 @@ QFrame *vertical_space(int height, QWidget *parent) {
 }
 
 // AbstractControlSP
+std::vector<AbstractControlSP*> AbstractControlSP::advanced_controls_;
+AbstractControlSP::~AbstractControlSP() { UnregisterAdvancedControl(this); }
 
-AbstractControlSP::AbstractControlSP(const QString &title, const QString &desc, const QString &icon, QWidget *parent)
+void AbstractControlSP::RegisterAdvancedControl(AbstractControlSP *ctrl) { advanced_controls_.push_back(ctrl); }
-    : AbstractControl(title, desc, icon, parent) {
+
+void AbstractControlSP::UnregisterAdvancedControl(AbstractControlSP *ctrl) {
+  advanced_controls_.erase(std::remove(advanced_controls_.begin(), advanced_controls_.end(), ctrl), advanced_controls_.end());
+}
+
+void AbstractControlSP::UpdateAllAdvancedControls() {
+  bool visibility = Params().getBool("ShowAdvancedControls");
+  advanced_controls_.erase(std::remove(advanced_controls_.begin(), advanced_controls_.end(), nullptr), advanced_controls_.end());
+  for (auto *ctrl : advanced_controls_) ctrl->setVisible(visibility);
+}
+
+AbstractControlSP::AbstractControlSP(const QString &title, const QString &desc, const QString &icon, QWidget *parent, bool advancedControl)
+    : AbstractControl(title, desc, icon, parent), isAdvancedControl(advancedControl) {
+  if (isAdvancedControl) RegisterAdvancedControl(this);
 
   main_layout = new QVBoxLayout(this);
   main_layout->setMargin(0);
@@ -82,8 +97,8 @@ void AbstractControlSP::hideEvent(QHideEvent *e) {
   }
 }
 
-AbstractControlSP_SELECTOR::AbstractControlSP_SELECTOR(const QString &title, const QString &desc, const QString &icon, QWidget *parent)
+AbstractControlSP_SELECTOR::AbstractControlSP_SELECTOR(const QString &title, const QString &desc, const QString &icon, QWidget *parent, bool advancedControl)
-    : AbstractControlSP(title, desc, icon, parent) {
+    : AbstractControlSP(title, desc, icon, parent, advancedControl) {
 
   if (title_label != nullptr) {
     delete title_label;
@@ -169,8 +184,8 @@ void AbstractControlSP_SELECTOR::hideEvent(QHideEvent *e) {
 
 // controls
 
-ButtonControlSP::ButtonControlSP(const QString &title, const QString &text, const QString &desc, QWidget *parent)
+ButtonControlSP::ButtonControlSP(const QString &title, const QString &text, const QString &desc, QWidget *parent, bool advancedControl)
-    : AbstractControlSP(title, desc, "", parent) {
+    : AbstractControlSP(title, desc, "", parent, advancedControl) {
 
   btn.setText(text);
   btn.setStyleSheet(R"(
@@ -225,8 +240,8 @@ void ElidedLabelSP::paintEvent(QPaintEvent *event) {
 
 // ParamControlSP
 
-ParamControlSP::ParamControlSP(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent)
+ParamControlSP::ParamControlSP(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent, bool advancedControl)
-    : ToggleControlSP(title, desc, icon, false, parent) {
+    : ToggleControlSP(title, desc, icon, false, parent, advancedControl){
 
   key = param.toStdString();
   QObject::connect(this, &ParamControlSP::toggleFlipped, this, &ParamControlSP::toggleClicked);

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

@@ -57,6 +57,7 @@ class AbstractControlSP : public AbstractControl {
   Q_OBJECT
 
 public:
+  ~AbstractControlSP();
   void setDescription(const QString &desc) override {
     if (description) description->setText(desc);
   }
@@ -81,13 +82,30 @@ public slots:
     description->setVisible(true);
   }
 
+  void setVisible(bool visible) override {
+    bool _visible = visible;
+    if (isAdvancedControl && !params.getBool("ShowAdvancedControls")) {
+      _visible = false;
+    }
+    AbstractControl::setVisible(_visible);
+  }
+
+  static void RegisterAdvancedControl(AbstractControlSP *ctrl);
+  static void UnregisterAdvancedControl(AbstractControlSP *ctrl);
+  static void UpdateAllAdvancedControls();
+
 protected:
-  AbstractControlSP(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr);
+  AbstractControlSP(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr, bool advancedControl = false);
   void hideEvent(QHideEvent *e) override;
 
   QVBoxLayout *main_layout;
   ElidedLabelSP *value;
   QLabel *description = nullptr;
+  bool isAdvancedControl;
+
+private:
+  Params params;
+  static std::vector<AbstractControlSP*> advanced_controls_;
 };
 
 // AbstractControlSP_SELECTOR
@@ -97,7 +115,7 @@ class AbstractControlSP_SELECTOR : public AbstractControlSP {
 
 protected:
   QSpacerItem *spacingItem = new QSpacerItem(44, 44, QSizePolicy::Minimum, QSizePolicy::Fixed);
-  AbstractControlSP_SELECTOR(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr);
+  AbstractControlSP_SELECTOR(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr, bool advancedControl = false);
   void hideEvent(QHideEvent *e) override;
 
 };
@@ -123,7 +141,7 @@ class ButtonControlSP : public AbstractControlSP {
   Q_OBJECT
 
 public:
-  ButtonControlSP(const QString &title, const QString &text, const QString &desc = "", QWidget *parent = nullptr);
+  ButtonControlSP(const QString &title, const QString &text, const QString &desc = "", QWidget *parent = nullptr, bool advancedControl = false);
   inline void setText(const QString &text) { btn.setText(text); }
   inline QString text() const { return btn.text(); }
   inline void click() { btn.click(); }
@@ -142,7 +160,7 @@ class ToggleControlSP : public AbstractControlSP {
   Q_OBJECT
 
 public:
-  ToggleControlSP(const QString &title, const QString &desc = "", const QString &icon = "", const bool state = false, QWidget *parent = nullptr) : AbstractControlSP(title, desc, icon, parent) {
+  ToggleControlSP(const QString &title, const QString &desc = "", const QString &icon = "", const bool state = false, QWidget *parent = nullptr, bool advancedControl = false) : AbstractControlSP(title, desc, icon, parent, advancedControl) {
     // space between toggle and title
     icon_label = new QLabel(this);
     hlayout->addWidget(icon_label);
@@ -173,7 +191,7 @@ class ParamControlSP : public ToggleControlSP {
   Q_OBJECT
 
 public:
-  ParamControlSP(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr);
+  ParamControlSP(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr, bool advancedControl = false);
   void setConfirmation(bool _confirm, bool _store_confirm) {
     confirm = _confirm;
     store_confirm = _store_confirm;
@@ -219,7 +237,7 @@ class MultiButtonControlSP : public AbstractControlSP_SELECTOR {
 
 public:
   MultiButtonControlSP(const QString &title, const QString &desc, const QString &icon,
-                     const std::vector<QString> &button_texts, const int minimum_button_width = 225, const bool inline_layout = false) : AbstractControlSP_SELECTOR(title, desc, icon), button_texts(button_texts), is_inline_layout(inline_layout) {
+                     const std::vector<QString> &button_texts, const int minimum_button_width = 225, const bool inline_layout = false, bool advancedControl = false) : AbstractControlSP_SELECTOR(title, desc, icon, nullptr, advancedControl), button_texts(button_texts), is_inline_layout(inline_layout) {
     const QString style = R"(
       QPushButton {
         border-radius: 20px;
@@ -371,8 +389,8 @@ class ButtonParamControlSP : public MultiButtonControlSP {
   Q_OBJECT
 public:
   ButtonParamControlSP(const QString &param, const QString &title, const QString &desc, const QString &icon,
-                     const std::vector<QString> &button_texts, const int minimum_button_width = 225, const bool inline_layout = false) : MultiButtonControlSP(title, desc, icon,
+                     const std::vector<QString> &button_texts, const int minimum_button_width = 225, const bool inline_layout = false, bool advancedControl = false) : MultiButtonControlSP(title, desc, icon,
-                                                                                                                          button_texts, minimum_button_width, inline_layout) {
+                                                                                                                          button_texts, minimum_button_width, inline_layout, advancedControl) {
     key = param.toStdString();
     int value = atoi(params.get(key).c_str());
 
@@ -499,7 +517,7 @@ private:
 public:
   OptionControlSP(const QString &param, const QString &title, const QString &desc, const QString &icon,
                   const MinMaxValue &range, const int per_value_change = 1, const bool inline_layout = false,
-                  const QMap<QString, QString> *valMap = nullptr, bool scale_float = false) : AbstractControlSP_SELECTOR(title, desc, icon, nullptr), _title(title), valueMap(valMap), is_inline_layout(inline_layout), use_float_scaling(scale_float) {
+                  const QMap<QString, QString> *valMap = nullptr, bool scale_float = false, bool advancedControl = false) : AbstractControlSP_SELECTOR(title, desc, icon, nullptr, advancedControl), _title(title), valueMap(valMap), is_inline_layout(inline_layout), use_float_scaling(scale_float) {
     const QString style = R"(
       QPushButton {
         border-radius: 20px;

sunnypilot/modeld/fill_model_msg.py

@@ -103,7 +103,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
   # temporal pose
-  temporal_pose = modelV2.temporalPose
+  temporal_pose = modelV2.temporalPoseDEPRECATED
   temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
   temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
   temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()

sunnypilot/modeld/modeld.py

@@ -62,7 +62,7 @@ class ModelState:
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
     bundle = get_active_bundle()
     overrides = {override.key: override.value for override in bundle.overrides}
-    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2"))
+    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
 
     model_paths = get_model_path()

sunnypilot/modeld_v2/fill_model_msg.py

@@ -10,8 +10,8 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 ConfidenceClass = log.ModelDataV2.ConfidenceClass
 
 
-def get_curvature_from_output(output, vego, lat_action_t, current_generation=None):
+def get_curvature_from_output(output, vego, lat_action_t, mlsim):
-  if current_generation != 11:
+  if not mlsim:
     if desired_curv := output.get('desired_curvature'):  # If the model outputs the desired curvature, use that directly
       return float(desired_curv[0, 0])
 
@@ -100,7 +100,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
   # temporal pose
-  temporal_pose = modelV2.temporalPose
+  temporal_pose = modelV2.temporalPoseDEPRECATED
   if 'sim_pose' in net_output_data:
     temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
     temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()

sunnypilot/modeld_v2/modeld.py

@@ -54,10 +54,10 @@ class ModelState:
       raise
 
     model_bundle = get_active_bundle()
-    self.generation = model_bundle.generation
+    self.generation = model_bundle.generation if model_bundle is not None else None
     overrides = {override.key: override.value for override in model_bundle.overrides}
 
-    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2"))
+    self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
     self.MIN_LAT_CONTROL_SPEED = 0.3
 
@@ -86,6 +86,10 @@ class ModelState:
       self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1,
                                   self.numpy_inputs['desire'].shape[2])
 
+  @property
+  def mlsim(self) -> bool:
+    return bool(self.generation is not None and self.generation >= 11)
+
   def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
                 inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
     # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
@@ -151,7 +155,7 @@ class ModelState:
       self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
       self.full_prev_desired_curv[0,-1,:] = outputs['desired_curvature'][0, :]
       self.numpy_inputs[input_name_prev][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
-      if self.generation == 11:
+      if self.mlsim:
         self.numpy_inputs[input_name_prev][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
     else:
       length = outputs['desired_curvature'][0].size
@@ -165,7 +169,7 @@ class ModelState:
                                                      action_t=long_action_t)
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
 
-    desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, self.generation)
+    desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, self.mlsim)
     if v_ego > self.MIN_LAT_CONTROL_SPEED:
       desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS)
     else:

sunnypilot/modeld_v2/parse_model_outputs_split.py

@@ -1,5 +1,6 @@
 import numpy as np
 from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
+from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 
 def safe_exp(x, out=None):
@@ -24,6 +25,8 @@ def softmax(x, axis=-1):
 class Parser:
   def __init__(self, ignore_missing=False):
     self.ignore_missing = ignore_missing
+    model_bundle = get_active_bundle()
+    self.generation = model_bundle.generation if model_bundle is not None else None
 
   def check_missing(self, outs, name):
     if name not in outs and not self.ignore_missing:
@@ -88,37 +91,64 @@ class Parser:
     outs[name] = pred_mu_final.reshape(final_shape)
     outs[name + '_stds'] = pred_std_final.reshape(final_shape)
 
+  def _parse_plan_mhp(self, outs):
+    self.parse_mdn('plan', outs, in_N=SplitModelConstants.PLAN_MHP_N, out_N=SplitModelConstants.PLAN_MHP_SELECTION,
+                         out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH))
+
+  def parse_dynamic_outputs(self, outs: dict[str, np.ndarray]) -> None:
+    if 'lead' in outs:
+      if self.generation >= 12 and \
+         outs['lead'].shape[1] == 2 * SplitModelConstants.LEAD_MHP_SELECTION * SplitModelConstants.LEAD_TRAJ_LEN * SplitModelConstants.LEAD_WIDTH:
+        self.parse_mdn('lead', outs, in_N=0, out_N=0,
+                       out_shape=(SplitModelConstants.LEAD_MHP_SELECTION, SplitModelConstants.LEAD_TRAJ_LEN, SplitModelConstants.LEAD_WIDTH))
+      else:
+        self.parse_mdn('lead', outs, in_N=SplitModelConstants.LEAD_MHP_N, out_N=SplitModelConstants.LEAD_MHP_SELECTION,
+                       out_shape=(SplitModelConstants.LEAD_TRAJ_LEN, SplitModelConstants.LEAD_WIDTH))
+    if 'plan' in outs:
+      if self.generation >= 12 and \
+        outs['plan'].shape[1] > 2 * SplitModelConstants.PLAN_WIDTH * SplitModelConstants.IDX_N:
+        self._parse_plan_mhp(outs)
+      elif self.generation >= 12:
+        self.parse_mdn('plan', outs, in_N=0, out_N=0,
+                       out_shape=(SplitModelConstants.IDX_N, SplitModelConstants.PLAN_WIDTH))
+      else:
+        self._parse_plan_mhp(outs)
+
   def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
+    if 'desired_curvature' in outs:
+      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.DESIRED_CURV_WIDTH,))
+    if 'desire_pred' in outs:
+      self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH))
+    if 'desire_state' in outs:
+      self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,))
     if 'lane_lines' in outs:
       self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
-                     out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
+                    out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
+    if 'lane_lines_prob' in outs:
+      self.parse_binary_crossentropy('lane_lines_prob', outs)
+    if 'lead_prob' in outs:
+      self.parse_binary_crossentropy('lead_prob', outs)
+    if 'lat_planner_solution' in outs:
+      self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
+    if 'meta' in outs:
+      self.parse_binary_crossentropy('meta', outs)
+    if 'road_edges' in outs:
       self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
-                     out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
+                    out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
-      self.parse_mdn('lead', outs, in_N=SplitModelConstants.LEAD_MHP_N, out_N=SplitModelConstants.LEAD_MHP_SELECTION,
+    if 'sim_pose' in outs:
-                     out_shape=(SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH))
+      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
-      if 'sim_pose' in outs:
-        self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
-      for k in ['lead_prob', 'lane_lines_prob']:
-        self.parse_binary_crossentropy(k, outs)
 
   def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
     self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
+    self.parse_dynamic_outputs(outs)
     self.split_outputs(outs)
-    self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH))
-    self.parse_binary_crossentropy('meta', outs)
     return outs
 
   def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    self.parse_mdn('plan', outs, in_N=SplitModelConstants.PLAN_MHP_N, out_N=SplitModelConstants.PLAN_MHP_SELECTION,
+    self.parse_dynamic_outputs(outs)
-                   out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH))
     self.split_outputs(outs)
-    if 'lat_planner_solution' in outs:
-      self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
-    if 'desired_curvature' in outs:
-      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.DESIRED_CURV_WIDTH,))
-    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,))
     return outs
 
   def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:

sunnypilot/models/helpers.py

@@ -19,7 +19,7 @@ from openpilot.system.hardware import PC
 from openpilot.system.hardware.hw import Paths
 from pathlib import Path
 
-CURRENT_SELECTOR_VERSION = 6
+CURRENT_SELECTOR_VERSION = 7
 REQUIRED_MIN_SELECTOR_VERSION = 5
 
 USE_ONNX = os.getenv('USE_ONNX', PC)

sunnypilot/models/manager.py

@@ -178,6 +178,10 @@ class ModelManagerSP:
             finally:
               self.params.put("ModelManager_DownloadIndex", "")
 
+        if self.params.get("ModelManager_ClearCache", block=False, encoding="utf-8"):
+            self.clear_model_cache()
+            self.params.remove("ModelManager_ClearCache")
+
         self._report_status()
         rk.keep_time()
 
@@ -185,6 +189,31 @@ class ModelManagerSP:
         cloudlog.exception(f"Error in main thread: {str(e)}")
         rk.keep_time()
 
+  def clear_model_cache(self) -> None:
+    """
+    Clears the model cache directory of all files except those in the active model bundle.
+    """
+
+    # Get list of files used by active model bundle
+    active_files = []
+    if self.active_bundle is not None: # When the default model is active
+      for model in self.active_bundle.models:
+        if hasattr(model, 'artifact') and model.artifact.fileName:
+          active_files.append(model.artifact.fileName)
+        if hasattr(model, 'metadata') and model.metadata.fileName:
+          active_files.append(model.metadata.fileName)
+
+    # Remove all files except active ones
+    model_dir = Paths.model_root()
+    try:
+      for filename in os.listdir(model_dir):
+        if filename not in active_files:
+          file_path = os.path.join(model_dir, filename)
+          if os.path.isfile(file_path):
+            os.remove(file_path)
+      cloudlog.info("Model cache cleared, keeping active model files")
+    except Exception as e:
+      cloudlog.exception(f"Error clearing model cache: {str(e)}")
 
 def main():
   ModelManagerSP().main_thread()

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -16,12 +16,12 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
-    model_bundle = get_active_bundle()
+    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
-    self.generation = model_bundle.generation if model_bundle is not None else None
 
   @property
   def mlsim(self) -> bool:
-    return self.generation == 11
+    # If we don't have a generation set, we assume it's default model. Which as of today are mlsim.
+    return bool(self.generation is None or self.generation >= 11)
 
   def get_mpc_mode(self) -> str | None:
     if not self.dec.active():

system/hardware/hw.h

@@ -55,4 +55,8 @@ namespace Path {
      return "/dev/shm";
     #endif
  }
+
+  inline std::string model_root() {
+    return Hardware::PC() ? Path::comma_home() + "/media/0/models" : "/data/media/0/models";
+  }
 }  // namespace Path

system/manager/manager.py

@@ -50,10 +50,13 @@ def manager_init() -> None:
     ("BlindSpot", "0"),
     ("BlinkerMinLateralControlSpeed", "20"),  # MPH or km/h
     ("BlinkerPauseLateralControl", "0"),
+    ("Brightness", "0"),
+    ("ChevronInfo", "4"),
     ("CustomAccIncrementsEnabled", "0"),
     ("CustomAccLongPressIncrement", "5"),
     ("CustomAccShortPressIncrement", "1"),
     ("DeviceBootMode", "0"),
+    ("DisableUpdates", "0"),
     ("DynamicExperimentalControl", "0"),
     ("HyundaiLongitudinalTuning", "0"),
     ("InteractivityTimeout", "0"),
@@ -65,11 +68,18 @@ def manager_init() -> None:
     ("MadsUnifiedEngagementMode", "1"),
     ("MapdVersion", f"{VERSION}"),
     ("MaxTimeOffroad", "1800"),
-    ("Brightness", "0"),
     ("ModelManager_LastSyncTime", "0"),
     ("ModelManager_ModelsCache", ""),
     ("NeuralNetworkLateralControl", "0"),
+    ("QuickBootToggle", "0"),
     ("QuietMode", "0"),
+    ("ShowAdvancedControls", "0" if build_metadata.tested_channel else "1"),
+    ("EnableHkgTuningAngleSmoothingFactor", "1"),
+    ("HkgTuningAngleMinTorqueReductionGain", "10"),
+    ("HkgTuningAngleActiveTorqueReductionGain", "60"),
+    ("HkgTuningAngleMaxTorqueReductionGain", "100"),
+    ("HkgTuningOverridingCycles", "17"),
+    ("HkgAngleLiveTuning", "0"),
   ]
 
   # device boot mode
@@ -101,6 +111,7 @@ def manager_init() -> None:
   params.put("GitCommitDate", build_metadata.openpilot.git_commit_date)
   params.put("GitBranch", build_metadata.channel)
   params.put("GitRemote", build_metadata.openpilot.git_origin)
+  params.put_bool("IsDevelopmentBranch", build_metadata.development_channel)
   params.put_bool("IsTestedBranch", build_metadata.tested_channel)
   params.put_bool("IsReleaseBranch", build_metadata.release_channel)
   params.put("HardwareSerial", serial)

system/manager/process_config.py

@@ -114,7 +114,7 @@ procs = [
 
   PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
-  PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
+  PythonProcess("soundd", "selfdrive.ui.soundd", and_(only_onroad, not_joystick)),
   PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
   NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
   PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),

system/version.py

@@ -117,9 +117,13 @@ class BuildMetadata:
   def master_channel(self) -> bool:
     return self.channel in MASTER_SP_BRANCHES
 
+  @property
+  def development_channel(self) -> bool:
+    return self.channel.startswith("dev-") or self.channel.endswith("-prebuilt")
+
   @property
   def channel_type(self) -> str:
-    if self.channel.startswith("dev-"):
+    if self.development_channel:
       return "development"
     elif self.channel.startswith("staging-"):
       return "staging"

tools/joystick/README.md

@@ -5,6 +5,10 @@
 With joystick_control, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard.
 joystick_control uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks.
 
+## Note
+You might need to install [xow](https://github.com/medusalix/xow) to use the Xbox One controller on comma device. 
+Even though **xone** is recommended on the xow page, **xow** is the one that works with the comma device.
+
 ## Usage
 
 The car must be off, and openpilot must be offroad before starting `joystick_control`.

tools/joystick/joystick_control.py

@@ -3,7 +3,10 @@ import os
 import argparse
 import threading
 import numpy as np
-from inputs import UnpluggedError, get_gamepad
+try:
+  import pygame as pg
+except ImportError:
+  print("pygame is not installed. Please install it with 'uv pip install pygame'")
 
 from cereal import messaging
 from openpilot.common.params import Params
@@ -11,7 +14,10 @@ from openpilot.common.realtime import Ratekeeper
 from openpilot.system.hardware import HARDWARE
 from openpilot.tools.lib.kbhit import KBHit
 
-EXPO = 0.4
+# Set SDL environment variable to avoid using a video driver for headless operation
+os.environ["SDL_VIDEODRIVER"] = "dummy"
+
+EXPO = 0.4  # Exponential factor for joystick response curve
 
 
 class Keyboard:
@@ -40,60 +46,122 @@ class Keyboard:
     return True
 
 
-class Joystick:
+class PyGameJoystick:
   def __init__(self):
-    # This class supports a PlayStation 5 DualSense controller on the comma 3X
+    # Initialize pygame and joystick subsystem
-    # TODO: find a way to get this from API or detect gamepad/PC, perhaps "inputs" doesn't support it
+    pg.init()
-    self.cancel_button = 'BTN_NORTH'  # BTN_NORTH=X/triangle
+    if not pg.joystick.get_init():
-    if HARDWARE.get_device_type() == 'pc':
+      pg.joystick.init()
-      accel_axis = 'ABS_Z'
-      steer_axis = 'ABS_RX'
-      # TODO: once the longcontrol API is finalized, we can replace this with outputting gas/brake and steering
-      self.flip_map = {'ABS_RZ': accel_axis}
-    else:
-      accel_axis = 'ABS_RX'
-      steer_axis = 'ABS_Z'
-      self.flip_map = {'ABS_RY': accel_axis}
 
-    self.min_axis_value = {accel_axis: 0., steer_axis: 0.}
+    # Find connected joysticks
-    self.max_axis_value = {accel_axis: 255., steer_axis: 255.}
+    joystick_count = pg.joystick.get_count()
-    self.axes_values = {accel_axis: 0., steer_axis: 0.}
+    if joystick_count == 0:
-    self.axes_order = [accel_axis, steer_axis]
+      print("No joysticks found. Please connect a controller.")
+      exit(1)
+
+    # Initialize the first joystick
+    self.joystick = pg.joystick.Joystick(0)
+    self.joystick.init()
+
+    print(f"Using joystick: {self.joystick.get_name()}")
+    print(f"Number of axes: {self.joystick.get_numaxes()}")
+    print(f"Number of buttons: {self.joystick.get_numbuttons()}")
+    print(f"Number of hats: {self.joystick.get_numhats()}")
+
+    # This supports PlayStation and Xbox controllers on different platforms
+    if HARDWARE.get_device_type() == 'pc':
+      # Xbox mapping on PC
+      self.accel_axis = 5  # Right trigger
+      self.brake_axis = 4  # Left trigger
+      self.steer_axis = 0  # Left stick horizontal
+      self.cancel_button = 3  # Y/Triangle button
+    else:
+      # PlayStation mapping on comma device
+      self.accel_axis = 5  # R2
+      self.brake_axis = 4  # L2
+      self.steer_axis = 0  # Left stick horizontal
+      self.cancel_button = 3  # Triangle button
+
+    # Configure for adaptive mappings based on detected controller
+    controller_name = self.joystick.get_name().lower()
+    if "xbox" in controller_name:
+      print("Xbox controller detected, using Xbox mappings")
+      self.accel_axis = 5  # Right trigger (RT)
+      self.brake_axis = 4  # Left trigger (LT)
+      self.cancel_button = 3  # Y button
+    elif "playstation" in controller_name or "dual" in controller_name:
+      print("PlayStation controller detected, using PlayStation mappings")
+      self.accel_axis = 5  # R2
+      self.brake_axis = 4  # L2
+      self.cancel_button = 3  # Triangle
+
+    # Initialize values
+    self.axes_values = {'gb': 0., 'steer': 0.}  # Maintain same keys as Keyboard class
+    self.axes_order = ['gb', 'steer']  # Match expected format
     self.cancel = False
+    self.deadzone = 0.03  # 3% deadzone for noisy joysticks
+
+    # Process events once to clear the event queue
+    pg.event.pump()
 
   def update(self):
+    # Process pygame events
     try:
-      joystick_event = get_gamepad()[0]
+      for event in pg.event.get():
-    except (OSError, UnpluggedError):
+        if event.type == pg.JOYDEVICEREMOVED:
-      self.axes_values = dict.fromkeys(self.axes_values, 0.)
+          if event.instance_id == self.joystick.get_instance_id():
+            print("Joystick disconnected!")
+            self.axes_values = {'gb': 0., 'steer': 0.}
+            return False
+
+        elif event.type == pg.JOYBUTTONDOWN:
+          if event.button == self.cancel_button:
+            self.cancel = True
+
+        elif event.type == pg.JOYBUTTONUP:
+          if event.button == self.cancel_button:
+            self.cancel = False
+    except Exception as e:
+      print(f"Error processing events: {e}")
       return False
 
-    event = (joystick_event.code, joystick_event.state)
+    # Read current joystick state directly
+    try:
+      if not self.joystick.get_init():
+        print("Joystick not initialized")
+        return False
 
-    # flip left trigger to negative accel
+      # Read steering (left stick horizontal)
-    if event[0] in self.flip_map:
+      steer_raw = self.joystick.get_axis(self.steer_axis) * -1
-      event = (self.flip_map[event[0]], -event[1])
+      steer = steer_raw if abs(steer_raw) > self.deadzone else 0.0
 
-    if event[0] == self.cancel_button:
+      # Read gas (right trigger)
-      if event[1] == 1:
+      accel_raw = self.joystick.get_axis(self.accel_axis)
-        self.cancel = True
+      # Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed)
-      elif event[1] == 0:   # state 0 is falling edge
+      accel = (accel_raw + 1) / 2 if self.accel_axis in [4, 5] else accel_raw
-        self.cancel = False
+      accel = accel if accel > self.deadzone else 0.0
-    elif event[0] in self.axes_values:
-      self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
-      self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
 
-      norm = -float(np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]))
+      # Read brake (left trigger)
-      norm = norm if abs(norm) > 0.03 else 0.  # center can be noisy, deadzone of 3%
+      brake_raw = self.joystick.get_axis(self.brake_axis)
-      self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm  # less action near center for fine control
+      # Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed)
-    else:
+      brake = (brake_raw + 1) / 2 if self.brake_axis in [4, 5] else brake_raw
+      brake = brake if brake > self.deadzone else 0.0
+
+      # Apply expo for steering
+      self.axes_values['steer'] = EXPO * steer**3 + (1 - EXPO) * steer  # Apply expo for fine control
+
+      # Calculate combined gas/brake value for output [-1, 1] where negative is brake
+      self.axes_values['gb'] = accel - brake
+
+    except Exception as e:
+      print(f"Error reading joystick: {e}")
+      self.axes_values = {'gb': 0., 'steer': 0.}
       return False
     return True
 
 
 def send_thread(joystick):
   pm = messaging.PubMaster(['testJoystick'])
-
   rk = Ratekeeper(100, print_delay_threshold=None)
 
   while True:
@@ -105,7 +173,6 @@ def send_thread(joystick):
     joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order]
 
     pm.send('testJoystick', joystick_msg)
-
     rk.keep_time()
 
 
@@ -117,13 +184,12 @@ def joystick_control_thread(joystick):
 
 
 def main():
-  joystick_control_thread(Joystick())
+  joystick_control_thread(PyGameJoystick())
-
 
 if __name__ == '__main__':
   parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' +
                                                'openpilot must be offroad before starting joystick_control. This tool supports ' +
-                                               'a PlayStation 5 DualSense controller on the comma 3X.',
+                                               'PlayStation and Xbox controllers on various platforms.',
                                    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
   parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
   args = parser.parse_args()
@@ -139,9 +205,12 @@ if __name__ == '__main__':
     print('Buttons')
     print('- `R`: Resets axes')
     print('- `C`: Cancel cruise control')
+    joystick_control_thread(Keyboard())
   else:
-    print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!')
+    print('Using pygame joystick')
-    print('If not running on a comma device, the mapping may need to be adjusted.')
+    print('Standard controller mapping:')
-
+    print('- Left stick: Steering')
-  joystick = Keyboard() if args.keyboard else Joystick()
+    print('- Right trigger (R2): Gas')
-  joystick_control_thread(joystick)
+    print('- Left trigger (L2): Brake')
+    print('- Triangle/Y button: Cancel')
+    joystick_control_thread(PyGameJoystick())

tools/joystick/joystickd.py

@@ -19,18 +19,24 @@ def joystickd_thread():
   CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   VM = VehicleModel(CP)
 
-  sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL)
+  sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick', 'selfdriveStateSP'], frequency=1. / DT_CTRL)
   pm = messaging.PubMaster(['carControl', 'controlsState'])
 
   rk = Ratekeeper(100, print_delay_threshold=None)
   while 1:
     sm.update(0)
+    ss_sp = sm['selfdriveStateSP']
+    _lat_active = False
+    if ss_sp.mads.available:
+      _lat_active = ss_sp.mads.active
+    else:
+      _lat_active = sm['selfdriveState'].active
 
     cc_msg = messaging.new_message('carControl')
     cc_msg.valid = True
     CC = cc_msg.carControl
     CC.enabled = sm['selfdriveState'].enabled
-    CC.latActive = sm['selfdriveState'].active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
+    CC.latActive = _lat_active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
     CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in sm['onroadEvents']) and CP.openpilotLongitudinalControl
     CC.cruiseControl.cancel = sm['carState'].cruiseState.enabled and (not CC.enabled or not CP.pcmCruise)
     CC.hudControl.leadDistanceBars = 2

tools/plotjuggler/layouts/analyzing-panda-block-angle-hkg.xml

@@ -0,0 +1,131 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="actuator data" containers="1">
+   <Container>
+    <DockSplitter sizes="0.25;0.25;0.25;0.25" count="4" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-1.050000" top="1.050000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#17becf" name="/carControl/actuators/torque"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-385.132391" top="536.899115" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/carControl/actuators/steeringAngleDeg"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.364800" top="14.956815" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="/carState/vEgoRaw"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.208517" top="0.149191" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/carControl/actuators/curvature"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="steering messages (CAN)" containers="1">
+   <Container>
+    <DockSplitter sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-189.000000" top="189.000000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1ac938" name="/can/2/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#9467bd" name="/can/128/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#ff0e1c" name="/can/192/LKAS_ALT/LKAS_ANGLE_CMD"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-6.200000" top="254.200000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/can/128/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.025000" top="1.025000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#d62728" name="/carState/steeringPressed"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="460.325000" top="1874.675000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/pandaStates/0/safetyTxBlocked"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.025000" top="1.025000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/can/1/CCNC_0x161/DAW_ICON"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="Understanding Torque" containers="1">
+   <Container>
+    <DockSplitter sizes="0.5;0.5" count="2" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-6.175000" top="253.175000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-41.182501" top="42.082498" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1ac938" name="/carState/steeringTorqueEps"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="1"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

tools/plotjuggler/layouts/analyzing-torque-angle-hkg.xml

@@ -0,0 +1,209 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="actuator data" containers="1">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.25;0.25;0.25;0.25" count="4">
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-1.050000" top="1.050000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#17becf" name="/carControl/actuators/torque"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-453.043646" top="538.555487" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/carControl/actuators/steeringAngleDeg"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.647461" top="26.545898" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="/carState/vEgoRaw"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.209157" top="0.175448" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/carControl/actuators/curvature"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="steering messages (CAN)" containers="1">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5">
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-189.000000" top="189.000000" right="723.115449" left="6.371879"/>
+       <limitY/>
+       <curve color="#b6ff00" name="/can/128/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#00fab2" name="/can/0/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#d62728" name="/can/192/LKAS_ALT/LKAS_ANGLE_CMD"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-6.250000" top="256.250000" right="722.420439" left="6.497277"/>
+       <limitY/>
+       <curve color="#00ffe8" name="/can/128/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+       <curve color="#e5fd00" name="/can/0/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-4.399729" top="4.056632" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="IMU_LatAccelVal_ms^2_roll_compensated"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.025000" top="1.025000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#d62728" name="/carState/steeringPressed"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.025000" top="1.025000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/can/1/CCNC_0x161/DAW_ICON"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="Understanding Torque" containers="1">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.5;0.5" count="2">
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-6.250000" top="256.250000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-42.062501" top="78.162503" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#1ac938" name="/carState/steeringTorqueEps"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="1"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default delimiter="0" time_axis=""/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations>
+  <snippet name="Desired lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/desiredCurvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="Actual lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_ms^2_roll_compensated">
+   <global></global>
+   <function>if (v1 == 0 and v2 == 1) then
+  return  (value * -9.8) - (v3 * 9.81)
+end
+--return 0
+  return  (value * -9.8) - (v3 * 9.81)</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+    <v3>/liveParameters/roll</v3>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^3">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^2">
+   <global></global>
+   <function>if (v1 == 0 and v2 == 1) then
+  return  value * -9.8
+end
+return 0</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="roll compensated lateral acceleration">
+   <global></global>
+   <function>if (v3 == 0 and v4 == 1) then
+  return  (value * v1 ^ 2) - (v2 * 9.81)
+end
+return 0</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+    <v3>/carState/steeringPressed</v3>
+    <v4>/carControl/latActive</v4>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_ms^2">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+ </customMathEquations>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

tools/plotjuggler/layouts/hkg_angle_control.xml

@@ -0,0 +1,281 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget name="Main Window" parent="main_window">
+  <Tab containers="1" tab_name="tab1">
+   <Container>
+    <DockSplitter sizes="0.500397;0.499603" count="2" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" mode="TimeSeries" style="Lines">
+       <range top="256.250000" left="0.000000" right="421.102293" bottom="-6.250000"/>
+       <limitY/>
+       <curve color="#1ac938" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+       <curve color="#17becf" name="max torque(calc)">
+        <transform name="Moving Average" alias="max torque(calc)[Moving Average]">
+         <options compensate_offset="true" value="10"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" mode="TimeSeries" style="Lines">
+       <range top="2.776497" left="0.000000" right="421.102293" bottom="-2.918548"/>
+       <limitY/>
+       <curve color="#f14cc1" name="desired lat accel"/>
+       <curve color="#888888" name="zero"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles>
+  <fileInfo prefix="" filename="../tmpa_e8kwpj.rlog">
+   <selected_datasources value=""/>
+  </fileInfo>
+ </previouslyLoaded_Datafiles>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations>
+  <snippet name="zero">
+   <global>min=0
+max=250
+max_from_speed=96
+
+rate_lim = 500
+
+la_deadzone = 0.38
+
+k1=200
+k2=20
+k3=1.0
+k4=1
+k5=10
+
+old = 0
+
+function sign(number)
+    return number > 0 and 1 or (number == 0 and 0 or -1)
+end
+
+function apply_rate_limit(old, new, limit)
+    return math.min(math.max(new, old - limit), old + limit)
+end
+
+function apply_deadzone(val, deadzone)
+    if math.abs(val) &lt;= deadzone then
+        return 0.0
+    elseif val &lt; 0.0 then
+        return val + deadzone
+    else
+        return val - deadzone
+    end
+end</global>
+   <function>return 0</function>
+   <linked_source>/carState/aEgo</linked_source>
+  </snippet>
+  <snippet name="max torque lj adj">
+   <global>min=0
+max=250
+max_from_speed=96
+
+k1=200
+k2=30
+k3=1    
+k4=1
+k5=10
+
+function sign(number)
+    return number > 0 and 1 or (number == 0 and 0 or -1)
+end</global>
+   <function>return 250 - value * 20</function>
+   <linked_source>desired lateral jark</linked_source>
+  </snippet>
+  <snippet name="ang_cmd rate">
+   <global>firstX = 0
+firstY = 0
+is_first = true
+secondX = 0
+secondY = 0
+is_second = false</global>
+   <function>-- Wait for initial values
+if (is_first) then
+  is_first = false
+  is_second = true
+  firstX = time
+  firstY = value
+end
+
+if (is_second) then
+  is_second = false
+  secondX = time
+  secondY = value
+end
+
+-- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x)
+dx = time - firstX
+dy = value - firstY
+-- Increment
+firstX = secondX
+firstY = secondY
+secondX = time
+secondY = value
+
+return dy/dx</function>
+   <linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
+  </snippet>
+  <snippet name="max torque(calc)">
+   <global>min=0
+max=250
+max_from_speed=96
+
+rate_lim = 500
+
+la_deadzone = 0.38
+
+k1=200
+k2=20
+k3=1.0
+k4=1
+k5=10
+
+old = 0
+
+function sign(number)
+    return number > 0 and 1 or (number == 0 and 0 or -1)
+end
+
+function apply_rate_limit(old, new, limit)
+    return math.min(math.max(new, old - limit), old + limit)
+end
+
+function apply_deadzone(val, deadzone)
+    if math.abs(val) &lt;= deadzone then
+        return 0.0
+    elseif val &lt; 0.0 then
+        return val + deadzone
+    else
+        return val - deadzone
+    end
+end</global>
+   <function>la = apply_deadzone(v2, la_deadzone)
+lj = v3
+
+if la == 0.0 then
+    lj = 0.0
+end
+
+fla = math.min(math.abs(k1 * la)^k3, max)
+flj = math.min(math.abs(k2 * lj)^k4, max)
+
+out = fla
+
+flv = math.min(max_from_speed, k5 * v4)
+
+out = out + flv
+
+out = math.max(math.min(out, max), min)
+
+if sign(la) == sign(lj) then
+    out = out - flj
+else
+    out = out + flj
+end
+
+
+if v5 == 1.0 then
+    out = 0.0
+end
+
+out = math.max(math.min(out, max), min)
+out = apply_rate_limit(old, out, rate_lim)
+old = out
+
+return out</function>
+   <linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
+   <additional_sources>
+    <v1>ang_cmd rate</v1>
+    <v2>desired lat accel</v2>
+    <v3>desired lateral jark</v3>
+    <v4>/carState/vEgo</v4>
+    <v5>/can/1/LFA_ALT/LKAS_ANGLE_ACTIVE</v5>
+   </additional_sources>
+  </snippet>
+  <snippet name="desired lateral jark">
+   <global>firstX = 0
+firstY = 0
+is_first = true
+secondX = 0
+secondY = 0
+is_second = false</global>
+   <function>-- Wait for initial values
+if (is_first) then
+  is_first = false
+  is_second = true
+  firstX = time
+  firstY = value
+end
+
+if (is_second) then
+  is_second = false
+  secondX = time
+  secondY = value
+end
+
+-- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x)
+dx = time - firstX
+dy = value - firstY
+-- Increment
+firstX = secondX
+firstY = secondY
+secondX = time
+secondY = value
+
+return dy/dx</function>
+   <linked_source>desired lat accel</linked_source>
+  </snippet>
+  <snippet name="abs(des la accel)">
+   <global></global>
+   <function>return math.abs(value)</function>
+   <linked_source>desired lat accel</linked_source>
+  </snippet>
+  <snippet name="desired lat accel">
+   <global></global>
+   <function>return value * v1^2</function>
+   <linked_source>/controlsState/desiredCurvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+   </additional_sources>
+  </snippet>
+  <snippet name="abs(ang_cmd)">
+   <global></global>
+   <function>return math.abs(value)</function>
+   <linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
+  </snippet>
+ </customMathEquations>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

tools/plotjuggler/layouts/safety-limits-angle-kkg.xml

@@ -0,0 +1,175 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="tab1" containers="1">
+   <Container>
+    <DockSplitter sizes="0.25;0.25;0.25;0.25" count="4" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-3.582051" right="1431.113121" top="5.314632" left="5.322399"/>
+       <limitY/>
+       <curve name="Actual lateral accel (roll compensated)" color="#1ac938"/>
+       <curve name="Desired lateral accel (roll compensated)" color="#ff7f0e"/>
+       <curve name="IMU_LatAccelVal_ms^2" color="#f14cc1"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-3.741271" right="1431.113121" top="3.756006" left="5.322399"/>
+       <limitY/>
+       <curve name="roll compensated lateral acceleration" color="#ff7f0e"/>
+       <curve name="IMU_LatAccelVal_Ms^2" color="#1ac938"/>
+       <curve name="IMU_LatAccelVal_ms^2_roll_compensated" color="#9467bd"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-0.025000" right="1431.113121" top="1.025000" left="5.322399"/>
+       <limitY/>
+       <curve name="/carState/steeringPressed" color="#0097ff"/>
+       <curve name="/carOutput/actuatorsOutput/torque" color="#d62728"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-1.660728" right="1431.113121" top="67.942958" left="5.322399"/>
+       <limitY/>
+       <curve name="/carState/vEgo" color="#f14cc1">
+        <transform name="Scale/Offset" alias="/carState/vEgo[Scale/Offset]">
+         <options value_scale="2.23694" time_offset="0" value_offset="0"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="tab2" containers="1">
+   <Container>
+    <DockSplitter sizes="0.5;0.5" count="2" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="30595.900000" right="1431.113121" top="34884.100000" left="5.322399"/>
+       <limitY/>
+       <curve name="/can/1/IMU_01_10ms/IMU_RollRtVal" color="#17becf"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-0.058795" right="1431.113121" top="0.072448" left="5.322399"/>
+       <limitY/>
+       <curve name="/liveParameters/roll" color="#bcbd22"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="1"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations>
+  <snippet name="IMU_LatAccelVal_ms^2">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="roll compensated lateral acceleration">
+   <global></global>
+   <function>if (v3 == 0 and v4 == 1) then
+  return  (value * v1 ^ 2) - (v2 * 9.81)
+end
+return 0</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+    <v3>/carState/steeringPressed</v3>
+    <v4>/carControl/latActive</v4>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^2">
+   <global></global>
+   <function>if (v1 == 0 and v2 == 1) then
+  return  value * -9.8
+end
+return 0</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^3">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_ms^2_roll_compensated">
+   <global></global>
+   <function>
+
+if (v1 == 0 and v2 == 1) then
+  return  (value * -9.8) - (v3 * 9.81)
+end
+return 0</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+    <v3>/liveParameters/roll</v3>
+   </additional_sources>
+  </snippet>
+  <snippet name="Actual lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="Desired lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/desiredCurvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+ </customMathEquations>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+