Compare commits

..

2 Commits

Author SHA1 Message Date
Jason Wen f8e04b3776 Hyundai CAN Non-SCC: split HEV and EV rx safety checks 2025-08-25 19:56:33 -04:00
Nayan 9579d331fc ui: sunnylink panel title & message (#1181)
add title & message to clarify sponsorship isn't required for basic functions
2025-08-25 19:49:49 +02:00
30 changed files with 26 additions and 1402 deletions
-25
View File
@@ -123,9 +123,6 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
dec @0 :DynamicExperimentalControl;
events @1 :List(OnroadEventSP.Event);
slc @2 :SpeedLimitControl;
struct DynamicExperimentalControl {
state @0 :DynamicExperimentalControlState;
enabled @1 :Bool;
@@ -136,23 +133,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
blended @1;
}
}
struct SpeedLimitControl {
state @0 :SpeedLimitControlState;
enabled @1 :Bool;
active @2 :Bool;
speedLimit @3 :Float32;
speedLimitOffset @4 :Float32;
distToSpeedLimit @5 :Float32;
}
enum SpeedLimitControlState {
inactive @0; # No speed limit set or not enabled by parameter.
tempInactive @1; # User wants to ignore speed limit until it changes.
preActive @2;
adapting @3; # Reducing speed to match new speed limit.
active @4; # Cruising at speed limit.
}
}
struct OnroadEventSP @0xda96579883444c35 {
@@ -192,10 +172,6 @@ struct OnroadEventSP @0xda96579883444c35 {
experimentalModeSwitched @14;
wrongCarModeAlertOnly @15;
pedalPressedAlertOnly @16;
speedLimitPreActive @17;
speedLimitActive @18;
speedLimitConfirmed @19;
speedLimitValueChange @20;
}
}
@@ -271,7 +247,6 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
}
struct CarStateSP @0xb86e6369214c01c8 {
speedLimit @0 :Float32; # m/s
}
struct LiveMapDataSP @0xf416ec09499d9d19 {
-10
View File
@@ -216,14 +216,4 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"OsmStateTitle", {PERSISTENT, STRING}},
{"OsmWayTest", {PERSISTENT, STRING}},
{"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
// Speed Limit Control
{"SpeedLimitControl", {PERSISTENT | BACKUP, BOOL, "0"}},
{"SpeedLimitControlPolicy", {PERSISTENT | BACKUP, INT, "3"}},
{"SpeedLimitEngageType", {PERSISTENT | BACKUP, INT, "0"}},
{"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
{"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
{"SpeedLimitWarningType", {PERSISTENT | BACKUP, INT, "0"}},
{"SpeedLimitWarningOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
{"SpeedLimitWarningValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
};
@@ -146,9 +146,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
# Get new v_cruise from Speed Limit Control
v_cruise = LongitudinalPlannerSP.update_v_cruise(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
if force_slow_decel:
v_cruise = 0.0
+1 -5
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python3
from cereal import car
from openpilot.common.gps import get_gps_location_service
from openpilot.common.params import Params
from openpilot.common.realtime import Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
@@ -17,13 +16,10 @@ def main():
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
cloudlog.info("plannerd got CarParams: %s", CP.brand)
gps_location_service = get_gps_location_service(params)
ldw = LaneDepartureWarning()
longitudinal_planner = LongitudinalPlanner(CP)
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
'liveMapDataSP', 'carStateSP', gps_location_service],
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
poll='modelV2')
while True:
+1 -2
View File
@@ -96,7 +96,7 @@ class SelfdriveD(CruiseHelper):
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \
self.camera_packets + self.sensor_packets + self.gps_packets + ["longitudinalPlanSP"],
self.camera_packets + self.sensor_packets + self.gps_packets,
ignore_alive=ignore, ignore_avg_freq=ignore,
ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -209,7 +209,6 @@ class SelfdriveD(CruiseHelper):
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events)
# Add car events, ignore if CAN isn't valid
if CS.canValid:
@@ -67,9 +67,6 @@ class Plant:
lp = messaging.new_message('liveParameters')
car_control = messaging.new_message('carControl')
model = messaging.new_message('modelV2')
car_state_sp = messaging.new_message('carStateSP')
live_map_data_sp = messaging.new_message('liveMapDataSP')
gps_data = messaging.new_message('gpsLocation')
a_lead = (v_lead - self.v_lead_prev)/self.ts
self.v_lead_prev = v_lead
@@ -136,10 +133,7 @@ class Plant:
'controlsState': control.controlsState,
'selfdriveState': ss.selfdriveState,
'liveParameters': lp.liveParameters,
'modelV2': model.modelV2,
'carStateSP': car_state_sp.carStateSP,
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
'gpsLocation': gps_data.gpsLocation}
'modelV2': model.modelV2}
self.planner.update(sm)
self.acceleration = self.planner.output_a_target
self.speed = self.speed + self.acceleration * self.ts
-4
View File
@@ -52,10 +52,6 @@ lateral_panel_qt_src = [
longitudinal_panel_qt_src = [
"sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc",
"sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control.cc",
"sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control_policy.cc",
"sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control_subpanel.cc",
"sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control_warning.cc",
]
network_src = [
@@ -1,25 +0,0 @@
/**
* 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/longitudinal/slc/speed_limit_control.h"
SpeedLimitControl::SpeedLimitControl(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent)
: ExpandableToggleRow(param, title, desc, icon, parent) {
auto *slcFrame = new QFrame(this);
auto *slcFrameLayout = new QVBoxLayout();
slcFrame->setLayout(slcFrameLayout);
slcFrameLayout->setSpacing(0);
slcFrameLayout->setContentsMargins(0, 0, 0, 0);
slcSettings = new PushButtonSP(tr("Customize SLC"));
slcFrameLayout->addWidget(slcSettings);
connect(slcSettings, &QPushButton::clicked, [&]() {
emit slcSettingsButtonClicked();
});
addItem(slcFrame);
}
@@ -1,77 +0,0 @@
/*
* 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"
enum class SLCEngageType {
AUTO,
USER_CONFIRM,
};
inline const char *SLCEngageTypeText[]{
QT_TR_NOOP("Auto"),
QT_TR_NOOP("User Confirm")
};
enum class SLCOffsetType {
NONE,
FIXED,
PERCENT,
};
inline const char *SLCOffsetTypeText[]{
QT_TR_NOOP("None"),
QT_TR_NOOP("Fixed"),
QT_TR_NOOP("Percent"),
};
enum class SLCWarningType {
OFF,
DISPLAY,
CHIME,
};
inline const char *SLCWarningTypeText[]{
QT_TR_NOOP("Off"),
QT_TR_NOOP("Display"),
QT_TR_NOOP("Chime")
};
enum class SLCSourcePolicy {
CAR_ONLY,
MAP_ONLY,
CAR_FIRST,
MAP_FIRST,
COMBINED
};
inline const char *SLCSourcePolicyText[]{
QT_TR_NOOP("Car\nOnly"),
QT_TR_NOOP("Map\nOnly"),
QT_TR_NOOP("Car\nFirst"),
QT_TR_NOOP("Map\nFirst"),
QT_TR_NOOP("Combined\nData")
};
class SpeedLimitControl : public ExpandableToggleRow {
Q_OBJECT
public:
SpeedLimitControl(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr);
signals:
void slcSettingsButtonClicked();
private:
Params params;
PushButtonSP *slcSettings;
};
@@ -1,53 +0,0 @@
/*
* 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/longitudinal/slc/speed_limit_control_policy.h"
SpeedLimitControlPolicy::SpeedLimitControlPolicy(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(0, 0, 0, 0);
main_layout->setSpacing(0);
// Back button
PanelBackButton *back = new PanelBackButton(tr("Back"));
connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
main_layout->addWidget(back, 0, Qt::AlignLeft);
main_layout->addSpacing(10);
ListWidgetSP *list = new ListWidgetSP(this, true);
std::vector<QString> slc_policy_texts{
SLCSourcePolicyText[static_cast<int>(SLCSourcePolicy::CAR_ONLY)],
SLCSourcePolicyText[static_cast<int>(SLCSourcePolicy::MAP_ONLY)],
SLCSourcePolicyText[static_cast<int>(SLCSourcePolicy::CAR_FIRST)],
SLCSourcePolicyText[static_cast<int>(SLCSourcePolicy::MAP_FIRST)],
SLCSourcePolicyText[static_cast<int>(SLCSourcePolicy::COMBINED)]
};
slc_policy = new ButtonParamControlSP(
"SpeedLimitControlPolicy",
tr("Speed Limit Source"),
"",
"",
slc_policy_texts,
250);
list->addItem(slc_policy);
connect(slc_policy, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitControlPolicy::refresh);
refresh();
main_layout->addWidget(list);
};
void SpeedLimitControlPolicy::refresh() {
SLCSourcePolicy policy_param = static_cast<SLCSourcePolicy>(std::atoi(params.get("SpeedLimitControlPolicy").c_str()));
slc_policy->setDescription(sourceDescription(policy_param));
}
void SpeedLimitControlPolicy::showEvent(QShowEvent *event) {
refresh();
slc_policy->showDescription();
}
@@ -1,56 +0,0 @@
/*
* 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/offroad/settings/longitudinal/slc/speed_limit_control.h"
#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
class SpeedLimitControlPolicy : public QWidget {
Q_OBJECT
public:
explicit SpeedLimitControlPolicy(QWidget *parent = nullptr);
void refresh();
void showEvent(QShowEvent *event) override;
signals:
void backPress();
private:
Params params;
ButtonParamControlSP *slc_policy;
static QString sourceDescription(SLCSourcePolicy type = SLCSourcePolicy::CAR_ONLY) {
QString car_only = tr("⦿ Car Only: Use Speed Limit data only from Car");
QString map_only = tr("⦿ Map Only: Use Speed Limit data only from OpenStreetMaps");
QString car_first = tr("⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps");
QString map_first = tr("⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car");
QString combined = tr("⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps");
if (type == SLCSourcePolicy::CAR_ONLY) {
car_only = "<font color='white'><b>" + car_only + "</b></font>";
} else if (type == SLCSourcePolicy::MAP_ONLY) {
map_only = "<font color='white'><b>" + map_only + "</b></font>";
} else if (type == SLCSourcePolicy::CAR_FIRST) {
car_first = "<font color='white'><b>" + car_first + "</b></font>";
} else if (type == SLCSourcePolicy::MAP_FIRST) {
map_first = "<font color='white'><b>" + map_first + "</b></font>";
} else {
combined = "<font color='white'><b>" + combined + "</b></font>";
}
return QString("%1<br>%2<br>%3<br>%4<br>%5")
.arg(car_only)
.arg(map_only)
.arg(car_first)
.arg(map_first)
.arg(combined);
}
};
@@ -1,142 +0,0 @@
/*
* 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/longitudinal/slc/speed_limit_control_subpanel.h"
SpeedLimitControlSubpanel::SpeedLimitControlSubpanel(QWidget *parent) : QStackedWidget(parent) {
subPanelFrame = new QFrame();
QVBoxLayout *subPanelLayout = new QVBoxLayout(subPanelFrame);
subPanelLayout->setContentsMargins(0, 0, 0, 0);
subPanelLayout->setSpacing(0);
// Back button
PanelBackButton *back = new PanelBackButton(tr("Back"));
connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
subPanelLayout->addWidget(back, 0, Qt::AlignLeft);
subPanelLayout->addSpacing(20);
ListWidgetSP *list = new ListWidgetSP(this, true);
auto *slcBtnFrame = new QFrame(this);
auto *slcBtnFrameLayout = new QGridLayout();
slcBtnFrame->setLayout(slcBtnFrameLayout);
slcBtnFrameLayout->setContentsMargins(0, 40, 0, 40);
slcBtnFrameLayout->setSpacing(0);
slcWarningScreen = new SpeedLimitControlWarning(this);
slcPolicyScreen = new SpeedLimitControlPolicy(this);
slcWarningControl = new PushButtonSP(tr("Customize Warning"));
connect(slcWarningControl, &QPushButton::clicked, [&]() {
setCurrentWidget(slcWarningScreen);
slcWarningScreen->refresh();
});
connect(slcWarningScreen, &SpeedLimitControlWarning::backPress, [&]() {
setCurrentWidget(subPanelFrame);
showEvent(new QShowEvent());
});
slcSourceControl = new PushButtonSP(tr("Customize Source"));
connect(slcSourceControl, &QPushButton::clicked, [&]() {
setCurrentWidget(slcPolicyScreen);
slcPolicyScreen->refresh();
});
connect(slcPolicyScreen, &SpeedLimitControlPolicy::backPress, [&]() {
setCurrentWidget(subPanelFrame);
showEvent(new QShowEvent());
});
slcWarningControl->setFixedWidth(720);
slcSourceControl->setFixedWidth(720);
slcBtnFrameLayout->addWidget(slcWarningControl, 0, 0, Qt::AlignLeft);
slcBtnFrameLayout->addWidget(slcSourceControl, 0, 1, Qt::AlignRight);
list->addItem(slcBtnFrame);
std::vector<QString> slc_engage_texts{
SLCEngageTypeText[static_cast<int>(SLCEngageType::AUTO)],
SLCEngageTypeText[static_cast<int>(SLCEngageType::USER_CONFIRM)]
};
slc_engage_setting = new ButtonParamControlSP(
"SpeedLimitEngageType",
tr("Engage Mode"),
"",
"",
slc_engage_texts,
500);
slc_engage_setting->showDescription();
list->addItem(slc_engage_setting);
QFrame *offsetFrame = new QFrame(this);
QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame);
std::vector<QString> slc_offset_texts{
SLCOffsetTypeText[static_cast<int>(SLCOffsetType::NONE)],
SLCOffsetTypeText[static_cast<int>(SLCOffsetType::FIXED)],
SLCOffsetTypeText[static_cast<int>(SLCOffsetType::PERCENT)]
};
slc_offset_setting = new ButtonParamControlSP(
"SpeedLimitOffsetType",
tr("Speed Limit Offset"),
"",
"",
slc_offset_texts,
500);
offsetLayout->addWidget(slc_offset_setting);
slc_offset = new OptionControlSP(
"SpeedLimitValueOffset",
"",
"",
"",
{-30, 30}
);
offsetLayout->addWidget(slc_offset);
list->addItem(offsetFrame);
connect(slc_offset, &OptionControlSP::updateLabels, this, &SpeedLimitControlSubpanel::refresh);
connect(slc_offset_setting, &ButtonParamControlSP::showDescriptionEvent, slc_offset, &OptionControlSP::showDescription);
connect(slc_engage_setting, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitControlSubpanel::refresh);
connect(slc_offset_setting, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitControlSubpanel::refresh);
refresh();
subPanelLayout->addWidget(list);
addWidget(subPanelFrame);
addWidget(slcWarningScreen);
addWidget(slcPolicyScreen);
setCurrentWidget(subPanelFrame);
};
void SpeedLimitControlSubpanel::refresh() {
SLCEngageType engage_type_param = static_cast<SLCEngageType>(std::atoi(params.get("SpeedLimitEngageType").c_str()));
SLCOffsetType offset_type_param = static_cast<SLCOffsetType>(std::atoi(params.get("SpeedLimitOffsetType").c_str()));
QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset"));
slc_engage_setting->setDescription(engageModeDescription(engage_type_param));
slc_offset->setDescription(offsetDescription(offset_type_param));
if (offset_type_param == SLCOffsetType::PERCENT) {
offsetLabel += "%";
}
if (offset_type_param == SLCOffsetType::NONE) {
slc_offset->setVisible(false);
} else {
slc_offset->setVisible(true);
slc_offset->setLabel(offsetLabel);
slc_offset->showDescription();
}
}
void SpeedLimitControlSubpanel::showEvent(QShowEvent *event) {
refresh();
slc_engage_setting->showDescription();
slc_offset->showDescription();
}
@@ -1,72 +0,0 @@
/*
* 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/offroad/settings/longitudinal/slc/speed_limit_control.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control_policy.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control_warning.h"
#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
class SpeedLimitControlSubpanel : public QStackedWidget {
Q_OBJECT
public:
SpeedLimitControlSubpanel(QWidget *parent = nullptr);
void refresh();
void showEvent(QShowEvent *event) override;
signals:
void backPress();
private:
Params params;
QFrame *subPanelFrame;
PushButtonSP *slcWarningControl;
PushButtonSP *slcSourceControl;
ButtonParamControlSP *slc_offset_setting;
OptionControlSP *slc_offset;
ButtonParamControlSP *slc_engage_setting;
SpeedLimitControlWarning *slcWarningScreen;
SpeedLimitControlPolicy *slcPolicyScreen;
static QString engageModeDescription(SLCEngageType type = SLCEngageType::AUTO) {
QString auto_str = tr("⦿ Auto: Automatic speed adjustment based on speed limit data");
QString user_str = tr("⦿ User Confirm: Asks driver to confirm speed adjustment based on speed limit data");
if (type == SLCEngageType::USER_CONFIRM) {
user_str = "<font color='white'><b>" + user_str + "</b></font>";
} else {
auto_str = "<font color='white'><b>" + auto_str + "</b></font>";
}
return QString("%1<br>%2")
.arg(auto_str)
.arg(user_str);
}
static QString offsetDescription(SLCOffsetType type = SLCOffsetType::NONE) {
QString none_str = tr("⦿ None: No Offset");
QString fixed_str = tr("⦿ Fixed: Adds a fixed offset [Speed Limit + Offset]");
QString percent_str = tr("⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)]");
if (type == SLCOffsetType::FIXED) {
fixed_str = "<font color='white'><b>" + fixed_str + "</b></font>";
} else if (type == SLCOffsetType::PERCENT) {
percent_str = "<font color='white'><b>" + percent_str + "</b></font>";
} else {
none_str = "<font color='white'><b>" + none_str + "</b></font>";
}
return QString("%1<br>%2<br>%3")
.arg(none_str)
.arg(fixed_str)
.arg(percent_str);
}
};
@@ -1,100 +0,0 @@
/*
* 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/longitudinal/slc/speed_limit_control_warning.h"
SpeedLimitControlWarning::SpeedLimitControlWarning(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(0, 0, 0, 0);
main_layout->setSpacing(0);
// Back button
PanelBackButton *back = new PanelBackButton(tr("Back"));
connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
main_layout->addWidget(back, 0, Qt::AlignLeft);
main_layout->addSpacing(10);
ListWidgetSP *list = new ListWidgetSP(this, true);
std::vector<QString> slc_warning_texts{
SLCWarningTypeText[static_cast<int>(SLCWarningType::OFF)],
SLCWarningTypeText[static_cast<int>(SLCWarningType::DISPLAY)],
SLCWarningTypeText[static_cast<int>(SLCWarningType::CHIME)]
};
slc_warning_settings = new ButtonParamControlSP(
"SpeedLimitWarningType", tr("Speed Limit Warning"),
"",
"",
slc_warning_texts,
300);
list->addItem(slc_warning_settings);
QFrame *offsetFrame = new QFrame(this);
QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame);
std::vector<QString> slc_warning_offset_texts{
SLCOffsetTypeText[static_cast<int>(SLCOffsetType::NONE)],
SLCOffsetTypeText[static_cast<int>(SLCOffsetType::FIXED)],
SLCOffsetTypeText[static_cast<int>(SLCOffsetType::PERCENT)]
};
slc_warning_offset_settings = new ButtonParamControlSP(
"SpeedLimitWarningOffsetType",
tr("Warning Offset"),
"",
"",
slc_warning_offset_texts,
300);
offsetLayout->addWidget(slc_warning_offset_settings);
slc_warning_offset = new OptionControlSP(
"SpeedLimitWarningValueOffset",
"",
"",
"",
{-30, 30}
);
slc_warning_offset->setFixedWidth(100);
offsetLayout->addWidget(slc_warning_offset);
list->addItem(offsetFrame);
connect(slc_warning_offset, &OptionControlSP::updateLabels, this, &SpeedLimitControlWarning::refresh);
connect(slc_warning_offset_settings, &ButtonParamControlSP::showDescriptionEvent, slc_warning_offset, &OptionControlSP::showDescription);
connect(slc_warning_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitControlWarning::refresh);
connect(slc_warning_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitControlWarning::refresh);
refresh();
main_layout->addWidget(list);
};
void SpeedLimitControlWarning::refresh() {
SLCWarningType warning_type_param = static_cast<SLCWarningType>(std::atoi(params.get("SpeedLimitWarningType").c_str()));
SLCOffsetType offset_type_param = static_cast<SLCOffsetType>(std::atoi(params.get("SpeedLimitWarningOffsetType").c_str()));
QString offsetLabel = QString::fromStdString(params.get("SpeedLimitWarningValueOffset"));
slc_warning_settings->setDescription(warningDescription(warning_type_param));
slc_warning_offset->setDescription(offsetDescription(offset_type_param));
if (offset_type_param == SLCOffsetType::PERCENT) {
offsetLabel += "%";
}
if (offset_type_param == SLCOffsetType::NONE) {
slc_warning_offset->setDisabled(true);
slc_warning_offset->setLabel(tr("N/A"));
} else {
slc_warning_offset->setDisabled(false);
slc_warning_offset->setLabel(offsetLabel);
}
}
void SpeedLimitControlWarning::showEvent(QShowEvent *event) {
refresh();
slc_warning_settings->showDescription();
slc_warning_offset->showDescription();
}
@@ -1,71 +0,0 @@
/*
* 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/offroad/settings/longitudinal/slc/speed_limit_control.h"
#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
class SpeedLimitControlWarning : public QWidget {
Q_OBJECT
public:
SpeedLimitControlWarning(QWidget *parent = nullptr);
void refresh();
void showEvent(QShowEvent *event) override;
signals:
void backPress();
private:
Params params;
ButtonParamControlSP *slc_warning_settings;
ButtonParamControlSP *slc_warning_offset_settings;
OptionControlSP *slc_warning_offset;
static QString warningDescription(SLCWarningType type = SLCWarningType::OFF) {
QString off_str = tr("⦿ Off: No Warning");
QString display_str = tr("⦿ Display: Speed Limit Sign will visually alert");
QString chime_str = tr("⦿ Chime: Speed Limit Sign will visually alert along with an audible chime");
if (type == SLCWarningType::DISPLAY) {
display_str = "<font color='white'><b>" + display_str + "</b></font>";
} else if (type == SLCWarningType::CHIME) {
chime_str = "<font color='white'><b>" + chime_str + "</b></font>";
} else {
off_str = "<font color='white'><b>" + off_str + "</b></font>";
}
return QString("%1<br>%2<br>%3")
.arg(off_str)
.arg(display_str)
.arg(chime_str);
}
static QString offsetDescription(SLCOffsetType type = SLCOffsetType::NONE) {
QString none_str = tr("⦿ None: No Offset");
QString fixed_str = tr("⦿ Fixed: Adds a fixed offset [Speed Limit + Offset]");
QString percent_str = tr("⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)]");
if (type == SLCOffsetType::FIXED) {
fixed_str = "<font color='white'><b>" + fixed_str + "</b></font>";
} else if (type == SLCOffsetType::PERCENT) {
percent_str = "<font color='white'><b>" + percent_str + "</b></font>";
} else {
none_str = "<font color='white'><b>" + none_str + "</b></font>";
}
return QString("%1<br>%2<br>%3")
.arg(none_str)
.arg(fixed_str)
.arg(percent_str);
}
};
@@ -8,21 +8,6 @@
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h"
LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
setStyleSheet(R"(
#back_btn {
font-size: 50px;
margin: 0px;
padding: 15px;
border-width: 0;
border-radius: 30px;
color: #dddddd;
background-color: #393939;
}
#back_btn:pressed {
background-color: #4a4a4a;
}
)");
main_layout = new QStackedLayout(this);
ListWidget *list = new ListWidget(this, false);
@@ -38,28 +23,7 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
QObject::connect(uiState(), &UIState::offroadTransition, this, &LongitudinalPanel::refresh);
slcControl = new SpeedLimitControl(
"SpeedLimitControl",
tr("Speed Limit Control (SLC)"),
tr("When you engage ACC, you will be prompted to set the cruising speed to the speed limit of the road adjusted by the Offset and Source Policy specified, or the current driving speed. "
"The maximum cruising speed will always be the MAX set speed."),
"",
this);
list->addItem(slcControl);
connect(slcControl, &SpeedLimitControl::slcSettingsButtonClicked, [=]() {
cruisePanelScroller->setLastScrollPosition();
main_layout->setCurrentWidget(slcScreen);
});
slcScreen = new SpeedLimitControlSubpanel(this);
connect(slcScreen, &SpeedLimitControlSubpanel::backPress, [=]() {
cruisePanelScroller->restoreScrollPosition();
main_layout->setCurrentWidget(cruisePanelScreen);
});
main_layout->addWidget(cruisePanelScreen);
main_layout->addWidget(slcScreen);
main_layout->setCurrentWidget(cruisePanelScreen);
refresh(offroad);
}
@@ -8,11 +8,8 @@
#pragma once
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/slc/speed_limit_control_subpanel.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
#include "selfdrive/ui/sunnypilot/ui.h"
class LongitudinalPanel : public QWidget {
Q_OBJECT
@@ -32,6 +29,4 @@ private:
ScrollViewSP *cruisePanelScroller = nullptr;
QWidget *cruisePanelScreen = nullptr;
CustomAccIncrement *customAccIncrement = nullptr;
SpeedLimitControlSubpanel *slcScreen;
SpeedLimitControl *slcControl;
};
@@ -34,6 +34,27 @@ SunnylinkPanel::SunnylinkPanel(QWidget *parent) : QFrame(parent) {
vlayout->setContentsMargins(50, 20, 50, 20);
auto *list = new ListWidget(this, false);
QVBoxLayout *titleLayout = new QVBoxLayout;
QLabel *title = new QLabel(tr("🚀 sunnylink 🚀"));
title->setStyleSheet("font-size: 90px; font-weight: 500; font-family: 'Noto Color Emoji';");
titleLayout->addWidget(title, 0, Qt::AlignCenter);
QLabel *sunnylinkDesc = new QLabel("<div align='center'><font color='green'>"+
tr("For secure backup, restore, and remote configuration")+ "</font></div>");
QLabel *sponsorMsg = new QLabel("<div align='center'><font color='orange'>"+
tr("Sponsorship isn't required for basic backup/restore") + "<br>" +
tr("Click the sponsor button for more details")+ "</font></div>");
sunnylinkDesc->setStyleSheet("font-size: 40px; font-weight: 100; font-family: 'Noto';");
sponsorMsg->setStyleSheet("font-size: 35px; font-weight: 100; font-family: 'Noto';");
titleLayout->addWidget(sunnylinkDesc, 0, Qt::AlignCenter);
titleLayout->addWidget(sponsorMsg, 0, Qt::AlignCenter);
list->addItem(titleLayout);
QString sunnylinkEnabledBtnDesc = tr("This is the master switch, it will allow you to cutoff any sunnylink requests should you want to do that.");
sunnylinkEnabledBtn = new ParamControl(
"SunnylinkEnabled",
@@ -18,7 +18,6 @@ ExpandableToggleRow::ExpandableToggleRow(const QString &param, const QString &ti
collapsibleWidget->setVisible(false);
QVBoxLayout *collapsible_layout = new QVBoxLayout();
collapsibleWidget->setLayout(collapsible_layout);
collapsible_layout->setContentsMargins(0, 20, 0, 20);
list = new ListWidgetSP(this, false);
@@ -7,10 +7,7 @@ See the LICENSE.md file in the root directory for more details.
from cereal import messaging, custom
from opendbc.car import structs
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.models.helpers import get_active_bundle
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
@@ -18,11 +15,8 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen
class LongitudinalPlannerSP:
def __init__(self, CP: structs.CarParams, mpc):
self.events_sp = EventsSP()
self.dec = DynamicExperimentalController(CP, mpc)
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
self.slc = SpeedLimitController(CP)
@property
def mlsim(self) -> bool:
@@ -35,17 +29,6 @@ class LongitudinalPlannerSP:
return self.dec.mode()
def update_v_cruise(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> float:
self.events_sp.clear()
self.slc.update(sm, v_ego, a_ego, v_cruise, self.events_sp)
v_cruise_slc = self.slc.speed_limit_offseted if self.slc.is_active else V_CRUISE_UNSET
v_cruise_final = min(v_cruise, v_cruise_slc)
return v_cruise_final
def update(self, sm: messaging.SubMaster) -> None:
self.dec.update(sm)
@@ -55,7 +38,6 @@ class LongitudinalPlannerSP:
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
longitudinalPlanSP.events = self.events_sp.to_msg()
# Dynamic Experimental Control
dec = longitudinalPlanSP.dec
@@ -63,13 +45,4 @@ class LongitudinalPlannerSP:
dec.enabled = self.dec.enabled()
dec.active = self.dec.active()
# Speed Limit Control
slc = longitudinalPlanSP.slc
slc.state = self.slc.state
slc.enabled = self.slc.is_enabled
slc.active = self.slc.is_active
slc.speedLimit = float(self.slc.speed_limit)
slc.speedLimitOffset = float(self.slc.speed_limit_offset)
slc.distToSpeedLimit = float(self.slc.distance)
pm.send('longitudinalPlanSP', plan_sp_send)
@@ -1,19 +0,0 @@
from cereal import custom
DEBUG = True
PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates.
TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before considering temp deactivation signal.
# Lookup table for speed limit percent offset depending on speed.
LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h
LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h
# Constants for Limit controllers.
LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide.
LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers.
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
@@ -1,25 +0,0 @@
from enum import IntEnum
class Source(IntEnum):
none = 0
car_state = 1
map_data = 2
class Policy(IntEnum):
map_data_only = 0
car_state_only = 1
map_data_priority = 2
car_state_priority = 3
combined = 4
class Engage(IntEnum):
auto = 0
class OffsetType(IntEnum):
default = 0
fixed = 1
percentage = 2
@@ -1,19 +0,0 @@
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import DEBUG, SpeedLimitControlState
from openpilot.common.swaglog import cloudlog
def debug(msg):
if not DEBUG:
return
cloudlog.debug(msg)
def description_for_state(speed_limit_control_state):
if speed_limit_control_state == SpeedLimitControlState.inactive:
return 'INACTIVE'
if speed_limit_control_state == SpeedLimitControlState.tempInactive:
return 'TEMP_INACTIVE'
if speed_limit_control_state == SpeedLimitControlState.adapting:
return 'ADAPTING'
if speed_limit_control_state == SpeedLimitControlState.active:
return 'ACTIVE'
@@ -1,284 +0,0 @@
import numpy as np
import time
from cereal import messaging, custom
from openpilot.common.constants import CV
from openpilot.common.params import Params
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \
PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy, Engage, OffsetType
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.selfdrive.modeld.constants import ModelConstants
EventNameSP = custom.OnroadEventSP.EventName
ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting)
ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.tempInactive, *ACTIVE_STATES)
class SpeedLimitController:
_speed_limit: float
_distance: float
_source: Source
_v_ego: float
_a_ego: float
_v_offset: float
def __init__(self, CP):
self.params = Params()
self._CP = CP
self._policy = self.params.get("SpeedLimitControlPolicy", return_default=True)
self._resolver = SpeedLimitResolver(self._policy)
self._last_params_update = 0.0
self._last_op_engaged_time = 0.0
self._is_metric = self.params.get_bool("IsMetric")
self._enabled = self.params.get_bool("SpeedLimitControl")
self._op_engaged = False
self._op_engaged_prev = False
self._v_ego = 0.
self._a_ego = 0.
self._v_offset = 0.
self._v_cruise_setpoint = 0.
self._v_cruise_setpoint_prev = 0.
self._v_cruise_setpoint_changed = False
self._speed_limit = 0.
self._speed_limit_prev = 0.
self._speed_limit_changed = False
self._distance = 0.
self._source = Source.none
self._state = SpeedLimitControlState.inactive
self._state_prev = SpeedLimitControlState.inactive
self._gas_pressed = False
self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
self._offset_type = OffsetType(self.params.get("SpeedLimitWarningValueOffset", return_default=True))
self._offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
self._warning_type = self.params.get("SpeedLimitWarningType", return_default=True)
self._warning_offset_type = OffsetType(self.params.get("SpeedLimitWarningOffsetType", return_default=True))
self._warning_offset_value = self.params.get("SpeedLimitWarningValueOffset", return_default=True)
self._engage_type = self._read_engage_type_param()
self._current_time = 0.
self._v_cruise_rounded = 0.
self._v_cruise_prev_rounded = 0.
self._speed_limit_offsetted_rounded = 0.
self._speed_limit_warning_offsetted_rounded = 0.
self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
# Mapping functions to state transitions
self.state_transition_strategy = {
# Transition functions for each state
SpeedLimitControlState.inactive: self.transition_state_from_inactive,
SpeedLimitControlState.tempInactive: self.transition_state_from_temp_inactive,
SpeedLimitControlState.adapting: self.transition_state_from_adapting,
SpeedLimitControlState.active: self.transition_state_from_active,
SpeedLimitControlState.preActive: self.transition_state_from_pre_active,
}
# FIXME-SP: unused?
# Solution functions mapped to respective states
self.acceleration_solutions = {
# Solution functions for each state
SpeedLimitControlState.tempInactive: self.get_current_acceleration_as_target,
SpeedLimitControlState.inactive: self.get_current_acceleration_as_target,
SpeedLimitControlState.adapting: self.get_adapting_state_target_acceleration,
SpeedLimitControlState.active: self.get_active_state_target_acceleration,
SpeedLimitControlState.preActive: self.get_current_acceleration_as_target,
}
@property
def state(self) -> SpeedLimitControlState:
return self._state
@state.setter
def state(self, value) -> None:
if value != self._state:
debug(f'Speed Limit Controller state: {description_for_state(value)}')
if value == SpeedLimitControlState.tempInactive:
# Reset previous speed limit to current value as to prevent going out of tempInactive in
# a single cycle when the speed limit changes at the same time the user has temporarily deactivated it.
self._speed_limit_prev = self._speed_limit
self._state = value
@property
def is_enabled(self) -> bool:
return self.state in ENABLED_STATES and self._enabled
@property
def is_active(self) -> bool:
return self.state in ACTIVE_STATES and self._enabled
@property
def speed_limit_offseted(self) -> float:
return self._speed_limit + self.speed_limit_offset
@property
def speed_limit_offset(self) -> float:
return self._get_offset(self._offset_type, self._offset_value)
@property
def speed_limit_warning_offset(self) -> float:
return self._get_offset(self._warning_offset_type, self._warning_offset_value)
@property
def speed_limit(self) -> float:
return self._speed_limit
@property
def distance(self) -> float:
return self._distance
@property
def source(self) -> Source:
return self._source
def _get_offset(self, offset_type: OffsetType, offset_value: int) -> float:
if offset_type == OffsetType.default:
return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit)
elif offset_type == OffsetType.fixed:
return offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
elif offset_type == OffsetType.percentage:
return offset_value * 0.01 * self._speed_limit
else:
raise NotImplementedError("Offset not supported")
def _update_v_cruise_setpoint_prev(self) -> None:
self._v_cruise_setpoint_prev = self._v_cruise_setpoint
def _update_params(self) -> None:
if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD:
self._enabled = self.params.get_bool("SpeedLimitControl")
self._offset_type = OffsetType(self.params.get("SpeedLimitWarningValueOffset", return_default=True))
self._offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
self._warning_type = self.params.get("SpeedLimitWarningType", return_default=True)
self._warning_offset_type = OffsetType(self.params.get("SpeedLimitWarningOffsetType", return_default=True))
self._warning_offset_value = self.params.get("SpeedLimitWarningValueOffset", return_default=True)
self._policy = Policy(self.params.get("SpeedLimitControlPolicy", return_default=True))
self._is_metric = self.params.get_bool("IsMetric")
self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
self._resolver.change_policy(self._policy)
self._engage_type = self._read_engage_type_param()
self._last_params_update = self._current_time
def _read_engage_type_param(self) -> Engage:
if self._pcm_cruise_op_long:
return Engage.auto
return Engage.auto
def _update_calculations(self) -> None:
# Update current velocity offset (error)
self._v_offset = self.speed_limit_offseted - self._v_ego
# Track the time op becomes active to prevent going to tempInactive right away after
# op enabling since controlsd will change the cruise speed every time on enabling and this will
# cause a temp inactive transition if the controller is updated before controlsd sets actual cruise
# speed.
if not self._op_engaged_prev and self._op_engaged:
self._last_op_engaged_time = self._current_time
# Update change tracking variables
self._speed_limit_changed = self._speed_limit != self._speed_limit_prev
self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev
self._speed_limit_prev = self._speed_limit
self._update_v_cruise_setpoint_prev() # always for Engage.auto
self._op_engaged_prev = self._op_engaged
self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._speed_factor))
self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._speed_factor))
self._speed_limit_offsetted_rounded = 0 if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_offset) * self._speed_factor))
self._speed_limit_warning_offsetted_rounded = 0 if self._speed_limit == 0 else \
int(round((self._speed_limit + self.speed_limit_warning_offset) * self._speed_factor))
def transition_state_from_inactive(self) -> None:
""" Make state transition from inactive state """
if self._engage_type == Engage.auto:
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitControlState.adapting
else:
self.state = SpeedLimitControlState.active
def transition_state_from_temp_inactive(self) -> None:
""" Make state transition from temporary inactive state """
if self._speed_limit_changed:
if self._engage_type == Engage.auto:
self.state = SpeedLimitControlState.inactive
def transition_state_from_pre_active(self) -> None:
""" Make state transition from preActive state """
def transition_state_from_adapting(self) -> None:
""" Make state transition from adapting state """
if self._v_offset >= LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitControlState.active
def transition_state_from_active(self) -> None:
""" Make state transition from active state """
if self._engage_type == Engage.auto:
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitControlState.adapting
def _state_transition(self) -> None:
self._state_prev = self._state
# In any case, if op is disabled, or speed limit control is disabled or no valid speed limit
# or gas is pressed, deactivate.
if not self._op_engaged or not self._enabled or self._speed_limit == 0:
self.state = SpeedLimitControlState.inactive
return
# In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed.
# Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations
# due to controlsd logic changing cruise setpoint when going active.
if self._engage_type == Engage.auto and self._v_cruise_setpoint_changed and \
self._current_time > (self._last_op_engaged_time + TEMP_INACTIVE_GUARD_PERIOD):
self.state = SpeedLimitControlState.tempInactive
return
self.state_transition_strategy[self.state]()
self._update_v_cruise_setpoint_prev() # always for Engage.auto
def get_current_acceleration_as_target(self) -> float:
""" When state is inactive or tempInactive, preserve current acceleration """
return self._a_ego
def get_adapting_state_target_acceleration(self) -> float:
""" In adapting state, calculate target acceleration based on speed limit and current velocity """
if self.distance > 0:
return (self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance)
return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
def get_active_state_target_acceleration(self) -> float:
""" In active state, aim to keep speed constant around control time horizon """
return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
def _update_events(self, events_sp: EventsSP) -> None:
if self.is_active:
if self._engage_type == Engage.auto:
if self._state_prev not in ACTIVE_STATES:
events_sp.add(EventNameSP.speedLimitActive)
elif self._speed_limit_changed != 0:
events_sp.add(EventNameSP.speedLimitValueChange)
def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise_setpoint: float, events_sp: EventsSP) -> None:
_car_state = sm['carState']
self._op_engaged = sm['carControl'].longActive
self._v_ego = v_ego
self._a_ego = a_ego
self._v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
self._gas_pressed = _car_state.gasPressed
self._current_time = time.monotonic()
self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm)
self._update_params()
self._update_calculations()
self._state_transition()
self._update_events(events_sp)
@@ -1,119 +0,0 @@
import time
import numpy as np
from cereal import messaging
from openpilot.common.gps import get_gps_location_service
from openpilot.common.params import Params
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import debug
class SpeedLimitResolver:
_limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources
_distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources
_v_ego: float
_current_speed_limit: float
def __init__(self, policy: Policy):
self._gps_location_service = get_gps_location_service(Params())
self._limit_solutions = {}
self._distance_solutions = {}
self._policy = policy
self._policy_to_sources_map = {
Policy.car_state_only: [Source.car_state],
Policy.car_state_priority: [Source.car_state, Source.map_data],
Policy.map_data_priority: [Source.map_data, Source.car_state],
Policy.map_data_only: [Source.map_data],
Policy.combined: [Source.car_state, Source.map_data],
}
for source in Source:
self._reset_limit_sources(source)
def change_policy(self, policy: Policy) -> None:
self._policy = policy
def _reset_limit_sources(self, source: Source) -> None:
self._limit_solutions[source] = 0.
self._distance_solutions[source] = 0.
def resolve(self, v_ego: float, current_speed_limit: float, sm: messaging.SubMaster) -> tuple[float, float, Source]:
self._v_ego = v_ego
self._current_speed_limit = current_speed_limit
self._resolve_limit_sources(sm)
return self._consolidate()
def _resolve_limit_sources(self, sm: messaging.SubMaster) -> None:
"""Get limit solutions from each data source"""
self._get_from_car_state(sm)
self._get_from_map_data(sm)
def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
self._reset_limit_sources(Source.car_state)
self._limit_solutions[Source.car_state] = sm['carStateSP'].speedLimit
self._distance_solutions[Source.car_state] = 0.
def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
self._reset_limit_sources(Source.map_data)
self._process_map_data(sm)
def _process_map_data(self, sm: messaging.SubMaster) -> None:
gps_data = sm[self._gps_location_service]
map_data = sm['liveMapDataSP']
gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}')
return
speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
gps_data = sm[self._gps_location_service]
map_data = sm['liveMapDataSP']
distance_since_fix = self._v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
self._limit_solutions[Source.map_data] = speed_limit
self._distance_solutions[Source.map_data] = 0.
if 0. < next_speed_limit < self._v_ego:
adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC
adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
if distance_to_speed_limit_ahead <= adapt_distance:
self._limit_solutions[Source.map_data] = next_speed_limit
self._distance_solutions[Source.map_data] = distance_to_speed_limit_ahead
def _consolidate(self) -> tuple[float, float, Source]:
source = self._get_source_solution_according_to_policy()
self.speed_limit = self._limit_solutions[source] if source else 0.
self.distance = self._distance_solutions[source] if source else 0.
self.source = source or Source.none
debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}')
return self.speed_limit, self.distance, self.source
def _get_source_solution_according_to_policy(self) -> Source | None:
sources_for_policy = self._policy_to_sources_map[self._policy]
if self._policy != Policy.combined:
# They are ordered in the order of preference, so we pick the first that's non zero
for source in sources_for_policy:
if self._limit_solutions[source] > 0.:
return Source(source)
limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float)
sources = np.array([source.value for source in sources_for_policy], dtype=int)
if len(limits) > 0:
min_idx = np.argmin(limits)
return Source(sources[min_idx])
return None
@@ -1,58 +0,0 @@
"""
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 custom
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
EventNameSP = custom.OnroadEventSP.EventName
State = custom.LongitudinalPlanSP.SpeedLimitControlState
ACTIVE_STATES = (State.active, State.adapting)
ENABLED_STATES = (State.preActive, State.tempInactive, *ACTIVE_STATES)
class StateMachine:
def __init__(self):
self.state = State.inactive
def update(self, events_sp: EventsSP) -> tuple[bool, bool]:
# INACTIVE
if self.state == State.inactive:
if events_sp.has(EventNameSP.speedLimitAdapting):
self.state = State.adapting
elif events_sp.has(EventNameSP.speedLimitActive):
self.state = State.active
# ACTIVE
elif self.state == State.active:
if events_sp.has(EventNameSP.speedLimitDisable):
self.state = State.inactive
elif events_sp.has(EventNameSP.speedLimitUserCancel):
self.state = State.tempInactive
elif events_sp.has(EventNameSP.speedLimitAdapting):
self.state = State.adapting
# ADAPTING
elif self.state == State.adapting:
if events_sp.has(EventNameSP.speedLimitDisable):
self.state = State.inactive
elif events_sp.has(EventNameSP.speedLimitUserCancel):
self.state = State.tempInactive
elif events_sp.has(EventNameSP.speedLimitReached):
self.state = State.active
# TEMP INACTIVE
elif self.state == State.tempInactive:
if events_sp.has(EventNameSP.speedLimitDisable):
self.state = State.inactive
elif events_sp.has(EventNameSP.speedLimitValueChange):
# When speed limit changes, reactivate
self.state = State.inactive
enabled = self.state in ENABLED_STATES
active = self.state in ACTIVE_STATES
return enabled, active
@@ -1,130 +0,0 @@
import random
import time
import pytest
from pytest_mock import MockerFixture
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE
# from selfdrive.controls.lib.speed_limit_controller_tbd import SpeedLimitResolver as OriginalSpeedLimitResolver
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver as RefactoredSpeedLimitResolver
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy
def create_mock(properties, mocker: MockerFixture):
mock = mocker.MagicMock()
for _property, value in properties.items():
setattr(mock, _property, value)
return mock
def setup_sm_mock(mocker: MockerFixture):
cruise_speed_limit = random.uniform(0, 120)
live_map_data_limit = random.uniform(0, 120)
car_state = create_mock({
'gasPressed': False,
'brakePressed': False,
'standstill': False,
}, mocker)
car_state_sp = create_mock({
'speedLimit': cruise_speed_limit,
}, mocker)
live_map_data = create_mock({
'speedLimit': live_map_data_limit,
'speedLimitValid': True,
'speedLimitAhead': 0.,
'speedLimitAheadValid': 0.,
'speedLimitAheadDistance': 0.,
}, mocker)
gps_data = create_mock({
'unixTimestampMillis': time.monotonic() * 1e3,
}, mocker)
sm_mock = mocker.MagicMock()
sm_mock.__getitem__.side_effect = lambda key: {
'carState': car_state,
'liveMapDataSP': live_map_data,
'carStateSP': car_state_sp,
'gpsLocation': gps_data,
}[key]
return sm_mock
parametrized_policies = pytest.mark.parametrize(
"policy, sm_key, function_key", [
(Policy.car_state_only, 'carStateSP', 'car_state'),
(Policy.car_state_priority, 'carStateSP', 'car_state'),
(Policy.map_data_only, 'liveMapDataSP', 'map_data'),
(Policy.map_data_priority, 'liveMapDataSP', 'map_data'),
],
ids=lambda val: val.name if hasattr(val, 'name') else str(val)
)
@pytest.mark.parametrize("resolver_class", [RefactoredSpeedLimitResolver], ids=["Refactored"])
class TestSpeedLimitResolverValidation:
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_initial_state(self, resolver_class, policy):
resolver = resolver_class(policy)
for source in Source:
if source in resolver._limit_solutions:
assert resolver._limit_solutions[source] == 0.
assert resolver._distance_solutions[source] == 0.
@parametrized_policies
def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
resolver = resolver_class(policy)
sm_mock = setup_sm_mock(mocker)
source_speed_limit = sm_mock[sm_key].speedLimit
# Assert the resolver
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
assert speed_limit == source_speed_limit
assert source == Source[function_key]
def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
resolver = resolver_class(Policy.combined)
sm_mock = setup_sm_mock(mocker)
socket_to_source = {'carStateSP': Source.car_state, 'liveMapDataSP': Source.map_data}
minimum_key, minimum_speed_limit = min(
((key, sm_mock[key].speedLimit) for key in
socket_to_source.keys()), key=lambda x: x[1])
# Assert the resolver
speed_limit, _, source = resolver.resolve(minimum_speed_limit, 0, sm_mock)
assert speed_limit == minimum_speed_limit
assert source == socket_to_source[minimum_key]
@parametrized_policies
def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
resolver = resolver_class(policy)
sm_mock = setup_sm_mock(mocker)
source_speed_limit = sm_mock[sm_key].speedLimit
# Assert the parsing
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
assert resolver._limit_solutions[Source[function_key]] == source_speed_limit
assert resolver._distance_solutions[Source[function_key]] == 0.
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
v_ego = 50
resolver = resolver_class(policy)
sm_mock = setup_sm_mock(mocker)
_speed_limit, _distance, _source = resolver.resolve(v_ego, 0, sm_mock)
# After resolution
assert _speed_limit is not None
assert _distance is not None
assert _source is not None
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
resolver = resolver_class(policy)
sm_mock = mocker.MagicMock()
sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
resolver._get_from_map_data(sm_mock)
assert resolver._limit_solutions[Source.map_data] == 0.
assert resolver._distance_solutions[Source.map_data] == 0.
+1 -26
View File
@@ -1,6 +1,4 @@
import cereal.messaging as messaging
from cereal import log, car, custom
from openpilot.common.constants import CV
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
@@ -16,17 +14,6 @@ EventNameSP = custom.OnroadEventSP.EventName
EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
speedLimit = sm['longitudinalPlanSP'].slc.speedLimit
speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
return Alert(
message,
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
class EventsSP(EventsBase):
def __init__(self):
super().__init__()
@@ -145,18 +132,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
EventNameSP.pedalPressedAlertOnly: {
ET.WARNING: NoEntryAlert("Pedal Pressed")
},
EventNameSP.speedLimitActive: {
ET.WARNING: Alert(
"Set speed changed to match posted speed limit",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 3.),
},
EventNameSP.speedLimitValueChange: {
ET.WARNING: speed_limit_adjust_alert,
},
}
}