brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Speed limit controller

Browse Source 
Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself.

Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
Co-Authored-By: Efini <19368997+efini@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 6fa4b545a8
commit 22bfc8d9b7
16 changed files with 440 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
selfdrive/ui/qt/onroad.cc
View File

@@ -129,16 +129,23 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
QRect speedRect(rect().center().x() - 175, 50, 350, 350);
bool isSpeedClicked = speedRect.contains(e->pos());
if (isMaxSpeedClicked || isSpeedClicked) {
// Speed limit offset button
const QRect speedLimitRect(7, 250, 225, 225);
const bool isSpeedLimitClicked = speedLimitRect.contains(e->pos());
if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) {
// Check if the click was within the max speed area
if (isMaxSpeedClicked) {
reverseCruise = !params.getBool("ReverseCruise");
params.putBoolNonBlocking("ReverseCruise", reverseCruise);
paramsMemory.putBoolNonBlocking("FrogPilotTogglesUpdated", true);
// Check if the click was within the speed text area
} else {
} else if (isSpeedClicked) {
speedHidden = !params.getBool("HideSpeed");
params.putBoolNonBlocking("HideSpeed", speedHidden);
} else {
showSLCOffset = !params.getBool("ShowSLCOffset");
params.putBoolNonBlocking("ShowSLCOffset", showSLCOffset);
}
widgetClicked = true;
// If the click wasn't for anything specific, change the value of "ExperimentalMode"
@@ -490,10 +497,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit = slcOverridden ? slcOverriddenSpeed : slcSpeedLimit ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
if (slcSpeedLimit && !slcOverridden) {
speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
}
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || slcSpeedLimit;
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
is_metric = s.scene.is_metric;
speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph");
@@ -527,6 +537,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
QString speedLimitOffsetStr = (showSLCOffset) ? "+" + QString::number(std::nearbyint(slcSpeedLimitOffset)) : "–";
QString speedStr = QString::number(std::nearbyint(speed));
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
@@ -599,11 +610,23 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.setPen(QPen(blackColor(), 6));
p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
p.save();
p.setOpacity(slcOverridden ? 0.25 : 1.0);
if (showSLCOffset) {
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
p.setFont(InterFont(50, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
} else {
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
}
p.restore();
}
// EU (Vienna style) sign
@@ -1106,6 +1129,10 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
reverseCruise = true;
}
if (params.getBool("ShowSLCOffset")) {
showSLCOffset = true;
}
// Custom themes configuration
themeConfiguration = {
{1, {QString("frog_theme"), {QColor(23, 134, 68, 242), {{0.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.9))},
@@ -1160,6 +1187,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
obstacleDistanceStock = scene.obstacle_distance_stock;
roadNameUI = scene.road_name_ui;
showDriverCamera = scene.show_driver_camera;
slcOverridden = scene.speed_limit_overridden;
slcOverriddenSpeed = scene.speed_limit_overridden_speed;
slcSpeedLimit = scene.speed_limit;
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
stoppedEquivalence = scene.stopped_equivalence;
turnSignalLeft = scene.turn_signal_left;
turnSignalRight = scene.turn_signal_right;

5
selfdrive/ui/qt/onroad.h
View File

@@ -18,6 +18,7 @@ const int img_size = (btn_size / 4) * 3;
// FrogPilot global variables
static bool reverseCruise;
static bool showSLCOffset;
static bool speedHidden;
static double fps;
@@ -169,6 +170,7 @@ private:
bool muteDM;
bool roadNameUI;
bool showDriverCamera;
bool slcOverridden;
bool turnSignalLeft;
bool turnSignalRight;
bool useSI;
@@ -176,6 +178,9 @@ private:
float cruiseAdjustment;
float laneWidthLeft;
float laneWidthRight;
float slcOverriddenSpeed;
float slcSpeedLimit;
float slcSpeedLimitOffset;
int bearingDeg;
int cameraView;
int conditionalSpeed;

7
selfdrive/ui/ui.cc
View File

@@ -255,6 +255,12 @@ static void update_state(UIState *s) {
scene.obstacle_distance_stock = frogpilotLongitudinalPlan.getSafeObstacleDistanceStock();
scene.stopped_equivalence = frogpilotLongitudinalPlan.getStoppedEquivalenceFactor();
}
if (scene.speed_limit_controller) {
scene.speed_limit = frogpilotLongitudinalPlan.getSlcSpeedLimit();
scene.speed_limit_offset = frogpilotLongitudinalPlan.getSlcSpeedLimitOffset();
scene.speed_limit_overridden = frogpilotLongitudinalPlan.getSlcOverridden();
scene.speed_limit_overridden_speed = frogpilotLongitudinalPlan.getSlcOverriddenSpeed();
}
scene.adjusted_cruise = frogpilotLongitudinalPlan.getAdjustedCruise();
}
if (sm.updated("liveLocationKalman")) {
@@ -321,6 +327,7 @@ void ui_update_params(UIState *s) {
scene.mute_dm = params.getBool("FireTheBabysitter") && params.getBool("MuteDM");
scene.rotating_wheel = params.getBool("RotatingWheel");
scene.screen_brightness = params.getInt("ScreenBrightness");
scene.speed_limit_controller = params.getBool("SpeedLimitController");
scene.wheel_icon = params.getInt("WheelIcon");
}

5
selfdrive/ui/ui.h
View File

@@ -193,6 +193,8 @@ typedef struct UIScene {
bool rotating_wheel;
bool show_driver_camera;
bool show_fps;
bool speed_limit_controller;
bool speed_limit_overridden;
bool tethering_enabled;
bool turn_signal_left;
bool turn_signal_right;
@@ -205,6 +207,9 @@ typedef struct UIScene {
float path_edge_width;
float path_width;
float road_edge_width;
float speed_limit;
float speed_limit_offset;
float speed_limit_overridden_speed;
int bearing_deg;
int camera_view;
int conditional_speed;