diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3ddeed75f6..bffc1e4d3b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -82,7 +82,14 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { } } -struct CustomReserved2 @0xf35cc4560bbf6ec2 { +struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { + mpcSource @0 :MpcSource; + dynamicExperimentalControl @1 :Bool; + + enum MpcSource { + acc @0; + blended @1; + } } struct CustomReserved3 @0xda96579883444c35 { diff --git a/cereal/log.capnp b/cereal/log.capnp index a68621ea27..09276c7ae4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2634,7 +2634,7 @@ struct Event { # *********** Custom: reserved for forks *********** selfdriveStateSP @107 :Custom.SelfdriveStateSP; modelManagerSP @108 :Custom.ModelManagerSP; - customReserved2 @109 :Custom.CustomReserved2; + longitudinalPlanSP @109 :Custom.LongitudinalPlanSP; customReserved3 @110 :Custom.CustomReserved3; customReserved4 @111 :Custom.CustomReserved4; customReserved5 @112 :Custom.CustomReserved5; diff --git a/cereal/services.py b/cereal/services.py index 346bf81a09..ae4a35b3e4 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -77,6 +77,7 @@ _services: dict[str, tuple] = { # sunnypilot "modelManagerSP": (False, 1., 1), "selfdriveStateSP": (True, 100., 10), + "longitudinalPlanSP": (True, 20., 10), # debug "uiDebug": (True, 0., 1), diff --git a/common/params.cc b/common/params.cc index d86dd58c9f..2f34461e7e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -224,6 +224,8 @@ std::unordered_map keys = { {"SunnylinkdPid", PERSISTENT}, {"SunnylinkEnabled", PERSISTENT}, + {"DynamicExperimentalControl", PERSISTENT}, + // sunnypilot car specific params {"HyundaiRadarTracks", PERSISTENT}, {"HyundaiRadarTracksConfirmed", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index eba8019117..2272ebfeb8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,6 +16,8 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog +from openpilot.sunnypilot.selfdrive.controls.lib.dec.helpers import DecPlanner + LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] @@ -67,10 +69,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): return a_target, should_stop -class LongitudinalPlanner: +class LongitudinalPlanner(DecPlanner): def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) + DecPlanner.__init__(self, self.CP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True @@ -105,7 +108,10 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): + DecPlanner.update(self, sm) self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + if dec_mpc_mode := self.get_mpc_mode(sm): + self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) @@ -206,3 +212,4 @@ class LongitudinalPlanner: longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) + self.publish_longitudinal_plan_sp(sm, pm) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index bcfc4d0c14..16ac80db60 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -18,7 +18,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) - pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2', ignore_avg_freq=['radarState']) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 4911dc71e9..7f710328cc 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -40,6 +40,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "", "../assets/img_experimental_white.svg", }, + { + "DynamicExperimentalControl", + tr("Enable Dynamic Experimental Control"), + tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."), + "../assets/offroad/icon_blank.png", + }, { "DisengageOnAccelerator", tr("Disengage on Accelerator Pedal"), @@ -203,6 +209,11 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(pair_device); + QObject::connect(uiState()->prime_state, &PrimeState::changed, [this] (PrimeState::Type type) { + pair_device->setVisible(type == PrimeState::PRIME_TYPE_UNPAIRED); + }); + +#ifndef SUNNYPILOT // offroad-only buttons auto dcamBtn = new ButtonControl(tr("Driver Camera"), tr("PREVIEW"), @@ -214,6 +225,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { dcamBtn->setEnabled(true); }); addItem(dcamBtn); +#endif auto resetCalibBtn = new ButtonControl(tr("Reset Calibration"), tr("RESET"), ""); connect(resetCalibBtn, &ButtonControl::showDescriptionEvent, this, &DevicePanel::updateCalibDescription); @@ -225,6 +237,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(resetCalibBtn); +#ifndef SUNNYPILOT auto retrainingBtn = new ButtonControl(tr("Review Training Guide"), tr("REVIEW"), tr("Review the rules, features, and limitations of openpilot")); connect(retrainingBtn, &ButtonControl::clicked, [=]() { if (ConfirmationDialog::confirm(tr("Are you sure you want to review the training guide?"), tr("Review"), this)) { @@ -255,9 +268,6 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(translateBtn); - QObject::connect(uiState()->prime_state, &PrimeState::changed, [this] (PrimeState::Type type) { - pair_device->setVisible(type == PrimeState::PRIME_TYPE_UNPAIRED); - }); QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { for (auto btn : findChildren()) { if (btn != pair_device) { @@ -291,6 +301,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { #poweroff_btn:pressed { background-color: #FF2424; } )"); addItem(power_layout); +#endif } void DevicePanel::updateCalibDescription() { diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 8bfe1d79db..821801e5a7 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -16,6 +16,7 @@ qt_src = [ "sunnypilot/qt/window.cc", "sunnypilot/qt/home.cc", "sunnypilot/qt/offroad/offroad_home.cc", + "sunnypilot/qt/offroad/settings/device_panel.cc", "sunnypilot/qt/offroad/settings/settings.cc", "sunnypilot/qt/offroad/settings/software_panel.cc", "sunnypilot/qt/offroad/settings/sunnylink_panel.cc", diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc new file mode 100644 index 0000000000..36a56c9d88 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc @@ -0,0 +1,88 @@ +/** + * 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/device_panel.h" + +#include "common/watchdog.h" +#include "selfdrive/ui/qt/offroad/driverview.h" +#include "selfdrive/ui/qt/qt_window.h" + +DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) { + QGridLayout *device_grid_layout = new QGridLayout(); + device_grid_layout->setSpacing(30); + device_grid_layout->setHorizontalSpacing(5); + device_grid_layout->setVerticalSpacing(25); + + std::vector> device_btns = { + {"dcamBtn", tr("Driver Camera Preview")}, + {"retrainingBtn", tr("Training Guide")}, + {"regulatoryBtn", tr("Regulatory")}, + {"translateBtn", tr("Language")}, + }; + + for (int i = 0; i < device_btns.size(); i++) { + auto *btn = new PushButtonSP(device_btns[i].second, 720, this); + device_grid_layout->addWidget(btn, i / 2, i % 2); + buttons[device_btns[i].first] = btn; + } + + connect(buttons["dcamBtn"], &PushButtonSP::clicked, [this]() { + buttons["dcamBtn"]->setEnabled(false); + DriverViewDialog driver_view(this); + driver_view.exec(); + buttons["dcamBtn"]->setEnabled(true); + }); + + connect(buttons["retrainingBtn"], &PushButtonSP::clicked, [=]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to review the training guide?"), tr("Review"), this)) { + emit reviewTrainingGuide(); + } + }); + + if (Hardware::TICI()) { + connect(buttons["regulatoryBtn"], &PushButtonSP::clicked, [=]() { + const std::string txt = util::read_file("../assets/offroad/fcc.html"); + ConfirmationDialog::rich(QString::fromStdString(txt), this); + }); + } else { + buttons["regulatoryBtn"]->setEnabled(false); + } + + connect(buttons["translateBtn"], &PushButtonSP::clicked, [=]() { + QMap langs = getSupportedLanguages(); + QString selection = MultiOptionDialog::getSelection(tr("Select a language"), langs.keys(), langs.key(uiState()->language), this); + if (!selection.isEmpty()) { + // put language setting, exit Qt UI, and trigger fast restart + params.put("LanguageSetting", langs[selection].toStdString()); + qApp->exit(18); + watchdog_kick(0); + } + }); + + addItem(device_grid_layout); + + // offroad mode and power buttons + + QHBoxLayout *power_layout = new QHBoxLayout(); + power_layout->setSpacing(5); + + QPushButton *rebootBtn = new PushButtonSP(tr("Reboot"), 720, this); + rebootBtn->setStyleSheet(rebootButtonStyle); + power_layout->addWidget(rebootBtn); + QObject::connect(rebootBtn, &PushButtonSP::clicked, this, &DevicePanelSP::reboot); + + QPushButton *poweroffBtn = new PushButtonSP(tr("Power Off"), 720, this); + poweroffBtn->setStyleSheet(powerOffButtonStyle); + power_layout->addWidget(poweroffBtn); + QObject::connect(poweroffBtn, &PushButtonSP::clicked, this, &DevicePanelSP::poweroff); + + if (!Hardware::PC()) { + connect(uiState(), &UIState::offroadTransition, poweroffBtn, &PushButtonSP::setVisible); + } + + addItem(power_layout); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h new file mode 100644 index 0000000000..f1a18a8257 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h @@ -0,0 +1,51 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" + +class DevicePanelSP : public DevicePanel { + Q_OBJECT + +public: + explicit DevicePanelSP(SettingsWindowSP *parent = 0); + +private: + std::map buttons; + + const QString rebootButtonStyle = R"( + PushButtonSP { + border-radius: 20px; + font-size: 50px; + font-weight: 450; + height: 150px; + padding: 0 25px 0 25px; + color: #FFFFFF; + background-color: #393939; + } + PushButtonSP:pressed { + background-color: #4A4A4A; + } + )"; + + const QString powerOffButtonStyle = R"( + PushButtonSP { + border-radius: 20px; + font-size: 50px; + font-weight: 450; + height: 150px; + padding: 0 25px 0 25px; + color: #FFFFFF; + background-color: #E22C2C; + } + PushButtonSP:pressed { + background-color: #FF2424; + } + )"; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc index d8e9d83dee..9338b93220 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc @@ -11,12 +11,13 @@ #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" #include "selfdrive/ui/qt/offroad/developer_panel.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/trips_panel.h" -TogglesPanelSP::TogglesPanelSP(SettingsWindow *parent) : TogglesPanel(parent) { +TogglesPanelSP::TogglesPanelSP(SettingsWindowSP *parent) : TogglesPanel(parent) { QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &TogglesPanelSP::updateState); } @@ -50,7 +51,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) { )"); close_btn->setFixedSize(152, 152); close_btn_layout->addWidget(close_btn, 0, Qt::AlignLeft); - QObject::connect(close_btn, &QPushButton::clicked, this, &SettingsWindow::closeSettings); + QObject::connect(close_btn, &QPushButton::clicked, this, &SettingsWindowSP::closeSettings); // setup buttons widget QWidget *buttons_widget = new QWidget; @@ -59,11 +60,11 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) { buttons_layout->addSpacing(10); // setup panels - DevicePanel *device = new DevicePanel(this); - QObject::connect(device, &DevicePanel::reviewTrainingGuide, this, &SettingsWindow::reviewTrainingGuide); + DevicePanelSP *device = new DevicePanelSP(this); + QObject::connect(device, &DevicePanelSP::reviewTrainingGuide, this, &SettingsWindowSP::reviewTrainingGuide); TogglesPanelSP *toggles = new TogglesPanelSP(this); - QObject::connect(this, &SettingsWindow::expandToggleDescription, toggles, &TogglesPanel::expandToggleDescription); + QObject::connect(this, &SettingsWindowSP::expandToggleDescription, toggles, &TogglesPanel::expandToggleDescription); auto networking = new Networking(this); QObject::connect(uiState()->prime_state, &PrimeState::changed, networking, &Networking::setPrimeType); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h index 5678a3171b..2fe511cc48 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h @@ -32,7 +32,7 @@ class TogglesPanelSP : public TogglesPanel { Q_OBJECT public: - explicit TogglesPanelSP(SettingsWindow *parent); + explicit TogglesPanelSP(SettingsWindowSP *parent); private slots: void updateState(const UIStateSP &s); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.cc index 6671208fea..82d07febb8 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.cc @@ -27,13 +27,13 @@ SunnypilotPanel::SunnypilotPanel(SettingsWindowSP *parent) : QFrame(parent) { madsToggle->setConfirmation(true, false); list->addItem(madsToggle); - madsSettingsButton = new SubPanelButton(tr("Customize MADS")); + madsSettingsButton = new PushButtonSP(tr("Customize MADS")); madsSettingsButton->setObjectName("mads_btn"); connect(madsSettingsButton, &QPushButton::clicked, [=]() { sunnypilotScroller->setLastScrollPosition(); main_layout->setCurrentWidget(madsWidget); }); - QObject::connect(madsToggle, &ToggleControl::toggleFlipped, madsSettingsButton, &SubPanelButton::setEnabled); + QObject::connect(madsToggle, &ToggleControl::toggleFlipped, madsSettingsButton, &PushButtonSP::setEnabled); madsWidget = new MadsSettings(this); connect(madsWidget, &MadsSettings::backPress, [=]() { diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h index 8077522473..e955995b34 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h @@ -35,6 +35,6 @@ private: bool offroad; ParamControl *madsToggle; - SubPanelButton *madsSettingsButton; + PushButtonSP *madsSettingsButton; MadsSettings *madsWidget = nullptr; }; diff --git a/selfdrive/ui/sunnypilot/qt/widgets/controls.h b/selfdrive/ui/sunnypilot/qt/widgets/controls.h index a5071d97e7..a7c60980fa 100644 --- a/selfdrive/ui/sunnypilot/qt/widgets/controls.h +++ b/selfdrive/ui/sunnypilot/qt/widgets/controls.h @@ -546,11 +546,11 @@ private: bool button_enabled = true; }; -class SubPanelButton : public QPushButton { +class PushButtonSP : public QPushButton { Q_OBJECT public: - SubPanelButton(const QString &text, const int minimum_button_width = 800, QWidget *parent = nullptr) : QPushButton(text, parent) { + PushButtonSP(const QString &text, const int minimum_button_width = 800, QWidget *parent = nullptr) : QPushButton(text, parent) { const QString buttonStyle = R"( QPushButton { border-radius: 20px; diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index 5bc4857689..e3c71050ba 100644 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -129,7 +129,7 @@ def setup_keyboard(click, pm: PubMaster, scroll=None): def setup_driver_camera(click, pm: PubMaster, scroll=None): setup_settings_device(click, pm) - click(1950, 435) + click(950, 620) DATA['deviceState'].deviceState.started = False setup_onroad(click, pm) DATA['deviceState'].deviceState.started = True diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index bb3897034a..bc1ecbc10c 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -279,6 +279,45 @@ إقران + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + إعادة التشغيل + + + Regulatory + التنظيمية + + + Are you sure you want to review the training guide? + هل أنت متأكد أنك تريد مراجعة دليل التدريب؟ + + + Review + مراجعة + + + Select a language + اختر لغة + + + Power Off + إيقاف التشغيل + + DriveStats @@ -1376,6 +1415,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. تمكين مراقبة السائق حتى عندما لا يكون نظام OpenPilot مُفعّلاً. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 837a4ba86c..dd20fc2211 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -279,6 +279,45 @@ + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + Neustart + + + Regulatory + Rechtliche Hinweise + + + Are you sure you want to review the training guide? + Bist du sicher, dass du die Trainingsanleitung wiederholen möchtest? + + + Review + Überprüfen + + + Select a language + Sprache wählen + + + Power Off + Ausschalten + + DriveStats @@ -1360,6 +1399,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 6719ddd232..b1e147a7bd 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -279,6 +279,45 @@ Desactivar para apagar + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + Reiniciar + + + Regulatory + Regulador + + + Are you sure you want to review the training guide? + ¿Seguro que quiere revisar la guía de entrenamiento? + + + Review + Revisar + + + Select a language + + + + Power Off + Apagar + + DriveStats @@ -1360,6 +1399,14 @@ Esto puede tardar un minuto. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. Activar el control longitudinal (fase experimental) para permitir el modo Experimental. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index f84e2cba1c..33e5dc989a 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -279,6 +279,45 @@ + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + Redémarrer + + + Regulatory + Réglementaire + + + Are you sure you want to review the training guide? + Êtes-vous sûr de vouloir revoir le guide de formation ? + + + Review + Revoir + + + Select a language + Choisir une langue + + + Power Off + Éteindre + + DriveStats @@ -1360,6 +1399,14 @@ Cela peut prendre jusqu'à une minute. Enable driver monitoring even when openpilot is not engaged. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index dacf42b92e..5caa6b5f8e 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -279,6 +279,45 @@ + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + 再起動 + + + Regulatory + 認証情報 + + + Are you sure you want to review the training guide? + 使い方の確認をしますか？ + + + Review + 確認 + + + Select a language + 言語を選択 + + + Power Off + 電源を切る + + DriveStats @@ -1354,6 +1393,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 125667f36d..39d597a582 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -279,6 +279,45 @@ 동기화 + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + 재부팅 + + + Regulatory + 규제 + + + Are you sure you want to review the training guide? + 트레이닝 가이드를 다시 확인하시겠습니까? + + + Review + 다시보기 + + + Select a language + 언어를 선택하세요 + + + Power Off + 전원 끄기 + + DriveStats @@ -1356,6 +1395,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. Openpilot이 활성화되지 않은 경우에도 드라이버 모니터링을 활성화합니다. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index b87458b000..7a50643c6d 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -279,6 +279,45 @@ PAREAR + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + Reiniciar + + + Regulatory + Regulatório + + + Are you sure you want to review the training guide? + Tem certeza que quer rever o treinamento? + + + Review + Revisar + + + Select a language + Selecione o Idioma + + + Power Off + Desligar + + DriveStats @@ -1360,6 +1399,14 @@ Isso pode levar até um minuto. Enable driver monitoring even when openpilot is not engaged. Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index da21e8b088..1349bd7df7 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -279,6 +279,45 @@ จับคู่ + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + รีบูต + + + Regulatory + ระเบียบข้อบังคับ + + + Are you sure you want to review the training guide? + คุณแน่ใจหรือไม่ว่าต้องการทบทวนคู่มือการใช้งาน? + + + Review + ทบทวน + + + Select a language + เลือกภาษา + + + Power Off + ปิดเครื่อง + + DriveStats @@ -1356,6 +1395,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index 680ca89089..3f4eed7bc9 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -279,6 +279,45 @@ + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + Yeniden başlat + + + Regulatory + Mevzuat + + + Are you sure you want to review the training guide? + Eğitim kılavuzunu incelemek istediğinizden emin misiniz? + + + Review + + + + Select a language + Dil seçin + + + Power Off + Sistemi kapat + + DriveStats @@ -1354,6 +1393,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index e3bf863bda..c5ba3565c0 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -279,6 +279,45 @@ 配对 + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + 重启 + + + Regulatory + 监管信息 + + + Are you sure you want to review the training guide? + 您确定要查看新手指南吗？ + + + Review + 预览 + + + Select a language + 选择语言 + + + Power Off + 关机 + + DriveStats @@ -1356,6 +1395,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. 即使在openpilot未激活时也启用驾驶员监控。 + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index aff8f5a89b..c5586e3122 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -279,6 +279,45 @@ 配對 + + DevicePanelSP + + Driver Camera Preview + + + + Training Guide + + + + Language + + + + Reboot + 重新啟動 + + + Regulatory + 法規/監管 + + + Are you sure you want to review the training guide? + 您確定要觀看使用教學嗎？ + + + Review + 回顧 + + + Select a language + 選擇語言 + + + Power Off + 關機 + + DriveStats @@ -1356,6 +1395,14 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. 即使在openpilot未激活時也啟用駕駛監控。 + + Enable Dynamic Experimental Control + + + + Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. + + Updater diff --git a/sunnypilot/selfdrive/__init__.py b/sunnypilot/selfdrive/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/__init__.py b/sunnypilot/selfdrive/controls/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/__init__.py b/sunnypilot/selfdrive/controls/lib/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/dec/__init__.py b/sunnypilot/selfdrive/controls/lib/dec/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py new file mode 100644 index 0000000000..eb7660965e --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -0,0 +1,399 @@ +# The MIT License +# +# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +# +# Version = 2024-7-11 + +import numpy as np + +from cereal import messaging +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL + +# d-e2e, from modeldata.h +TRAJECTORY_SIZE = 33 + +LEAD_WINDOW_SIZE = 4 +LEAD_PROB = 0.6 + +SLOW_DOWN_WINDOW_SIZE = 4 +SLOW_DOWN_PROB = 0.6 + +SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] +SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.] + +SLOWNESS_WINDOW_SIZE = 12 +SLOWNESS_PROB = 0.5 +SLOWNESS_CRUISE_OFFSET = 1.05 + +DANGEROUS_TTC_WINDOW_SIZE = 3 +DANGEROUS_TTC = 2.3 + +HIGHWAY_CRUISE_KPH = 70 + +STOP_AND_GO_FRAME = 60 + +SET_MODE_TIMEOUT = 10 + +MPC_FCW_WINDOW_SIZE = 10 +MPC_FCW_PROB = 0.5 + +V_ACC_MIN = 9.72 + + +class SNG_State: + off = 0 + stopped = 1 + going = 2 + + +class GenericMovingAverageCalculator: + def __init__(self, window_size): + self.window_size = window_size + self.data = [] + self.total = 0 + + def add_data(self, value: float) -> None: + if len(self.data) == self.window_size: + self.total -= self.data.pop(0) + self.data.append(value) + self.total += value + + def get_moving_average(self) -> float | None: + return None if len(self.data) == 0 else self.total / len(self.data) + + def reset_data(self) -> None: + self.data = [] + self.total = 0 + + +class WeightedMovingAverageCalculator: + def __init__(self, window_size): + self.window_size = window_size + self.data = [] + self.weights = np.linspace(1, 3, window_size) # Linear weights, adjust as needed + + def add_data(self, value: float) -> None: + if len(self.data) == self.window_size: + self.data.pop(0) + self.data.append(value) + + def get_weighted_average(self) -> float | None: + if len(self.data) == 0: + return None + weighted_sum: float = float(np.dot(self.data, self.weights[-len(self.data):])) + weight_total: float = float(np.sum(self.weights[-len(self.data):])) + return weighted_sum / weight_total + + def reset_data(self) -> None: + self.data = [] + + +class DynamicExperimentalController: + def __init__(self, params=None): + self._params = params or Params() + self._is_enabled: bool = self._params.get_bool("DynamicExperimentalControl") + self._mode: str = 'acc' + self._mode_prev: str = 'acc' + self._mode_changed: bool = False + self._frame: int = 0 + + # Use weighted moving average for filtering leads + self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE) + self._has_lead_filtered = False + self._has_lead_filtered_prev = False + + self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE) + self._has_slow_down = False + + self._has_blinkers = False + + self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE) + self._has_slowness = False + + self._has_nav_instruction = False + + self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE) + self._has_dangerous_ttc = False + + self._v_ego_kph = 0. + self._v_cruise_kph = 0. + + self._has_lead = False + + self._has_standstill = False + self._has_standstill_prev = False + + self._sng_transit_frame = 0 + self._sng_state = SNG_State.off + + self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE) + self._has_mpc_fcw = False + self._mpc_fcw_crash_cnt = 0 + + self._set_mode_timeout = 0 + + @staticmethod + def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool: + """ + Basic anomaly detection using standard deviation. + """ + if len(recent_data) < 5: + return False + mean: float = float(np.mean(recent_data)) + std_dev: float = float(np.std(recent_data)) + anomaly: bool = bool(recent_data[-1] > mean + threshold * std_dev) + + # Context check to ensure repeated anomaly + if context_check: + return bool(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1) + return anomaly + + def _adaptive_slowdown_threshold(self) -> float: + """ + Adapts the slow-down threshold based on vehicle speed and recent behavior. + """ + adaptive_threshold: float = float( + interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) + ) + return adaptive_threshold + + def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2) -> bool: + """ + Smoothing the lead detection to avoid erratic behavior. + """ + self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob + return bool(self._has_lead_filtered > LEAD_PROB) + + def _adaptive_lead_prob_threshold(self) -> float: + """ + Adapts lead probability threshold based on driving conditions. + """ + if self._v_ego_kph > HIGHWAY_CRUISE_KPH: + return float(LEAD_PROB + 0.1) # Increase the threshold on highways + return float(LEAD_PROB) + + def _update(self, sm: messaging.SubMaster) -> None: + car_state = sm['carState'] + lead_one = sm['radarState'].leadOne + md = sm['modelV2'] + + self._v_ego_kph = car_state.vEgo * 3.6 + self._v_cruise_kph = car_state.vCruise + self._has_lead = lead_one.status + self._has_standstill = car_state.standstill + + # fcw detection + self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0) + self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB + + # nav enable detection + # self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13 + + # lead detection with smoothing + self._lead_gmac.add_data(lead_one.status) + #self._has_lead_filtered = self._lead_gmac.get_weighted_average() > LEAD_PROB + lead_prob = self._lead_gmac.get_weighted_average() or 0 + self._has_lead_filtered = self._smoothed_lead_detection(lead_prob) + + # adaptive slow down detection + adaptive_threshold = self._adaptive_slowdown_threshold() + slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold + self._slow_down_gmac.add_data(slow_down_trigger) + self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB + + # anomaly detection for slow down events + if self._anomaly_detection(self._slow_down_gmac.data): + # Handle anomaly: potentially log it, adjust behavior, or issue a warning + self._has_slow_down = False # Reset slow down if anomaly detected + + # blinker detection + self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker + + # sng detection + if self._has_standstill: + self._sng_state = SNG_State.stopped + self._sng_transit_frame = 0 + else: + if self._sng_transit_frame == 0: + if self._sng_state == SNG_State.stopped: + self._sng_state = SNG_State.going + self._sng_transit_frame = STOP_AND_GO_FRAME + elif self._sng_state == SNG_State.going: + self._sng_state = SNG_State.off + elif self._sng_transit_frame > 0: + self._sng_transit_frame -= 1 + + # slowness detection + if not self._has_standstill: + self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * SLOWNESS_CRUISE_OFFSET)) + self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB + + # dangerous TTC detection + if not self._has_lead_filtered and self._has_lead_filtered_prev: + self._dangerous_ttc_gmac.reset_data() + self._has_dangerous_ttc = False + + if self._has_lead and car_state.vEgo >= 0.01: + self._dangerous_ttc_gmac.add_data(lead_one.dRel / car_state.vEgo) + + self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC + + # keep prev values + self._has_standstill_prev = self._has_standstill + self._has_lead_filtered_prev = self._has_lead_filtered + + def _radarless_mode(self) -> None: + # when mpc fcw crash prob is high + # use blended to slow down quickly + if self._has_mpc_fcw: + self._set_mode('blended') + return + + # Nav enabled and distance to upcoming turning is 300 or below + # if self._has_nav_instruction: + # self._set_mode('blended') + # return + + # when blinker is on and speed is driving below V_ACC_MIN: blended + # we don't want it to switch mode at higher speed, blended may trigger hard brake + # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN: + # self._set_mode('blended') + # return + + # when at highway cruise and SNG: blended + # ensuring blended mode is used because acc is bad at catching SNG lead car + # especially those who accel very fast and then brake very hard. + # if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN: + # self._set_mode('blended') + # return + + # when standstill: blended + # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light. + if self._has_standstill: + self._set_mode('blended') + return + + # when detecting slow down scenario: blended + # e.g. traffic light, curve, stop sign etc. + if self._has_slow_down: + self._set_mode('blended') + return + + # when detecting lead slow down: blended + # use blended for higher braking capability + if self._has_dangerous_ttc: + self._set_mode('blended') + return + + # car driving at speed lower than set speed: acc + if self._has_slowness: + self._set_mode('acc') + return + + self._set_mode('acc') + + def _radar_mode(self) -> None: + # when mpc fcw crash prob is high + # use blended to slow down quickly + if self._has_mpc_fcw: + self._set_mode('blended') + return + + # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition, + if self._has_lead_filtered and not self._has_standstill: + self._set_mode('acc') + return + + # when blinker is on and speed is driving below V_ACC_MIN: blended + # we don't want it to switch mode at higher speed, blended may trigger hard brake + # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN: + # self._set_mode('blended') + # return + + # when standstill: blended + # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light. + if self._has_standstill: + self._set_mode('blended') + return + + # when detecting slow down scenario: blended + # e.g. traffic light, curve, stop sign etc. + if self._has_slow_down: + self._set_mode('blended') + return + + # car driving at speed lower than set speed: acc + if self._has_slowness: + self._set_mode('acc') + return + + # Nav enabled and distance to upcoming turning is 300 or below + # if self._has_nav_instruction: + # self._set_mode('blended') + # return + + self._set_mode('acc') + + def get_mpc_mode(self) -> str: + return str(self._mode) + + def has_changed(self) -> bool: + return bool(self._mode_changed) + + def set_enabled(self, enabled: bool) -> None: + self._is_enabled = enabled + + def is_enabled(self) -> bool: + return self._is_enabled + + def set_mpc_fcw_crash_cnt(self, crash_cnt: float) -> None: + self._mpc_fcw_crash_cnt = crash_cnt + + def _set_mode(self, mode: str) -> None: + if self._set_mode_timeout == 0: + self._mode = mode + if mode == 'blended': + self._set_mode_timeout = SET_MODE_TIMEOUT + + if self._set_mode_timeout > 0: + self._set_mode_timeout -= 1 + + def _read_params(self) -> None: + if self._frame % int(1. / DT_MDL) == 0: + self._is_enabled = self._params.get_bool("DynamicExperimentalControl") + + def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: + self._read_params() + + if self._is_enabled: + self._update(sm) + + if radar_unavailable: + self._radarless_mode() + else: + self._radar_mode() + + self._mode_changed = self._mode != self._mode_prev + self._mode_prev = self._mode + + self._frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py new file mode 100644 index 0000000000..602d3b64ba --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -0,0 +1,46 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from cereal import messaging, custom +from opendbc.car import structs +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController + +MpcSource = custom.LongitudinalPlanSP.MpcSource + + +class DecPlanner: + def __init__(self, CP: structs.CarParams, mpc): + self.CP = CP + self.mpc = mpc + + self.is_enabled = False + + self.dynamic_experimental_controller = DynamicExperimentalController() + + def get_mpc_mode(self, sm: messaging.SubMaster): + if not self.is_enabled or not sm['selfdriveState'].experimentalMode: + return None + + return self.dynamic_experimental_controller.get_mpc_mode() + + def update(self, sm: messaging.SubMaster) -> None: + self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) + self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) + + def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: + plan_sp_send = messaging.new_message('longitudinalPlanSP') + + plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) + + longitudinalPlanSP = plan_sp_send.longitudinalPlanSP + + # DEC + longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc + + longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() + + pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py new file mode 100644 index 0000000000..af77639424 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py @@ -0,0 +1,257 @@ +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ( + DynamicExperimentalController, + TRAJECTORY_SIZE, + LEAD_WINDOW_SIZE, + SLOW_DOWN_WINDOW_SIZE, + DANGEROUS_TTC_WINDOW_SIZE, + MPC_FCW_WINDOW_SIZE, + SNG_State, + STOP_AND_GO_FRAME +) + +import pytest +import numpy as np +from openpilot.common.params import Params + +class MockInterp: + def __call__(self, x, xp, fp): + return np.interp(x, xp, fp) + +class MockCarState: + def __init__(self, v_ego=0., standstill=False, left_blinker=False, right_blinker=False): + self.vEgo = v_ego + self.standstill = standstill + self.leftBlinker = left_blinker + self.rightBlinker = right_blinker + +class MockLeadOne: + def __init__(self, status=False, d_rel=0): + self.status = status + self.dRel = d_rel + +class MockModelData: + def __init__(self, x_vals=None, positions=None): + self.orientation = type('Orientation', (), {'x': x_vals})() + self.position = type('Position', (), {'x': positions})() + +class MockControlState: + def __init__(self, v_cruise=0): + self.vCruise = v_cruise + +@pytest.fixture +def interp(monkeypatch): + mock_interp = MockInterp() + monkeypatch.setattr('openpilot.common.numpy_fast.interp', mock_interp) + return mock_interp + +@pytest.fixture +def controller(interp): + params = Params() + params.put_bool("DynamicExperimentalControl", True) + controller = DynamicExperimentalController() + return controller + +def test_initial_state(controller): + """Test initial state of the controller""" + assert controller._mode == 'acc' + assert not controller._has_lead + assert not controller._has_standstill + assert controller._sng_state == SNG_State.off + assert not controller._has_lead_filtered + assert not controller._has_slow_down + assert not controller._has_dangerous_ttc + assert not controller._has_mpc_fcw + +@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) +def test_standstill_detection(controller, has_radar): + """Test standstill detection and state transitions""" + car_state = MockCarState(standstill=True) + lead_one = MockLeadOne() + md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) + controls_state = MockControlState() + + # Test transition to standstill + controller.update(not has_radar, car_state, lead_one, md, controls_state) + assert controller._sng_state == SNG_State.stopped + assert controller.get_mpc_mode() == 'blended' + + # Test transition from standstill to moving + car_state.standstill = False + controller.update(not has_radar, car_state, lead_one, md, controls_state) + assert controller._sng_state == SNG_State.going + + # Test complete transition to normal driving + for _ in range(STOP_AND_GO_FRAME + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + assert controller._sng_state == SNG_State.off + +@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) +def test_lead_detection(controller, has_radar): + """Test lead vehicle detection and filtering""" + car_state = MockCarState(v_ego=20) # 72 kph + lead_one = MockLeadOne(status=True, d_rel=50) # Safe distance + md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) + controls_state = MockControlState(v_cruise=72) + + # Let moving average stabilize + for _ in range(LEAD_WINDOW_SIZE + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert controller._has_lead_filtered + expected_mode = 'acc' if has_radar else 'blended' + assert controller.get_mpc_mode() == expected_mode + + # Test lead loss detection + lead_one.status = False + for _ in range(LEAD_WINDOW_SIZE + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert not controller._has_lead_filtered + +@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) +def test_slow_down_detection(controller, has_radar): + """Test slow down detection based on trajectory""" + car_state = MockCarState(v_ego=10/3.6) # 10 kph + lead_one = MockLeadOne() + x_vals = [0] * TRAJECTORY_SIZE + positions = [20] * TRAJECTORY_SIZE # Position within slow down threshold + md = MockModelData(x_vals=x_vals, positions=positions) + controls_state = MockControlState(v_cruise=30) + + # Test slow down detection + for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert controller._has_slow_down + assert controller.get_mpc_mode() == 'blended' + + # Test slow down recovery + positions = [200] * TRAJECTORY_SIZE # Position outside slow down threshold + md = MockModelData(x_vals=x_vals, positions=positions) + for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert not controller._has_slow_down + +@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) +def test_dangerous_ttc_detection(controller, has_radar): + """Test Time-To-Collision detection and handling""" + car_state = MockCarState(v_ego=10) # 36 kph + lead_one = MockLeadOne(status=True) + md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) + controls_state = MockControlState(v_cruise=36) + + # First establish normal conditions with lead + lead_one.dRel = 100 # Safe distance + for _ in range(LEAD_WINDOW_SIZE + 1): # First establish lead detection + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert controller._has_lead_filtered # Verify lead is detected + + # Now test dangerous TTC detection + lead_one.dRel = 10 # 10m distance - should trigger dangerous TTC + # TTC = dRel/vEgo = 10/10 = 1s (which is less than DANGEROUS_TTC = 2.3s) + + # Need to update multiple times to allow the weighted average to stabilize + for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert controller._has_dangerous_ttc, "TTC of 1s should be considered dangerous" + expected_mode = 'acc' if has_radar else 'blended' + assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC" + +@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) +def test_mode_transitions(controller, has_radar): + """Test comprehensive mode transitions under different conditions""" + # Initialize with normal driving conditions + car_state = MockCarState(v_ego=25) # 90 kph + lead_one = MockLeadOne(status=False) + md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[200] * TRAJECTORY_SIZE) + controls_state = MockControlState(v_cruise=100) + + def stabilize_filters(): + """Helper to let all moving averages stabilize""" + for _ in range(max(LEAD_WINDOW_SIZE, SLOW_DOWN_WINDOW_SIZE, + DANGEROUS_TTC_WINDOW_SIZE, MPC_FCW_WINDOW_SIZE) + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + # Test 1: Normal driving -> ACC mode + stabilize_filters() + assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode under normal driving conditions" + + # Test 2: Standstill -> Blended mode + car_state.standstill = True + controller.update(not has_radar, car_state, lead_one, md, controls_state) + assert controller.get_mpc_mode() == 'blended', "Should be in blended mode during standstill" + + # Test 3: Lead car appears -> ACC mode + car_state = MockCarState(v_ego=20) # Reset car state + lead_one.status = True + lead_one.dRel = 50 # Safe distance + stabilize_filters() + assert not controller._has_dangerous_ttc, "Should not have dangerous TTC" + assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode with safe lead distance" + + # Test 4: Dangerous TTC -> Blended mode + car_state = MockCarState(v_ego=20) # 72 kph + lead_one.status = True + lead_one.dRel = 50 # First establish normal lead detection + + # First establish lead detection + for _ in range(LEAD_WINDOW_SIZE + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert controller._has_lead_filtered # Verify lead is detected + + # Now create dangerous TTC condition + lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC + + for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition" + expected_mode = 'acc' if has_radar else 'blended' + assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC" + +@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) +def test_mpc_fcw_handling(controller, has_radar): + """Test MPC FCW crash count handling and mode transitions""" + car_state = MockCarState(v_ego=20) + lead_one = MockLeadOne() + md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) + controls_state = MockControlState(v_cruise=72) + + # Test FCW activation + controller.set_mpc_fcw_crash_cnt(5) + for _ in range(MPC_FCW_WINDOW_SIZE + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert controller._has_mpc_fcw + assert controller.get_mpc_mode() == 'blended' + + # Test FCW recovery + controller.set_mpc_fcw_crash_cnt(0) + for _ in range(MPC_FCW_WINDOW_SIZE + 1): + controller.update(not has_radar, car_state, lead_one, md, controls_state) + + assert not controller._has_mpc_fcw + +def test_radar_unavailable_handling(controller): + """Test behavior transitions between radar available and unavailable states""" + car_state = MockCarState(v_ego=27.78) # 100 kph + lead_one = MockLeadOne(status=True, d_rel=50) + md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) + controls_state = MockControlState(v_cruise=100) + + # Test with radar available + for _ in range(LEAD_WINDOW_SIZE + 1): + controller.update(False, car_state, lead_one, md, controls_state) + radar_mode = controller.get_mpc_mode() + + # Test with radar unavailable + for _ in range(LEAD_WINDOW_SIZE + 1): + controller.update(True, car_state, lead_one, md, controls_state) + radarless_mode = controller.get_mpc_mode() + + assert radar_mode is not None + assert radarless_mode is not None diff --git a/system/manager/manager.py b/system/manager/manager.py index 7a8ec9fc86..499ed6474b 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -43,6 +43,7 @@ def manager_init() -> None: ] sunnypilot_default_params: list[tuple[str, str | bytes]] = [ + ("DynamicExperimentalControl", "0"), ("Mads", "1"), ("MadsMainCruiseAllowed", "1"), ("MadsPauseLateralOnBrake", "0"),