brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Actions 4 Packages Projects Releases Wiki Activity

feat: speed limit sign UI and speed processing pipeline

Browse Source 
New speed_logic.py module converts raw CAN speed limit and GPS speed
into display-ready params. Called from controlsd (live) and
bench_onroad (bench) at ~2Hz. UI reads params to render:
- Current speed (top center, hidden when 0 or no GPS)
- MUTCD speed limit sign (lower-left, normal + nightrider styles)
- Unit-aware display (mph/kph based on CAN DISTANCE_UNIT)

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
Brian Hanson
2026-04-15 03:15:07 +00:00
parent b84c268b6e
commit 8ccdb47c88
8 changed files with 203 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -355,14 +355,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
setSpeed *= KM_TO_MILE;
}
// CLEARPILOT: use GPS speed if available, hide speed display if no GPS fix
has_gps_speed = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix();
if (has_gps_speed) {
float gps_speed_ms = sm["gpsLocation"].getGpsLocation().getSpeed();
speed = std::max<float>(0.0, gps_speed_ms);
speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
} else {
speed = 0.0;
// CLEARPILOT: read speed and speed limit from params (written by speed_logic.py ~2Hz)
clpParamFrame++;
if (clpParamFrame % 10 == 0) { // ~2Hz at 20Hz UI update rate
clpHasSpeed = paramsMemory.get("ClearpilotHasSpeed") == "1";
clpSpeedDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedDisplay"));
clpSpeedLimitDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedLimitDisplay"));
clpSpeedUnit = QString::fromStdString(paramsMemory.get("ClearpilotSpeedUnit"));
}
// auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
@@ -431,7 +430,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
// QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
// QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
QString speedStr = QString::number(std::nearbyint(speed));
// QString speedStr = QString::number(std::nearbyint(speed));
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
p.restore();
@@ -455,14 +454,17 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
// Todo: lead speed
// Todo: Experimental speed
// CLEARPILOT: show GPS speed, hide when no fix
if (has_gps_speed && !scene.hide_speed) {
// CLEARPILOT: show speed from speed_logic params, hide when no speed or speed=0
if (clpHasSpeed && !clpSpeedDisplay.isEmpty() && !scene.hide_speed) {
p.setFont(InterFont(140, QFont::Bold));
drawText(p, rect().center().x(), 210, speedStr);
drawText(p, rect().center().x(), 210, clpSpeedDisplay);
p.setFont(InterFont(50));
drawText(p, rect().center().x(), 290, speedUnit, 200);
drawText(p, rect().center().x(), 290, clpSpeedUnit, 200);
}
// CLEARPILOT: speed limit sign in lower-left
drawSpeedLimitSign(p);
// Draw FrogPilot widgets
paintFrogPilotWidgets(p);
}
@@ -556,6 +558,84 @@ void AnnotatedCameraWidget::drawSpeedWidget(QPainter &p, int x, int y, const QSt
}
void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
// Hide when no speed limit or speed limit is 0
if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return;
p.save();
const int signW = 150;
const int signH = 190;
const int margin = 20;
const int borderW = 5;
const int innerBorderW = 3;
const int innerMargin = 8;
const int cornerR = 12;
// Position: 20px from lower-left corner
QRect signRect(margin, height() - signH - margin, signW, signH);
if (nightriderMode) {
// Nightrider: black background, light gray-blue border and text
QColor borderColor(160, 180, 210);
QColor textColor(160, 180, 210);
// Outer border
p.setPen(QPen(borderColor, borderW));
p.setBrush(QColor(0, 0, 0, 220));
p.drawRoundedRect(signRect, cornerR, cornerR);
// Inner border
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
p.setPen(QPen(borderColor, innerBorderW));
p.setBrush(Qt::NoBrush);
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
// "SPEED" text
p.setPen(textColor);
p.setFont(InterFont(24, QFont::Bold));
p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
// "LIMIT" text
p.setFont(InterFont(24, QFont::Bold));
p.drawText(innerRect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
// Speed limit number
p.setFont(InterFont(72, QFont::Bold));
p.drawText(innerRect.adjusted(0, 20, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
} else {
// Normal: white background, black border and text
QColor borderColor(0, 0, 0);
QColor textColor(0, 0, 0);
// Outer border
p.setPen(QPen(borderColor, borderW));
p.setBrush(QColor(255, 255, 255, 240));
p.drawRoundedRect(signRect, cornerR, cornerR);
// Inner border
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
p.setPen(QPen(borderColor, innerBorderW));
p.setBrush(Qt::NoBrush);
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
// "SPEED" text
p.setPen(textColor);
p.setFont(InterFont(24, QFont::Bold));
p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
// "LIMIT" text
p.setFont(InterFont(24, QFont::Bold));
p.drawText(innerRect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
// Speed limit number
p.setFont(InterFont(72, QFont::Bold));
p.drawText(innerRect.adjusted(0, 20, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
}
p.restore();
}
void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
QRect real_rect = p.fontMetrics().boundingRect(text);
real_rect.moveCenter({x, y - real_rect.height() / 2});

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

@@ -51,6 +51,7 @@ public:
private:
void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
void drawSpeedWidget(QPainter &p, int x, int y, const QString &title, const QString &speedLimitStr, QColor colorSpeed, int width = 176);
void drawSpeedLimitSign(QPainter &p);
QVBoxLayout *main_layout;
QPixmap dm_img;
@@ -59,6 +60,14 @@ private:
bool nightriderMode = false;
int displayMode = 0;
QString speedUnit;
// ClearPilot speed state (from params_memory, updated ~2Hz)
bool clpHasSpeed = false;
QString clpSpeedDisplay;
QString clpSpeedLimitDisplay;
QString clpSpeedUnit;
int clpParamFrame = 0;
float setSpeed;
float speedLimit;
bool is_cruise_set = false;