Compare commits
3 Commits
clearpilot
...
speed-limi
| Author | SHA1 | Date | |
|---|---|---|---|
| adcffad276 | |||
| 6e7117b177 | |||
| 8ccdb47c88 |
@@ -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},
|
||||
@@ -250,7 +251,17 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
|
||||
{"CarSpeedLimitWarning", PERSISTENT},
|
||||
{"CarSpeedLimitLiteral", PERSISTENT},
|
||||
|
||||
{"CarIsMetric", PERSISTENT},
|
||||
|
||||
{"ClearpilotSpeedDisplay", PERSISTENT},
|
||||
{"ClearpilotSpeedLimitDisplay", PERSISTENT},
|
||||
{"ClearpilotHasSpeed", PERSISTENT},
|
||||
{"ClearpilotIsMetric", PERSISTENT},
|
||||
{"ClearpilotSpeedUnit", PERSISTENT},
|
||||
{"ClearpilotCruiseWarning", PERSISTENT},
|
||||
{"ClearpilotCruiseWarningSpeed", PERSISTENT},
|
||||
{"ClearpilotPlayDing", PERSISTENT},
|
||||
|
||||
// {"SpeedLimitLatDesired", PERSISTENT},
|
||||
// {"SpeedLimitVTSC", PERSISTENT},
|
||||
|
||||
|
||||
@@ -212,6 +212,7 @@ class CarState(CarStateBase):
|
||||
|
||||
# self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
|
||||
self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
|
||||
self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"])
|
||||
|
||||
|
||||
@@ -420,6 +421,7 @@ class CarState(CarStateBase):
|
||||
# print(self.calculate_speed_limit(cp, cp_cam))
|
||||
# self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
|
||||
self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
|
||||
|
||||
# CLEARPILOT: telemetry logging — disabled, re-enable when needed
|
||||
# speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
|
||||
|
||||
@@ -7,7 +7,9 @@ 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 ding (trigger speed limit ding sound)
|
||||
python3 -m selfdrive.clearpilot.bench_cmd debugbutton (simulate LKAS debug button press)
|
||||
python3 -m selfdrive.clearpilot.bench_cmd dump
|
||||
python3 -m selfdrive.clearpilot.bench_cmd wait_ready
|
||||
@@ -89,6 +91,7 @@ def main():
|
||||
"speed": "BenchSpeed",
|
||||
"speedlimit": "BenchSpeedLimit",
|
||||
"cruise": "BenchCruiseSpeed",
|
||||
"cruiseactive": "BenchCruiseActive",
|
||||
"engaged": "BenchEngaged",
|
||||
}
|
||||
|
||||
@@ -106,6 +109,10 @@ def main():
|
||||
elif cmd == "wait_ready":
|
||||
wait_ready()
|
||||
|
||||
elif cmd == "ding":
|
||||
params.put("ClearpilotPlayDing", "1")
|
||||
print("Ding triggered")
|
||||
|
||||
elif cmd == "debugbutton":
|
||||
# Simulate LKAS debug button — same state machine as controlsd.clearpilot_state_control()
|
||||
current = params.get_int("ScreenDisplayMode")
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -21,8 +22,8 @@ import time
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log, car
|
||||
from openpilot.common.params import Params
|
||||
|
||||
MS_PER_MPH = 0.44704
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
|
||||
|
||||
|
||||
def main():
|
||||
@@ -33,9 +34,13 @@ 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")
|
||||
|
||||
# ClearPilot speed processing
|
||||
speed_state = SpeedState()
|
||||
|
||||
# thermald handles deviceState (reads our fake pandaStates for ignition)
|
||||
pm = messaging.PubMaster([
|
||||
"pandaStates", "carState", "controlsState",
|
||||
@@ -61,11 +66,25 @@ 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 * MS_PER_MPH
|
||||
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,
|
||||
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:
|
||||
dat = messaging.new_message("pandaStates", 1)
|
||||
@@ -82,7 +101,7 @@ def main():
|
||||
cs.standstill = speed_ms < 0.1
|
||||
cs.cruiseState.available = True
|
||||
cs.cruiseState.enabled = engaged
|
||||
cs.cruiseState.speed = cruise_mph * MS_PER_MPH if cruise_mph > 0 else 0
|
||||
cs.cruiseState.speed = cruise_mph * CV.MPH_TO_MS if cruise_mph > 0 else 0
|
||||
pm.send("carState", dat)
|
||||
|
||||
# controlsState — 10 Hz
|
||||
|
||||
BIN
selfdrive/clearpilot/sounds/ding.wav
Normal file
BIN
selfdrive/clearpilot/sounds/ding.wav
Normal file
Binary file not shown.
90
selfdrive/clearpilot/speed_logic.py
Normal file
90
selfdrive/clearpilot/speed_logic.py
Normal file
@@ -0,0 +1,90 @@
|
||||
"""
|
||||
ClearPilot speed processing module.
|
||||
|
||||
Shared logic for converting raw speed and speed limit data into display-ready
|
||||
values. Called from controlsd (live mode) and bench_onroad (bench mode).
|
||||
|
||||
Reads raw inputs, converts to display units (mph or kph based on car's CAN
|
||||
unit setting), detects speed limit changes, and writes results to params_memory
|
||||
for the onroad UI to read.
|
||||
"""
|
||||
import math
|
||||
import time
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
|
||||
|
||||
class SpeedState:
|
||||
def __init__(self):
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
self.prev_speed_limit = 0
|
||||
|
||||
# Ding state tracking
|
||||
self.last_ding_time = 0.0
|
||||
self.prev_warning = ""
|
||||
self.prev_warning_speed_limit = 0
|
||||
|
||||
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.
|
||||
"""
|
||||
now = time.monotonic()
|
||||
|
||||
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))
|
||||
|
||||
self.prev_speed_limit = speed_limit_int
|
||||
|
||||
# Write display-ready values to params_memory
|
||||
self.params_memory.put("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0")
|
||||
self.params_memory.put("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "")
|
||||
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
|
||||
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)
|
||||
|
||||
# Ding logic: play when warning sign appears or speed limit changes while visible
|
||||
should_ding = False
|
||||
if warning:
|
||||
if not self.prev_warning:
|
||||
# Warning sign just appeared
|
||||
should_ding = True
|
||||
elif speed_limit_int != self.prev_warning_speed_limit:
|
||||
# Speed limit changed while warning sign is visible
|
||||
should_ding = True
|
||||
|
||||
if should_ding and now - self.last_ding_time >= 30:
|
||||
self.params_memory.put("ClearpilotPlayDing", "1")
|
||||
self.last_ding_time = now
|
||||
|
||||
self.prev_warning = warning
|
||||
self.prev_warning_speed_limit = speed_limit_int if warning else 0
|
||||
@@ -38,6 +38,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUIS
|
||||
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
|
||||
from openpilot.selfdrive.clearpilot.telemetry import tlog
|
||||
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
|
||||
|
||||
SOFT_DISABLE_TIME = 3 # seconds
|
||||
@@ -80,6 +81,10 @@ class Controls:
|
||||
self.params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
|
||||
# ClearPilot speed processing
|
||||
self.speed_state = SpeedState()
|
||||
self.speed_state_frame = 0
|
||||
|
||||
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
|
||||
|
||||
with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg:
|
||||
@@ -108,8 +113,9 @@ class Controls:
|
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
|
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
|
||||
'testJoystick', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets,
|
||||
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
|
||||
'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets,
|
||||
ignore_alive=ignore+['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'],
|
||||
ignore_valid=['testJoystick', 'gpsLocation'],
|
||||
frequency=int(1/DT_CTRL))
|
||||
|
||||
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
|
||||
@@ -1269,6 +1275,21 @@ class Controls:
|
||||
new_mode = 0 if current == 3 else 3
|
||||
|
||||
self.params_memory.put_int("ScreenDisplayMode", new_mode)
|
||||
|
||||
# ClearPilot speed processing (~2 Hz at 100 Hz loop)
|
||||
self.speed_state_frame += 1
|
||||
if self.speed_state_frame % 50 == 0:
|
||||
gps = self.sm['gpsLocation'].getGpsLocation()
|
||||
has_gps = self.sm.valid('gpsLocation') and gps.getHasFix()
|
||||
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"
|
||||
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
|
||||
|
||||
def main():
|
||||
|
||||
@@ -89,6 +89,15 @@ def manager_init(frogpilot_functions) -> None:
|
||||
params_memory.put("VpnEnabled", "1")
|
||||
params_memory.put("ModelStandby", "0")
|
||||
params_memory.put("ModelStandbyTs", "0")
|
||||
params_memory.put("CarIsMetric", "0")
|
||||
params_memory.put("ClearpilotSpeedDisplay", "")
|
||||
params_memory.put("ClearpilotSpeedLimitDisplay", "0")
|
||||
params_memory.put("ClearpilotHasSpeed", "0")
|
||||
params_memory.put("ClearpilotIsMetric", "0")
|
||||
params_memory.put("ClearpilotSpeedUnit", "mph")
|
||||
params_memory.put("ClearpilotCruiseWarning", "")
|
||||
params_memory.put("ClearpilotCruiseWarningSpeed", "")
|
||||
params_memory.put("ClearpilotPlayDing", "0")
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
|
||||
if is_release_branch():
|
||||
|
||||
@@ -355,14 +355,15 @@ 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"));
|
||||
clpCruiseWarning = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarning"));
|
||||
clpCruiseWarningSpeed = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarningSpeed"));
|
||||
}
|
||||
|
||||
// auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
|
||||
@@ -431,7 +432,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 +456,18 @@ 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, cruise warning above it
|
||||
drawSpeedLimitSign(p);
|
||||
drawCruiseWarningSign(p);
|
||||
|
||||
// Draw FrogPilot widgets
|
||||
paintFrogPilotWidgets(p);
|
||||
}
|
||||
@@ -556,6 +561,164 @@ 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 = 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;
|
||||
|
||||
// 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(30, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
|
||||
|
||||
// "LIMIT" text
|
||||
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(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);
|
||||
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(30, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
|
||||
|
||||
// "LIMIT" text
|
||||
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(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();
|
||||
}
|
||||
|
||||
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});
|
||||
|
||||
@@ -51,6 +51,8 @@ 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);
|
||||
void drawCruiseWarningSign(QPainter &p);
|
||||
|
||||
QVBoxLayout *main_layout;
|
||||
QPixmap dm_img;
|
||||
@@ -59,6 +61,16 @@ 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;
|
||||
QString clpCruiseWarning;
|
||||
QString clpCruiseWarningSpeed;
|
||||
int clpParamFrame = 0;
|
||||
|
||||
float setSpeed;
|
||||
float speedLimit;
|
||||
bool is_cruise_set = false;
|
||||
|
||||
@@ -93,6 +93,27 @@ class Soundd:
|
||||
|
||||
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
|
||||
|
||||
# ClearPilot ding (plays independently of alerts)
|
||||
self.ding_sound = None
|
||||
self.ding_frame = 0
|
||||
self.ding_playing = False
|
||||
self.ding_check_counter = 0
|
||||
self._load_ding()
|
||||
|
||||
def _load_ding(self):
|
||||
ding_path = BASEDIR + "/selfdrive/clearpilot/sounds/ding.wav"
|
||||
try:
|
||||
wavefile = wave.open(ding_path, 'r')
|
||||
assert wavefile.getnchannels() == 1
|
||||
assert wavefile.getsampwidth() == 2
|
||||
assert wavefile.getframerate() == SAMPLE_RATE
|
||||
length = wavefile.getnframes()
|
||||
self.ding_sound = np.frombuffer(wavefile.readframes(length), dtype=np.int16).astype(np.float32) / (2**16/2)
|
||||
cloudlog.info(f"ClearPilot ding loaded: {length} frames")
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Failed to load ding sound: {e}")
|
||||
self.ding_sound = None
|
||||
|
||||
def load_sounds(self):
|
||||
self.loaded_sounds: dict[int, np.ndarray] = {}
|
||||
|
||||
@@ -137,7 +158,20 @@ class Soundd:
|
||||
written_frames += frames_to_write
|
||||
self.current_sound_frame += frames_to_write
|
||||
|
||||
return ret * self.current_volume
|
||||
ret = ret * self.current_volume
|
||||
|
||||
# Mix in ClearPilot ding (independent of alerts, always max volume)
|
||||
if self.ding_playing and self.ding_sound is not None:
|
||||
ding_remaining = len(self.ding_sound) - self.ding_frame
|
||||
if ding_remaining > 0:
|
||||
frames_to_write = min(ding_remaining, frames)
|
||||
ret[:frames_to_write] += self.ding_sound[self.ding_frame:self.ding_frame + frames_to_write] * MAX_VOLUME
|
||||
self.ding_frame += frames_to_write
|
||||
else:
|
||||
self.ding_playing = False
|
||||
self.ding_frame = 0
|
||||
|
||||
return ret
|
||||
|
||||
def callback(self, data_out: np.ndarray, frames: int, time, status) -> None:
|
||||
if status:
|
||||
@@ -197,6 +231,14 @@ class Soundd:
|
||||
|
||||
self.get_audible_alert(sm)
|
||||
|
||||
# ClearPilot: check for ding trigger at ~2Hz
|
||||
self.ding_check_counter += 1
|
||||
if self.ding_check_counter % 10 == 0 and self.ding_sound is not None:
|
||||
if self.params_memory.get("ClearpilotPlayDing") == b"1":
|
||||
self.params_memory.put("ClearpilotPlayDing", "0")
|
||||
self.ding_playing = True
|
||||
self.ding_frame = 0
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
assert stream.active
|
||||
|
||||
Reference in New Issue
Block a user