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

feat: cruise warning signs and speed limit sign sizing

Browse Source 
Cruise warning sign appears above speed limit sign when cruise set
speed is too far from the speed limit:
- Red (over): cruise >= limit + 10 (if limit >= 50) or + 5 (if < 50)
- Green (under): cruise <= limit - 5
- Only when cruise active (not paused/disabled) and limit >= 20
- Nightrider mode: colored text/border on black background

Speed limit sign enlarged 5%. 20px gap between signs. Bench mode
gains cruiseactive command (0=disabled, 1=active, 2=paused).

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

3
common/params.cc
View File

@@ -217,6 +217,7 @@ std::unordered_map<std::string, uint32_t> keys = {
// FrogPilot parameters
{"AccelerationPath", PERSISTENT},
{"BenchCruiseActive", CLEAR_ON_MANAGER_START},
{"BenchCruiseSpeed", CLEAR_ON_MANAGER_START},
{"ClpUiState", CLEAR_ON_MANAGER_START},
{"BenchEngaged", CLEAR_ON_MANAGER_START},
@@ -257,6 +258,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"ClearpilotHasSpeed", PERSISTENT},
{"ClearpilotIsMetric", PERSISTENT},
{"ClearpilotSpeedUnit", PERSISTENT},
{"ClearpilotCruiseWarning", PERSISTENT},
{"ClearpilotCruiseWarningSpeed", PERSISTENT},
// {"SpeedLimitLatDesired", PERSISTENT},
// {"SpeedLimitVTSC", PERSISTENT},

2
selfdrive/clearpilot/bench_cmd.py
View File

@@ -7,6 +7,7 @@ Usage:
python3 -m selfdrive.clearpilot.bench_cmd speed 20
python3 -m selfdrive.clearpilot.bench_cmd speedlimit 45
python3 -m selfdrive.clearpilot.bench_cmd cruise 55
python3 -m selfdrive.clearpilot.bench_cmd cruiseactive 0|1|2 (0=disabled, 1=active, 2=paused)
python3 -m selfdrive.clearpilot.bench_cmd engaged 1
python3 -m selfdrive.clearpilot.bench_cmd debugbutton (simulate LKAS debug button press)
python3 -m selfdrive.clearpilot.bench_cmd dump
@@ -89,6 +90,7 @@ def main():
"speed": "BenchSpeed",
"speedlimit": "BenchSpeedLimit",
"cruise": "BenchCruiseSpeed",
"cruiseactive": "BenchCruiseActive",
"engaged": "BenchEngaged",
}

12
selfdrive/clearpilot/bench_onroad.py
View File

@@ -8,6 +8,7 @@ configurable vehicle state. Control values via params in /dev/shm/params:
BenchSpeed - vehicle speed in mph (default: 0)
BenchSpeedLimit - speed limit in mph (default: 0, 0=hidden)
BenchCruiseSpeed - cruise set speed in mph (default: 0, 0=not set)
BenchCruiseActive - 0=disabled, 1=active, 2=paused/standstill (default: 0)
BenchGear - P, D, R, N (default: P)
BenchEngaged - 0 or 1, cruise engaged (default: 0)
@@ -33,6 +34,7 @@ def main():
params_mem.put("BenchSpeed", "0")
params_mem.put("BenchSpeedLimit", "0")
params_mem.put("BenchCruiseSpeed", "0")
params_mem.put("BenchCruiseActive", "0")
params_mem.put("BenchGear", "P")
params_mem.put("BenchEngaged", "0")
@@ -64,16 +66,24 @@ def main():
speed_limit_mph = float((params_mem.get("BenchSpeedLimit", encoding="utf-8") or "0").strip())
cruise_mph = float((params_mem.get("BenchCruiseSpeed", encoding="utf-8") or "0").strip())
gear_str = (params_mem.get("BenchGear", encoding="utf-8") or "P").strip().upper()
cruise_active_str = (params_mem.get("BenchCruiseActive", encoding="utf-8") or "0").strip()
engaged = (params_mem.get("BenchEngaged", encoding="utf-8") or "0").strip() == "1"
speed_ms = speed_mph * CV.MPH_TO_MS
speed_limit_ms = speed_limit_mph * CV.MPH_TO_MS
cruise_ms = cruise_mph * CV.MPH_TO_MS
gear = gear_map.get(gear_str, car.CarState.GearShifter.park)
# Cruise state: 0=disabled, 1=active, 2=paused
cruise_active = cruise_active_str == "1"
cruise_standstill = cruise_active_str == "2"
# ClearPilot speed processing (~2 Hz at 10 Hz loop)
if frame % 5 == 0:
has_speed = speed_mph > 0
speed_state.update(speed_ms, has_speed, speed_limit_ms, is_metric=False)
speed_state.update(speed_ms, has_speed, speed_limit_ms, is_metric=False,
cruise_speed_ms=cruise_ms, cruise_active=cruise_active or cruise_standstill,
cruise_standstill=cruise_standstill)
# pandaStates — 10 Hz (thermald reads ignition from this)
if frame % 1 == 0:

27
selfdrive/clearpilot/speed_logic.py
View File

@@ -18,7 +18,8 @@ class SpeedState:
self.params_memory = Params("/dev/shm/params")
self.prev_speed_limit = 0
def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool):
def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool,
cruise_speed_ms: float = 0.0, cruise_active: bool = False, cruise_standstill: bool = False):
"""
Convert raw m/s values to display-ready strings and write to params_memory.
@@ -27,18 +28,24 @@ class SpeedState:
has_speed: whether we have a valid speed source
speed_limit_ms: current speed limit in m/s (from CAN or bench)
is_metric: True if car's CAN reports metric units (e.g. Canada)
cruise_speed_ms: cruise control set speed in m/s
cruise_active: True if cruise is engaged and not paused
cruise_standstill: True if cruise is paused at standstill
"""
if is_metric:
speed_display = speed_ms * CV.MS_TO_KPH
speed_limit_display = speed_limit_ms * CV.MS_TO_KPH
cruise_display = cruise_speed_ms * CV.MS_TO_KPH
unit = "km/h"
else:
speed_display = speed_ms * CV.MS_TO_MPH
speed_limit_display = speed_limit_ms * CV.MS_TO_MPH
cruise_display = cruise_speed_ms * CV.MS_TO_MPH
unit = "mph"
speed_int = int(math.floor(speed_display))
speed_limit_int = int(math.floor(speed_limit_display))
cruise_int = int(round(cruise_display))
# Detect speed limit changes (groundwork for future chime)
if speed_limit_int != self.prev_speed_limit:
@@ -50,3 +57,21 @@ class SpeedState:
self.params_memory.put("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0")
self.params_memory.put("ClearpilotSpeedUnit", unit)
self.params_memory.put("ClearpilotIsMetric", "1" if is_metric else "0")
# Cruise warning logic
# Only evaluate when speed limit >= 20 and cruise is active (not paused, not disabled)
warning = ""
warning_speed = ""
cruise_engaged = cruise_active and not cruise_standstill
if speed_limit_int >= 20 and cruise_engaged and cruise_int > 0:
over_threshold = 10 if speed_limit_int >= 50 else 5
if cruise_int >= speed_limit_int + over_threshold:
warning = "over"
warning_speed = str(cruise_int)
elif cruise_int <= speed_limit_int - 5:
warning = "under"
warning_speed = str(cruise_int)
self.params_memory.put("ClearpilotCruiseWarning", warning)
self.params_memory.put("ClearpilotCruiseWarningSpeed", warning_speed)

6
selfdrive/controls/controlsd.py
View File

@@ -1284,7 +1284,11 @@ class Controls:
speed_ms = gps.getSpeed() if has_gps else 0.0
speed_limit_ms = self.params_memory.get_float("CarSpeedLimit")
is_metric = (self.params_memory.get("CarIsMetric", encoding="utf-8") or "0") == "1"
self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric)
cruise_speed_ms = CS.cruiseState.speed
cruise_active = CS.cruiseState.enabled
cruise_standstill = CS.cruiseState.standstill
self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric,
cruise_speed_ms, cruise_active, cruise_standstill)
return CC

2
selfdrive/manager/manager.py
View File

@@ -95,6 +95,8 @@ def manager_init(frogpilot_functions) -> None:
params_memory.put("ClearpilotHasSpeed", "0")
params_memory.put("ClearpilotIsMetric", "0")
params_memory.put("ClearpilotSpeedUnit", "mph")
params_memory.put("ClearpilotCruiseWarning", "")
params_memory.put("ClearpilotCruiseWarningSpeed", "")
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
if is_release_branch():

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

@@ -362,6 +362,8 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
clpSpeedDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedDisplay"));
clpSpeedLimitDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedLimitDisplay"));
clpSpeedUnit = QString::fromStdString(paramsMemory.get("ClearpilotSpeedUnit"));
clpCruiseWarning = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarning"));
clpCruiseWarningSpeed = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarningSpeed"));
}
// auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
@@ -462,8 +464,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
drawText(p, rect().center().x(), 290, clpSpeedUnit, 200);
}
// CLEARPILOT: speed limit sign in lower-left
// CLEARPILOT: speed limit sign in lower-left, cruise warning above it
drawSpeedLimitSign(p);
drawCruiseWarningSign(p);
// Draw FrogPilot widgets
paintFrogPilotWidgets(p);
@@ -564,13 +567,13 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
p.save();
const int signW = 150;
const int signH = 190;
const int signW = 189;
const int signH = 239;
const int margin = 20;
const int borderW = 5;
const int innerBorderW = 3;
const int innerMargin = 8;
const int cornerR = 12;
const int borderW = 6;
const int innerBorderW = 4;
const int innerMargin = 10;
const int cornerR = 15;
// Position: 20px from lower-left corner
QRect signRect(margin, height() - signH - margin, signW, signH);
@@ -593,16 +596,16 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
// "SPEED" text
p.setPen(textColor);
p.setFont(InterFont(24, QFont::Bold));
p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
p.setFont(InterFont(30, QFont::Bold));
p.drawText(innerRect.adjusted(0, 15, 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");
p.setFont(InterFont(30, QFont::Bold));
p.drawText(innerRect.adjusted(0, 48, 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.setFont(InterFont(90, QFont::Bold));
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
} else {
// Normal: white background, black border and text
QColor borderColor(0, 0, 0);
@@ -621,16 +624,96 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
// "SPEED" text
p.setPen(textColor);
p.setFont(InterFont(24, QFont::Bold));
p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
p.setFont(InterFont(30, QFont::Bold));
p.drawText(innerRect.adjusted(0, 15, 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");
p.setFont(InterFont(30, QFont::Bold));
p.drawText(innerRect.adjusted(0, 48, 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.setFont(InterFont(90, QFont::Bold));
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
}
p.restore();
}
void AnnotatedCameraWidget::drawCruiseWarningSign(QPainter &p) {
// Only show when there's an active warning and the speed limit sign is visible
if (clpCruiseWarning.isEmpty() || clpCruiseWarningSpeed.isEmpty()) return;
if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return;
bool isOver = (clpCruiseWarning == "over");
if (!isOver && clpCruiseWarning != "under") return;
p.save();
// Same dimensions as speed limit sign
const int signW = 189;
const int signH = 239;
const int margin = 20;
const int borderW = 6;
const int innerBorderW = 4;
const int innerMargin = 10;
const int cornerR = 15;
const int gap = 20;
// Position: directly above the speed limit sign
int speedLimitY = height() - signH - margin;
QRect signRect(margin, speedLimitY - signH - gap, signW, signH);
if (nightriderMode) {
// Nightrider: black background with colored border/text
QColor accentColor = isOver ? QColor(220, 50, 50) : QColor(50, 180, 80);
p.setPen(QPen(accentColor, borderW));
p.setBrush(QColor(0, 0, 0, 220));
p.drawRoundedRect(signRect, cornerR, cornerR);
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
p.setPen(QPen(accentColor, innerBorderW));
p.setBrush(Qt::NoBrush);
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
// "CRUISE" text
p.setPen(accentColor);
p.setFont(InterFont(26, QFont::Bold));
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE");
// "SET" text
p.setFont(InterFont(26, QFont::Bold));
p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET");
// Cruise speed number
p.setFont(InterFont(90, QFont::Bold));
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
} else {
// Normal: colored background with white border/text
QColor bgColor = isOver ? QColor(200, 30, 30, 240) : QColor(40, 160, 60, 240);
QColor fgColor(255, 255, 255);
p.setPen(QPen(fgColor, borderW));
p.setBrush(bgColor);
p.drawRoundedRect(signRect, cornerR, cornerR);
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
p.setPen(QPen(fgColor, innerBorderW));
p.setBrush(Qt::NoBrush);
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
// "CRUISE" text
p.setPen(fgColor);
p.setFont(InterFont(26, QFont::Bold));
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE");
// "SET" text
p.setFont(InterFont(26, QFont::Bold));
p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET");
// Cruise speed number
p.setFont(InterFont(90, QFont::Bold));
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
}
p.restore();

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

@@ -52,6 +52,7 @@ 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);
void drawCruiseWarningSign(QPainter &p);
QVBoxLayout *main_layout;
QPixmap dm_img;
@@ -66,6 +67,8 @@ private:
QString clpSpeedDisplay;
QString clpSpeedLimitDisplay;
QString clpSpeedUnit;
QString clpCruiseWarning;
QString clpCruiseWarningSpeed;
int clpParamFrame = 0;
float setSpeed;