diff --git a/common/params_keys.h b/common/params_keys.h index 0844fff2b..11ec8da63 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -242,4 +242,13 @@ inline static std::unordered_map 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"}}, }; diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index ca9c3fde6..917fb233f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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 diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index e0409957f..57d53fdee 100755 --- a/selfdrive/locationd/torqued.py +++ b/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())) diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 3f0e013d3..12c09c53f 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/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 = [ diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc index 0b66b8366..02ab810c5 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc +++ b/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 = "" + STATUS_CHECK_COMPATIBILITY + ""; QString notLoadedText = "" + STATUS_NOT_LOADED + ""; QString loadedText = "" + STATUS_LOADED + ""; + 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; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h index df80fbf09..9e872b11e 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h +++ b/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."); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc new file mode 100644 index 000000000..894fbc2ec --- /dev/null +++ b/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 ¶m, 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)); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h new file mode 100644 index 000000000..8442bd0d4 --- /dev/null +++ b/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 ¶m, const QString &title, const QString &description, const QString &icon, QWidget *parent = nullptr); + void refresh(); + +private: + Params params; + ParamControl *torqueLateralControlParamsOverride; + OptionControlSP *torqueParamsOverrideFriction; + OptionControlSP *torqueParamsOverrideLatAccelFactor; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc new file mode 100644 index 000000000..914ce7a93 --- /dev/null +++ b/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> 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; +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h new file mode 100644 index 000000000..6254073df --- /dev/null +++ b/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 toggles; + + TorqueLateralControlCustomParams *torqueLateralControlCustomParams; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index 59636ca5a..ac004e885 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/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; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h index 7b5e73fce..88c4d401d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h +++ b/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 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)."); diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 55244caa5..ed0c11fb1 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/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]]: diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py index 6e134276e..39525b3b8 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py +++ b/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, diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py new file mode 100644 index 000000000..f07a7292c --- /dev/null +++ b/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 diff --git a/sunnypilot/selfdrive/locationd/torqued_ext.py b/sunnypilot/selfdrive/locationd/torqued_ext.py new file mode 100644 index 000000000..084a82e94 --- /dev/null +++ b/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