Torque Lateral Control: customized settings (#1314)

* init

* use internal frame

* exit early if toggle not enabled

* ui init

* ui: replace `static_cast<int>` with `std::nearbyint` for precise rounding

* revert

* update title

* handle live relaxed

* fix

* tweak ui

* toggle behaviors

* lint

* Update torqued_ext.py

* always update

* make sure it's updated properly with offroad states

* fix

* make sure to initialize

---------

Co-authored-by: nayan <nayan8teen@gmail.com>

common/params_keys.h

@@ -242,4 +242,13 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"MapTargetVelocities", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
     {"SmartCruiseControlMap", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"SmartCruiseControlVision", {PERSISTENT | BACKUP, BOOL, "0"}},
+
+    // Torque lateral control custom params
+    {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}},
+    {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}},
+    {"LiveTorqueParamsToggle", {PERSISTENT | BACKUP , BOOL}},
+    {"LiveTorqueParamsRelaxedToggle", {PERSISTENT | BACKUP , BOOL}},
+    {"TorqueParamsOverrideEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"TorqueParamsOverrideFriction", {PERSISTENT | BACKUP, FLOAT, "0.1"}},
+    {"TorqueParamsOverrideLatAccelFactor", {PERSISTENT | BACKUP, FLOAT, "2.5"}},
 };

selfdrive/controls/lib/latcontrol_torque.py

@@ -48,6 +48,10 @@ class LatControlTorque(LatControl):
                         self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
 
   def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited):
+    # Override torque params from extension
+    if self.extension.update_override_torque_params(self.torque_params):
+      self.update_limits()
+
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     if not active:
       output_torque = 0.0

selfdrive/locationd/torqued.py

@@ -11,6 +11,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.selfdrive.locationd.torqued_ext import TorqueEstimatorExt
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -50,9 +51,10 @@ class TorqueBuckets(PointBuckets):
         break
 
 
-class TorqueEstimator(ParameterEstimator):
+class TorqueEstimator(ParameterEstimator, TorqueEstimatorExt):
   def __init__(self, CP, decimated=False, track_all_points=False):
-    super().__init__()
+    ParameterEstimator.__init__(self)
+    TorqueEstimatorExt.__init__(self, CP)
     self.CP = CP
     self.hist_len = int(HISTORY / DT_MDL)
     self.lag = 0.0
@@ -82,6 +84,8 @@ class TorqueEstimator(ParameterEstimator):
 
     self.calibrator = PoseCalibrator()
 
+    TorqueEstimatorExt.initialize_custom_params(self, decimated)
+
     self.reset()
 
     initial_params = {
@@ -260,6 +264,8 @@ def main(demo=False):
           t = sm.logMonoTime[which] * 1e-9
           estimator.handle_log(t, which, sm[which])
 
+    TorqueEstimatorExt.update_use_params(estimator)
+
     # 4Hz driven by livePose
     if sm.frame % 5 == 0:
       pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))

selfdrive/ui/sunnypilot/SConscript

@@ -51,6 +51,8 @@ lateral_panel_qt_src = [
   "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
+  "sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc",
+  "sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc",
 ]
 
 longitudinal_panel_qt_src = [

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

@@ -10,14 +10,15 @@
 NeuralNetworkLateralControl::NeuralNetworkLateralControl() :
   ParamControl("NeuralNetworkLateralControl", tr("Neural Network Lateral Control (NNLC)"),  "", "") {
   setConfirmation(true, false);
-  updateToggle();
+  updateToggle(offroad);
 }
 
-void NeuralNetworkLateralControl::updateToggle() {
+void NeuralNetworkLateralControl::updateToggle(bool _offroad) {
   QString statusInitText = "<font color='yellow'>" + STATUS_CHECK_COMPATIBILITY + "</font>";
   QString notLoadedText = "<font color='yellow'>" + STATUS_NOT_LOADED + "</font>";
   QString loadedText = "<font color=#00ff00>" + STATUS_LOADED + "</font>";
 
+  bool allowed = true;
   auto cp_bytes = params.get("CarParamsPersistent");
   auto cp_sp_bytes = params.get("CarParamsSPPersistent");
   if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
@@ -31,7 +32,7 @@ void NeuralNetworkLateralControl::updateToggle() {
     if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) {
       params.remove("NeuralNetworkLateralControl");
       setDescription(nnffDescriptionBuilder(STATUS_NOT_AVAILABLE));
-      setEnabled(false);
+      allowed = false;
     } else {
       QString nn_model_name = QString::fromStdString(CP_SP.getNeuralNetworkLateralControl().getModel().getName());
       QString nn_fuzzy = CP_SP.getNeuralNetworkLateralControl().getFuzzyFingerprint() ?
@@ -56,4 +57,11 @@ void NeuralNetworkLateralControl::updateToggle() {
   if (getDescription() != getBaseDescription()) {
     showDescription();
   }
+
+  bool enforce_torque_toggle = params.getBool("EnforceTorqueControl");
+  setEnabled(_offroad && allowed && !enforce_torque_toggle);
+
+  refresh();
+
+  offroad = _offroad;
 }

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

@@ -20,10 +20,11 @@ public:
   NeuralNetworkLateralControl();
 
 public slots:
-  void updateToggle();
+  void updateToggle(bool _offroad);
 
 private:
   Params params;
+  bool offroad;
 
   // Status messages
   const QString STATUS_NOT_AVAILABLE = tr("NNLC is currently not available on this platform.");

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

@@ -0,0 +1,58 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h"
+
+TorqueLateralControlCustomParams::TorqueLateralControlCustomParams(const QString &param, const QString &title, const QString &description, const QString &icon, QWidget *parent)
+    : ExpandableToggleRow(param, title, description, icon, parent) {
+
+  QFrame *frame = new QFrame(this);
+  QGridLayout *frame_layout = new QGridLayout();
+  frame->setLayout(frame_layout);
+  frame_layout->setSpacing(0);
+
+  torqueLateralControlParamsOverride = new ParamControl(
+    "TorqueParamsOverrideEnabled",
+    tr("Manual Real-Time Tuning"),
+    tr("Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values."),
+    "../assets/offroad/icon_blank.png",
+    this
+  );
+  connect(torqueLateralControlParamsOverride, &ParamControl::toggleFlipped, this, &TorqueLateralControlCustomParams::refresh);
+
+  torqueParamsOverrideLatAccelFactor = new OptionControlSP("TorqueParamsOverrideLatAccelFactor", tr("Lateral Acceleration Factor"), "", "", {1, 500}, 1, false, nullptr, true, false);
+  connect(torqueParamsOverrideLatAccelFactor, &OptionControlSP::updateLabels, this, &TorqueLateralControlCustomParams::refresh);
+  torqueParamsOverrideLatAccelFactor->setFixedWidth(280);
+
+  torqueParamsOverrideFriction = new OptionControlSP("TorqueParamsOverrideFriction", tr("Friction"), "", "", {1, 100}, 1, false, nullptr, true, false);
+  connect(torqueParamsOverrideFriction, &OptionControlSP::updateLabels, this, &TorqueLateralControlCustomParams::refresh);
+  torqueParamsOverrideFriction->setFixedWidth(280);
+
+  frame_layout->addWidget(torqueLateralControlParamsOverride, 0, 0, 1, 2);
+  QSpacerItem *spacer = new QSpacerItem(20, 40);
+  frame_layout->addItem(spacer, 1, 0, 1, 2);
+  frame_layout->addWidget(torqueParamsOverrideLatAccelFactor, 2, 0, Qt::AlignCenter);
+  frame_layout->addWidget(torqueParamsOverrideFriction, 2, 1, Qt::AlignCenter);
+
+  addItem(frame);
+
+  refresh();
+}
+
+void TorqueLateralControlCustomParams::refresh() {
+  bool torque_override_param = params.getBool("TorqueParamsOverrideEnabled");
+  float laf_param = QString::fromStdString(params.get("TorqueParamsOverrideLatAccelFactor")).toFloat();
+  const QString laf_unit = "m/s²";
+
+  float friction_param = QString::fromStdString(params.get("TorqueParamsOverrideFriction")).toFloat();
+
+  torqueParamsOverrideLatAccelFactor->setTitle(tr("Lateral Acceleration Factor") + "\n(" + (torque_override_param ? tr("Real-time and Offline") : tr("Offline Only")) + ")");
+  torqueParamsOverrideFriction->setTitle(tr("Friction") + "\n(" + (torque_override_param ? tr("Real-time and Offline") : tr("Offline Only")) + ")");
+
+  torqueParamsOverrideLatAccelFactor->setLabel(QString::number(laf_param, 'f', 2) + " " + laf_unit);
+  torqueParamsOverrideFriction->setLabel(QString::number(friction_param, 'f', 2));
+}

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

@@ -0,0 +1,27 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
+
+class TorqueLateralControlCustomParams : public ExpandableToggleRow {
+  Q_OBJECT
+
+public:
+  TorqueLateralControlCustomParams(const QString &param, const QString &title, const QString &description, const QString &icon, QWidget *parent = nullptr);
+  void refresh();
+
+private:
+  Params params;
+  ParamControl *torqueLateralControlParamsOverride;
+  OptionControlSP *torqueParamsOverrideFriction;
+  OptionControlSP *torqueParamsOverrideLatAccelFactor;
+};

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

@@ -0,0 +1,81 @@
+/**
+ * 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/torque_lateral_control_settings.h"
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
+
+TorqueLateralControlSettings::TorqueLateralControlSettings(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);
+
+  ListWidget *list = new ListWidget(this, false);
+  // param, title, desc, icon
+  std::vector<std::tuple<QString, QString, QString, QString>> toggle_defs{
+    {
+      "LiveTorqueParamsToggle",
+      tr("Self-Tune"),
+      tr("Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default."),
+      "../assets/offroad/icon_blank.png",
+    },
+    {
+      "LiveTorqueParamsRelaxedToggle",
+      tr("Less Restrict Settings for Self-Tune (Beta)"),
+      tr("Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values."),
+      "../assets/offroad/icon_blank.png",
+    }
+  };
+
+  for (auto &[param, title, desc, icon] : toggle_defs) {
+    auto toggle = new ParamControlSP(param, title, desc, icon, this);
+    list->addItem(toggle);
+    toggles[param.toStdString()] = toggle;
+  }
+
+  torqueLateralControlCustomParams = new TorqueLateralControlCustomParams(
+    "CustomTorqueParams",
+    tr("Enable Custom Tuning"),
+    tr("Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within \"opendbc/car/torque_data\". "
+       "The values will also be used live when \"Manual Real-Time Tuning\" toggle is enabled."),
+    "../assets/offroad/icon_blank.png",
+    this);
+  list->addItem(torqueLateralControlCustomParams);
+
+  QObject::connect(uiState(), &UIState::offroadTransition, this, &TorqueLateralControlSettings::updateToggles);
+  QObject::connect(toggles["LiveTorqueParamsToggle"], &ParamControlSP::toggleFlipped, [=](bool state) {
+    if (!state) {
+      params.remove("LiveTorqueParamsRelaxedToggle");
+      toggles["LiveTorqueParamsRelaxedToggle"]->refresh();
+    }
+
+    updateToggles(offroad);
+  });
+
+  main_layout->addWidget(new ScrollViewSP(list, this));
+}
+
+void TorqueLateralControlSettings::showEvent(QShowEvent *event) {
+  updateToggles(offroad);
+}
+
+void TorqueLateralControlSettings::updateToggles(bool _offroad) {
+  bool live_toggle = toggles["LiveTorqueParamsToggle"]->isToggled();
+
+  toggles["LiveTorqueParamsToggle"]->setEnabled(_offroad);
+  toggles["LiveTorqueParamsRelaxedToggle"]->setEnabled(_offroad && live_toggle);
+
+  torqueLateralControlCustomParams->setEnabled(_offroad);
+  torqueLateralControlCustomParams->refresh();
+
+  offroad = _offroad;
+}

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

@@ -0,0 +1,36 @@
+/**
+ * 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/lateral/torque_lateral_control_custom_params.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+
+class TorqueLateralControlSettings : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit TorqueLateralControlSettings(QWidget *parent = nullptr);
+
+  void showEvent(QShowEvent *event) override;
+
+  signals:
+    void backPress();
+
+public slots:
+  void updateToggles(bool _offroad);
+
+private:
+  Params params;
+  bool offroad;
+  std::map<std::string, ParamControlSP*> toggles;
+
+  TorqueLateralControlCustomParams *torqueLateralControlCustomParams;
+};

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

@@ -75,6 +75,36 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
 
   list->addItem(horizontal_line());
 
+  // Customized Torque Lateral Control
+  torqueLateralControlToggle = new ParamControl(
+    "EnforceTorqueControl",
+    tr("Enforce Torque Lateral Control"),
+    tr("Enable this to enforce sunnypilot to steer with Torque lateral control."),
+    "");
+  list->addItem(torqueLateralControlToggle);
+
+  torqueLateralControlSettingsButton = new PushButtonSP(tr("Customize Params"));
+  torqueLateralControlSettingsButton->setObjectName("torque_btn");
+  connect(torqueLateralControlSettingsButton, &QPushButton::clicked, [=]() {
+    sunnypilotScroller->setLastScrollPosition();
+    main_layout->setCurrentWidget(torqueLateralControlWidget);
+  });
+  QObject::connect(torqueLateralControlToggle, &ToggleControl::toggleFlipped, [=](bool state) {
+    torqueLateralControlSettingsButton->setEnabled(state);
+    nnlcToggle->updateToggle(offroad);
+    updateToggles(offroad);
+  });
+
+  torqueLateralControlWidget = new TorqueLateralControlSettings(this);
+  connect(torqueLateralControlWidget, &TorqueLateralControlSettings::backPress, [=]() {
+    sunnypilotScroller->restoreScrollPosition();
+    main_layout->setCurrentWidget(sunnypilotScreen);
+  });
+  list->addItem(torqueLateralControlSettingsButton);
+
+  list->addItem(vertical_space(0));
+  list->addItem(horizontal_line());
+
   // Neural Network Lateral Control
   nnlcToggle = new NeuralNetworkLateralControl();
   list->addItem(nnlcToggle);
@@ -86,12 +116,10 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
       nnlcToggle->hideDescription();
     }
 
-    nnlcToggle->updateToggle();
+    nnlcToggle->updateToggle(offroad);
+    updateToggles(offroad);
   });
 
-  toggleOffroadOnly = {
-    madsToggle, nnlcToggle,
-  };
   QObject::connect(uiState(), &UIState::offroadTransition, this, &LateralPanel::updateToggles);
 
   sunnypilotScroller = new ScrollViewSP(list, this);
@@ -100,6 +128,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
   main_layout->addWidget(sunnypilotScreen);
   main_layout->addWidget(madsWidget);
   main_layout->addWidget(laneChangeWidget);
+  main_layout->addWidget(torqueLateralControlWidget);
 
   setStyleSheet(R"(
     #back_btn {
@@ -120,7 +149,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
 }
 
 void LateralPanel::showEvent(QShowEvent *event) {
-  nnlcToggle->updateToggle();
+  nnlcToggle->updateToggle(offroad);
   updateToggles(offroad);
 }
 
@@ -129,10 +158,7 @@ void LateralPanel::hideEvent(QHideEvent *event) {
 }
 
 void LateralPanel::updateToggles(bool _offroad) {
-  for (auto *toggle : toggleOffroadOnly) {
-    toggle->setEnabled(_offroad);
-  }
-
+  bool torque_allowed = true;
   auto cp_bytes = params.get("CarParamsPersistent");
   auto cp_sp_bytes = params.get("CarParamsSPPersistent");
   if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
@@ -148,12 +174,24 @@ void LateralPanel::updateToggles(bool _offroad) {
     } else {
       madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_FULL_COMPATIBILITY, MADS_BASE_DESC));
     }
+
+    if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) {
+      params.remove("EnforceTorqueControl");
+      torque_allowed = false;
+    }
   } else {
     madsToggle->setDescription(descriptionBuilder(STATUS_MADS_CHECK_COMPATIBILITY, MADS_BASE_DESC));
+
+    params.remove("EnforceTorqueControl");
+    torque_allowed = false;
   }
 
+  madsToggle->setEnabled(_offroad);
   madsSettingsButton->setEnabled(madsToggle->isToggled());
 
+  torqueLateralControlToggle->setEnabled(_offroad && torque_allowed && !nnlcToggle->isToggled());
+  torqueLateralControlSettingsButton->setEnabled(torqueLateralControlToggle->isToggled());
+
   blinkerPauseLateralSettings->refresh();
 
   offroad = _offroad;

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

@@ -15,6 +15,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_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/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
@@ -35,7 +36,6 @@ private:
   QStackedLayout* main_layout = nullptr;
   QWidget* sunnypilotScreen = nullptr;
   ScrollViewSP *sunnypilotScroller = nullptr;
-  std::vector<ParamControl *> toggleOffroadOnly;
   bool offroad;
 
   ParamControl *madsToggle;
@@ -45,6 +45,9 @@ private:
   LaneChangeSettings *laneChangeWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;
   BlinkerPauseLateralSettings *blinkerPauseLateralSettings = nullptr;
+  ParamControl *torqueLateralControlToggle;
+  PushButtonSP *torqueLateralControlSettingsButton;
+  TorqueLateralControlSettings *torqueLateralControlWidget = nullptr;
 
   const QString MADS_BASE_DESC = tr("Enables independent engagements of Automatic Lane Centering (ALC) and Adaptive Cruise Control (ACC).");
 

sunnypilot/selfdrive/car/interfaces.py

@@ -22,8 +22,18 @@ def log_fingerprint(CP: structs.CarParams) -> None:
     sentry.capture_fingerprint(CP.carFingerprint, CP.brand)
 
 
-def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
-                                               params: Params = None, enabled: bool = False) -> None:
+def _enforce_torque_lateral_control(CP: structs.CarParams, params: Params = None, enabled: bool = False) -> bool:
+  if params is None:
+    params = Params()
+
+  if CP.steerControlType != structs.CarParams.SteerControlType.angle:
+    enabled = params.get_bool("EnforceTorqueControl")
+
+  return enabled
+
+
+def _initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
+                                               params: Params = None, enabled: bool = False) -> bool:
   if params is None:
     params = Params()
 
@@ -35,13 +45,12 @@ def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs
   if nnlc_model_name != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle:
     enabled = params.get_bool("NeuralNetworkLateralControl")
 
-  if enabled:
-    CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
-
   CP_SP.neuralNetworkLateralControl.model.path = nnlc_model_path
   CP_SP.neuralNetworkLateralControl.model.name = nnlc_model_name
   CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match
 
+  return enabled
+
 
 def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
   if params is None:
@@ -52,12 +61,19 @@ def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_S
     CP_SP.pcmCruiseSpeed = False
 
 
+def _initialize_torque_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, enforce_torque: bool, nnlc_enabled: bool) -> None:
+  if nnlc_enabled or enforce_torque:
+    CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
+
+
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
   CP = CI.CP
   CP_SP = CI.CP_SP
 
-  _initialize_neural_network_lateral_control(CI, CP, CP_SP, params)
+  enforce_torque = _enforce_torque_lateral_control(CP, params)
+  nnlc_enabled = _initialize_neural_network_lateral_control(CP, CP_SP, params)
   _initialize_intelligent_cruise_button_management(CP, CP_SP, params)
+  _initialize_torque_lateral_control(CI, CP, enforce_torque, nnlc_enabled)
 
 
 def initialize_params(params) -> list[dict[str, Any]]:

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py

@@ -6,11 +6,13 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_override import LatControlTorqueExtOverride
 
 
-class LatControlTorqueExt(NeuralNetworkLateralControl):
+class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverride):
   def __init__(self, lac_torque, CP, CP_SP, CI):
-    super().__init__(lac_torque, CP, CP_SP, CI)
+    NeuralNetworkLateralControl.__init__(self, lac_torque, CP, CP_SP, CI)
+    LatControlTorqueExtOverride.__init__(self, CP)
 
   def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
              desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py

@@ -0,0 +1,34 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.common.params import Params
+
+
+class LatControlTorqueExtOverride:
+  def __init__(self, CP):
+    self.CP = CP
+    self.params = Params()
+    self.enforce_torque_control_toggle = self.params.get_bool("EnforceTorqueControl")  # only during init
+    self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
+    self.frame = -1
+
+  def update_override_torque_params(self, torque_params) -> bool:
+    if not self.enforce_torque_control_toggle:
+      return False
+
+    self.frame += 1
+    if self.frame % 300 == 0:
+      self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
+
+      if not self.torque_override_enabled:
+        return False
+
+      torque_params.latAccelFactor = float(self.params.get("TorqueParamsOverrideLatAccelFactor", return_default=True))
+      torque_params.friction = float(self.params.get("TorqueParamsOverrideFriction", return_default=True))
+      return True
+
+    return False

sunnypilot/selfdrive/locationd/torqued_ext.py

@@ -0,0 +1,65 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import numpy as np
+
+from cereal import car
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+
+RELAXED_MIN_BUCKET_POINTS = np.array([0, 200, 300, 500, 500, 300, 200, 0])
+
+ALLOWED_CARS = ['toyota', 'hyundai', 'rivian', 'honda']
+
+
+
+class TorqueEstimatorExt:
+  def __init__(self, CP: car.CarParams):
+    self.CP = CP
+    self._params = Params()
+    self.frame = -1
+
+    self.enforce_torque_control_toggle = self._params.get_bool("EnforceTorqueControl")  # only during init
+    self.use_params = self.CP.brand in ALLOWED_CARS and self.CP.lateralTuning.which() == 'torque'
+    self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle")
+    self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled")
+    self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS
+    self.factor_sanity = 0.0
+    self.friction_sanity = 0.0
+    self.offline_latAccelFactor = 0.0
+    self.offline_friction = 0.0
+
+  def initialize_custom_params(self, decimated=False):
+    self.update_use_params()
+
+    if self.enforce_torque_control_toggle:
+      if self._params.get_bool("LiveTorqueParamsRelaxedToggle"):
+        self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS / (10 if decimated else 1)
+        self.factor_sanity = 0.5 if decimated else 1.0
+        self.friction_sanity = 0.8 if decimated else 1.0
+
+      if self._params.get_bool("CustomTorqueParams"):
+        self.offline_latAccelFactor = float(self._params.get("TorqueParamsOverrideLatAccelFactor", return_default=True))
+        self.offline_friction = float(self._params.get("TorqueParamsOverrideFriction", return_default=True))
+
+  def _update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle")
+      self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled")
+
+  def update_use_params(self):
+    self._update_params()
+
+    if self.enforce_torque_control_toggle:
+      if self.torque_override_enabled:
+        self.use_params = False
+      else:
+        self.use_params = self.use_live_torque_params
+
+    self.frame += 1