diff --git a/frogpilot/tinygrad_modeld/dmonitoringmodeld.py b/frogpilot/tinygrad_modeld/dmonitoringmodeld.py index 51d14c89c..7fcf06f3e 100644 --- a/frogpilot/tinygrad_modeld/dmonitoringmodeld.py +++ b/frogpilot/tinygrad_modeld/dmonitoringmodeld.py @@ -20,6 +20,7 @@ from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import sigmoid +from openpilot.system import sentry from openpilot.frogpilot.tinygrad_modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE @@ -31,6 +32,9 @@ PROCESS_NAME = "frogpilot.tinygrad_modeld.dmonitoringmodeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl' +# Fixed small stagger to reduce overlap with driving model GPU work +DM_STAGGER_SEC = 0.010 + class DriverStateResult(ctypes.Structure): _fields_ = [ @@ -160,6 +164,9 @@ def main(): if sm.updated["liveCalibration"]: calib[:] = np.array(sm["liveCalibration"].rpyCalib) + # Small fixed delay to reduce GPU overlap with driving model + time.sleep(DM_STAGGER_SEC) + t1 = time.perf_counter() model_output, gpu_execution_time = model.run(buf, calib, model_transform) t2 = time.perf_counter() diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 7a1c687de..d18f09ecc 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -1,4 +1,6 @@ #include +#include +#include #include "frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h" @@ -26,9 +28,21 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent) loadGif("../../frogpilot/assets/other_images/experimental_mode_icon.gif", experimentalModeIcon, QSize(btn_size / 2, btn_size / 2), this); QObject::connect(animationTimer, &QTimer::timeout, [this] { + if (totalFrames <= 0) { + if (animationTimer->isActive()) { + animationTimer->stop(); + } + animationFrameIndex = 0; + return; + } + animationFrameIndex = (animationFrameIndex + 1) % totalFrames; + update(); + }); + QObject::connect(frogpilotUIState(), &FrogPilotUIState::themeUpdated, this, [this] { + signalsLoaded = false; + updateSignals(); }); - QObject::connect(frogpilotUIState(), &FrogPilotUIState::themeUpdated, this, &FrogPilotAnnotatedCameraWidget::updateSignals); QObject::connect(uiState(), &UIState::offroadTransition, [this] { standstillTimer.invalidate(); @@ -64,7 +78,9 @@ void FrogPilotAnnotatedCameraWidget::showEvent(QShowEvent *event) { speedConversionMetrics = MS_TO_MPH; } - updateSignals(); + if (!signalsLoaded) { + updateSignals(); + } } void FrogPilotAnnotatedCameraWidget::updateSignals() { @@ -115,6 +131,11 @@ void FrogPilotAnnotatedCameraWidget::updateSignals() { signalWidth = firstImage.width(); totalFrames = signalImages.size(); + signalImagesMirrored.reserve(signalImages.size()); + for (const QPixmap &pixmap : signalImages) { + signalImagesMirrored.append(pixmap.transformed(QTransform().scale(-1, 1))); + } + if (isGif && signalStyle == "traditional") { signalMovement = (width() + signalWidth * 2) / totalFrames; @@ -131,6 +152,19 @@ void FrogPilotAnnotatedCameraWidget::updateSignals() { signalStyle = "None"; } + + if (!blindspotImages.isEmpty()) { + blindspotImagesMirrored.reserve(blindspotImages.size()); + for (const QPixmap &pixmap : blindspotImages) { + blindspotImagesMirrored.append(pixmap.transformed(QTransform().scale(-1, 1))); + } + } + + if (totalFrames <= 1 && animationTimer->isActive()) { + animationTimer->stop(); + } + + signalsLoaded = true; } void FrogPilotAnnotatedCameraWidget::updateState(const FrogPilotUIState &fs, const QJsonObject &frogpilot_toggles) { @@ -244,14 +278,22 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState paintStoppingPoint(p, scene, frogpilot_scene, frogpilot_toggles); } - if (!bigMapOpen && (carState.getLeftBlinker() || carState.getRightBlinker()) && signalStyle != "None") { - if (!animationTimer->isActive()) { - animationTimer->start(signalAnimationLength); + const bool signalActive = carState.getLeftBlinker() || carState.getRightBlinker(); + const bool shouldShowSignals = !bigMapOpen && signalActive && signalStyle != "None"; + const bool shouldAnimateSignals = shouldShowSignals && totalFrames > 1; + + if (shouldAnimateSignals) { + const int interval = signalAnimationLength > 0 ? signalAnimationLength : 500; + if (!animationTimer->isActive() || animationTimer->interval() != interval) { + animationTimer->start(interval); } - paintTurnSignals(p, carState); } else if (animationTimer->isActive()) { animationTimer->stop(); } + + if (shouldShowSignals) { + paintTurnSignals(p, carState); + } } void FrogPilotAnnotatedCameraWidget::paintAdjacentPaths(QPainter &p, const cereal::CarState::Reader &carState, const FrogPilotUIScene &frogpilot_scene, const QJsonObject &frogpilot_toggles) { @@ -944,14 +986,24 @@ void FrogPilotAnnotatedCameraWidget::paintTurnSignals(QPainter &p, const cereal: bool leftBlinker = carState.getLeftBlinker(); bool blindspotActive = leftBlinker ? carState.getLeftBlindspot() : carState.getRightBlindspot(); + const QVector &activeSignalImages = leftBlinker ? signalImages : signalImagesMirrored; + const QVector &activeBlindspotImages = leftBlinker ? blindspotImages : blindspotImagesMirrored; + + if (activeSignalImages.isEmpty()) { + p.restore(); + return; + } + + const int frameIndex = animationFrameIndex % activeSignalImages.size(); + if (signalStyle == "static") { int signalXPosition = leftBlinker ? (rect().center().x() * 0.75) - signalWidth : rect().center().x() * 1.25; int signalYPosition = signalHeight / 2; - if (blindspotActive && !blindspotImages.empty()) { - p.drawPixmap(signalXPosition, signalYPosition, signalWidth, signalHeight, blindspotImages[0].transformed(QTransform().scale(leftBlinker ? 1 : -1, 1))); + if (blindspotActive && !activeBlindspotImages.isEmpty()) { + p.drawPixmap(signalXPosition, signalYPosition, signalWidth, signalHeight, activeBlindspotImages.front()); } else { - p.drawPixmap(signalXPosition, signalYPosition, signalWidth, signalHeight, signalImages[animationFrameIndex].transformed(QTransform().scale(leftBlinker ? 1 : -1, 1))); + p.drawPixmap(signalXPosition, signalYPosition, signalWidth, signalHeight, activeSignalImages[frameIndex]); } } else { int signalXPosition; @@ -963,10 +1015,10 @@ void FrogPilotAnnotatedCameraWidget::paintTurnSignals(QPainter &p, const cereal: int signalYPosition = height() - signalHeight - alertHeight; - if (blindspotActive && !blindspotImages.empty()) { - p.drawPixmap(leftBlinker ? width() - signalWidth : 0, signalYPosition, signalWidth, signalHeight, blindspotImages[0].transformed(QTransform().scale(leftBlinker ? 1 : -1, 1))); + if (blindspotActive && !activeBlindspotImages.isEmpty()) { + p.drawPixmap(leftBlinker ? width() - signalWidth : 0, signalYPosition, signalWidth, signalHeight, activeBlindspotImages.front()); } else { - p.drawPixmap(signalXPosition, signalYPosition, signalWidth, signalHeight, signalImages[animationFrameIndex].transformed(QTransform().scale(leftBlinker ? 1 : -1, 1))); + p.drawPixmap(signalXPosition, signalYPosition, signalWidth, signalHeight, activeSignalImages[frameIndex]); } } diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index 933bc5098..fa5bc67c9 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -22,42 +22,42 @@ public: void paintRainbowPath(QPainter &p, QLinearGradient &bg, float lin_grad_point, SubMaster &sm); void updateState(const FrogPilotUIState &fs, const QJsonObject &frogpilot_toggles); - bool bigMapOpen; - bool hideBottomIcons; - bool isCruiseSet; - bool mapButtonVisible; - bool mutcdSpeedLimit; - bool rightHandDM; - bool viennaSpeedLimit; + bool bigMapOpen = false; + bool hideBottomIcons = false; + bool isCruiseSet = false; + bool mapButtonVisible = false; + bool mutcdSpeedLimit = false; + bool rightHandDM = false; + bool viennaSpeedLimit = false; int alertHeight; int frogHopCount; int signMargin; int standstillDuration; - float distanceConversion; - float setSpeed; - float speed; - float speedConversion; - float speedConversionMetrics; + float distanceConversion = 0.0f; + float setSpeed = 0.0f; + float speed = 0.0f; + float speedConversion = 0.0f; + float speedConversionMetrics = 0.0f; QColor blueColor(int alpha = 255) { return QColor(0, 0, 255, alpha); } QColor purpleColor(int alpha = 255) { return QColor(128, 0, 128, alpha); } - QPoint dmIconPosition; - QPoint experimentalButtonPosition; + QPoint dmIconPosition = {}; + QPoint experimentalButtonPosition = {}; - QRect leadTextRect; - QRect newSpeedLimitRect; - QRect setSpeedRect; - QRect speedLimitRect; + QRect leadTextRect = {}; + QRect newSpeedLimitRect = {}; + QRect setSpeedRect = {}; + QRect speedLimitRect = {}; - QSize defaultSize; + QSize defaultSize = {}; QString accelerationUnit; QString leadDistanceUnit; QString leadSpeedUnit; - QString signalStyle; + QString signalStyle = "None"; QString speedLimitOffsetStr; QString speedUnit; @@ -81,12 +81,12 @@ private: void paintTurnSignals(QPainter &p, const cereal::CarState::Reader &carState); void updateSignals(); - int animationFrameIndex; - int signalAnimationLength; - int signalHeight; - int signalMovement; - int signalWidth; - int totalFrames; + int animationFrameIndex = 0; + int signalAnimationLength = 0; + int signalHeight = 0; + int signalMovement = 0; + int signalWidth = 0; + int totalFrames = 0; Params params; Params params_memory{"/dev/shm/params"}; @@ -126,8 +126,12 @@ private: QString cscSpeedStr; - QTimer *animationTimer; + QTimer *animationTimer = nullptr; QVector blindspotImages; + QVector blindspotImagesMirrored; QVector signalImages; + QVector signalImagesMirrored; + + bool signalsLoaded = false; }; diff --git a/frogpilot/ui/qt/onroad/frogpilot_onroad.cc b/frogpilot/ui/qt/onroad/frogpilot_onroad.cc index 462fe96e6..1d8bbc4f5 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_onroad.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_onroad.cc @@ -1,5 +1,7 @@ #include "frogpilot/ui/qt/onroad/frogpilot_onroad.h" +#include + FrogPilotOnroadWindow::FrogPilotOnroadWindow(QWidget *parent) : QWidget(parent) { signalTimer = new QTimer(this); @@ -77,14 +79,14 @@ void FrogPilotOnroadWindow::paintFPS(QPainter &p, const QRect &rect) { static double minFPS = 99.9; static double totalFPS = 0.0; - static QList> fpsHistory; + static QQueue> fpsHistory; - fpsHistory.append({now, fps}); + fpsHistory.enqueue({now, fps}); totalFPS += fps; while (!fpsHistory.isEmpty() && now - fpsHistory.first().first > 60000) { totalFPS -= fpsHistory.first().second; - fpsHistory.removeFirst(); + fpsHistory.dequeue(); } double avgFPS = fpsHistory.isEmpty() ? 0.0 : totalFPS / fpsHistory.size(); diff --git a/frogpilot/ui/qt/widgets/developer_sidebar.cc b/frogpilot/ui/qt/widgets/developer_sidebar.cc index c16ffdb8f..55a9401cc 100644 --- a/frogpilot/ui/qt/widgets/developer_sidebar.cc +++ b/frogpilot/ui/qt/widgets/developer_sidebar.cc @@ -1,5 +1,7 @@ #include "frogpilot/ui/qt/widgets/developer_sidebar.h" +#include + void DeveloperSidebar::drawMetric(QPainter &p, const QPair &label, QColor c, int y) { const QRect rect = {12, y, 275, 126}; @@ -17,10 +19,20 @@ void DeveloperSidebar::drawMetric(QPainter &p, const QPair &la p.setPen(QColor(0xff, 0xff, 0xff)); p.setFont(InterFont(35, QFont::DemiBold)); - p.drawText(rect.adjusted(0, 0, -22, 0), Qt::AlignCenter, label.first + "\n" + label.second); + QString text = label.second.isEmpty() + ? label.first // use whole box for first string + : (label.first + "\n" + label.second); + + int flags = Qt::AlignHCenter | Qt::AlignVCenter; + if (label.second.isEmpty()) { + flags |= Qt::TextWordWrap; // enable wrapping for single-line metrics + } + + p.drawText(rect.adjusted(8, 8, -22, -8), flags, text); } -DeveloperSidebar::DeveloperSidebar(QWidget *parent) : QFrame(parent) { +DeveloperSidebar::DeveloperSidebar(QWidget *parent) + : QFrame(parent), hasActiveMetrics(false), metricsDrawn(false) { setAttribute(Qt::WA_OpaquePaintEvent); setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding); setFixedWidth(300); @@ -47,6 +59,14 @@ void DeveloperSidebar::updateTheme() { } metricColor = frogpilot_scene.use_stock_colors ? QColor(255, 255, 255) : frogpilot_scene.sidebar_color1; + + bool nextHasActiveMetrics = std::any_of(metricAssignments.begin(), metricAssignments.end(), + [](int metricId) { return metricId > 0; }); + if (metricsDrawn && !nextHasActiveMetrics) { + metricsDrawn = false; + update(); + } + hasActiveMetrics = nextHasActiveMetrics; } void DeveloperSidebar::resetVariables() { @@ -114,6 +134,10 @@ void DeveloperSidebar::updateState(const UIState &s, const FrogPilotUIState &fs) torqueLabel += QString(" - (%1%)").arg(maxTorque); } + if (!hasActiveMetrics) { + return; + } + accelerationStatus = ItemStatus(QPair(tr("ACCEL"), QString::number(acceleration, 'f', 2) + accelerationUnit), metricColor); accelerationJerkStatus = ItemStatus(QPair(tr("ACCEL JERK"), QString::number(frogpilotPlan.getAccelerationJerk())), metricColor); actuatorAccelerationStatus = ItemStatus(QPair(tr("ACT ACCEL"), QString::number(carControl.getActuators().getAccel() * accelerationConversion, 'f', 2) + accelerationUnit), metricColor); @@ -130,6 +154,15 @@ void DeveloperSidebar::updateState(const UIState &s, const FrogPilotUIState &fs) stiffnessFactorStatus = ItemStatus(QPair(tr("STEER STIFF"), QString::number(liveParameters.getStiffnessFactor(), 'f', 5)), metricColor); torqueStatus = ItemStatus(QPair(tr("TORQUE %"), torqueLabel), metricColor); + //get model name and cleanup for display + QString rawName = fs.frogpilot_toggles.value("model_name").toString(); + QString cleanName = rawName; + cleanName.remove(QRegExp("\\(.*\\)")); + cleanName.remove(QRegExp("[^A-Za-z0-9\\s\\-\\.:]")); // Remove emojis/symbols (keep only letters, numbers, spaces, dashes, dots, colons) + cleanName = cleanName.trimmed(); + modelNameStatus = ItemStatus(QPair(cleanName, ""), metricColor); + + metricsDrawn = true; update(); } diff --git a/frogpilot/ui/qt/widgets/developer_sidebar.h b/frogpilot/ui/qt/widgets/developer_sidebar.h index 473bcced9..ae2c34b44 100644 --- a/frogpilot/ui/qt/widgets/developer_sidebar.h +++ b/frogpilot/ui/qt/widgets/developer_sidebar.h @@ -23,6 +23,9 @@ private: std::vector metricAssignments; + bool hasActiveMetrics; + bool metricsDrawn; + QColor metricColor; ItemStatus accelerationJerkStatus; @@ -35,6 +38,7 @@ private: ItemStatus lateralEngagementStatus; ItemStatus longitudinalEngagementStatus; ItemStatus maxAccelerationStatus; + ItemStatus modelNameStatus; ItemStatus speedJerkStatus; ItemStatus steerAngleStatus; ItemStatus steerRatioStatus; diff --git a/frogpilot/ui/screenrecorder/blocking_queue.h b/frogpilot/ui/screenrecorder/blocking_queue.h index 81ca4d5c2..53cfb5b27 100644 --- a/frogpilot/ui/screenrecorder/blocking_queue.h +++ b/frogpilot/ui/screenrecorder/blocking_queue.h @@ -39,6 +39,17 @@ public: not_empty.notify_one(); } + void force_push(T &&item) { + { + std::unique_lock lk(mutex); + if (content.size() == capacity && !content.empty()) { + content.pop_front(); + } + content.push_back(std::move(item)); + } + not_empty.notify_one(); + } + bool try_push(T &&item) { { std::unique_lock lk(mutex); diff --git a/frogpilot/ui/screenrecorder/screenrecorder.cc b/frogpilot/ui/screenrecorder/screenrecorder.cc index 1633ceb68..175b5f242 100644 --- a/frogpilot/ui/screenrecorder/screenrecorder.cc +++ b/frogpilot/ui/screenrecorder/screenrecorder.cc @@ -1,21 +1,19 @@ #include "libyuv.h" +#include +#include + #include "selfdrive/ui/qt/util.h" #include "frogpilot/ui/screenrecorder/screenrecorder.h" constexpr int MAX_DURATION = 1000 * 60 * 5; -constexpr int SCREEN_WIDTH = 2160; -constexpr int SCREEN_HEIGHT = 1080; - const QDir RECORDINGS_FOLDER("/data/media/screen_recordings"); ScreenRecorder::ScreenRecorder(QWidget *parent) : QPushButton(parent) { setFixedSize(btn_size, btn_size); - rgbScaleBuffer.resize(SCREEN_WIDTH * SCREEN_HEIGHT * 4); - rootWidget = topWidget(this); QObject::connect(this, &QPushButton::clicked, this, &ScreenRecorder::toggleRecording); @@ -23,6 +21,36 @@ ScreenRecorder::ScreenRecorder(QWidget *parent) : QPushButton(parent) { QObject::connect(uiState(), &UIState::uiUpdate, this, &ScreenRecorder::updateState); } +bool ScreenRecorder::prepareForRecording(QImage frame) { + recordingWidth = 0; + recordingHeight = 0; + frameBufferSize = 0; + rgbScaleBuffer.clear(); + + if (frame.isNull()) { + return false; + } + + frame = frame.convertToFormat(QImage::Format_RGBA8888); + if (frame.isNull() || frame.width() <= 0 || frame.height() <= 0) { + return false; + } + + recordingWidth = frame.width(); + recordingHeight = frame.height(); + frameBufferSize = static_cast(recordingWidth) * recordingHeight * 4; + if (frameBufferSize == 0) { + return false; + } + + rgbScaleBuffer.resize(frameBufferSize); + + imageQueue.clear(); + imageQueue.push(std::move(frame)); + + return true; +} + void ScreenRecorder::updateState() { if (!recording) { return; @@ -35,7 +63,44 @@ void ScreenRecorder::updateState() { } if (frameCount % 2 == 0) { - imageQueue.push(rootWidget->grab().toImage()); + if (!rootWidget || recordingWidth <= 0 || recordingHeight <= 0) { + return; + } + + QImage frame = rootWidget->grab().toImage(); + + if (!frame.isNull() && (frame.width() != recordingWidth || frame.height() != recordingHeight)) { + frame = frame.scaled(recordingWidth, recordingHeight, Qt::IgnoreAspectRatio, Qt::SmoothTransformation); + } + + if (!imageQueue.try_push(std::move(frame))) { + QImage dropped_frame; + + if (imageQueue.try_pop(dropped_frame)) { + if (frame.isNull()) { + frame = rootWidget->grab().toImage(); + } + + if (!frame.isNull() && (frame.width() != recordingWidth || frame.height() != recordingHeight)) { + frame = frame.scaled(recordingWidth, recordingHeight, Qt::IgnoreAspectRatio, Qt::SmoothTransformation); + } + + if (!imageQueue.try_push(std::move(frame))) { + // Drop backlog and enqueue the newest frame to keep the recorder responsive. + imageQueue.clear(); + + if (frame.isNull()) { + frame = rootWidget->grab().toImage(); + } + + if (!frame.isNull() && (frame.width() != recordingWidth || frame.height() != recordingHeight)) { + frame = frame.scaled(recordingWidth, recordingHeight, Qt::IgnoreAspectRatio, Qt::SmoothTransformation); + } + + imageQueue.try_push(std::move(frame)); + } + } + } } frameCount += 1; @@ -46,11 +111,27 @@ void ScreenRecorder::toggleRecording() { } void ScreenRecorder::startRecording() { - encoder = std::make_unique(RECORDINGS_FOLDER.path().toStdString().c_str(), SCREEN_WIDTH, SCREEN_HEIGHT, UI_FREQ * 2, 12 * 1024 * 1024); - encoder->encoder_open((QDateTime::currentDateTime().toString("MMMM_dd_yyyy-hh-mmAP").toStdString() + ".mp4").c_str()); + if (!rootWidget) { + return; + } + + QImage initialFrame = rootWidget->grab().toImage(); + if (!prepareForRecording(std::move(initialFrame))) { + return; + } + + encoder = std::make_unique(RECORDINGS_FOLDER.path().toStdString().c_str(), recordingWidth, recordingHeight, UI_FREQ * 2, 12 * 1024 * 1024); + + std::string filename = QDateTime::currentDateTime().toString("MMMM_dd_yyyy-hh-mmAP").toStdString() + ".mp4"; + encoder->encoder_open(filename.c_str()); if (!encoder->is_open) { encoder.reset(); + imageQueue.clear(); + frameBufferSize = 0; + recordingWidth = 0; + recordingHeight = 0; + rgbScaleBuffer.clear(); return; } @@ -70,10 +151,17 @@ void ScreenRecorder::stopRecording() { encodingThread.join(); } + imageQueue.clear(); + if (encoder) { encoder->encoder_close(); encoder.reset(); } + + frameBufferSize = 0; + recordingWidth = 0; + recordingHeight = 0; + rgbScaleBuffer.clear(); } QImage ScreenRecorder::synthesizeFrame(const QImage &frame1, const QImage &frame2, double alpha) { @@ -106,17 +194,33 @@ void ScreenRecorder::encodeImage() { if (imageQueue.pop_wait_for(image, std::chrono::milliseconds(1000 / UI_FREQ))) { image = image.convertToFormat(QImage::Format_RGBA8888); - if (!previousImage.isNull()) { + if (image.isNull() || recordingWidth <= 0 || recordingHeight <= 0 || frameBufferSize == 0 || !encoder || !encoder->is_open) { + previousImage = QImage(); + continue; + } + + if (image.width() != recordingWidth || image.height() != recordingHeight) { + previousImage = QImage(); + continue; + } + + size_t expectedSize = static_cast(recordingWidth) * recordingHeight * 4; + if (frameBufferSize != expectedSize) { + frameBufferSize = expectedSize; + rgbScaleBuffer.resize(frameBufferSize); + } + + if (!previousImage.isNull() && previousImage.size() == image.size()) { double alpha = std::clamp((currentTimestamp - previousTimestamp) / (1000.0 / UI_FREQ), 0.0, 1.0); QImage syntheticImage = synthesizeFrame(previousImage, image, alpha); - std::copy(syntheticImage.bits(), syntheticImage.bits() + SCREEN_WIDTH * SCREEN_HEIGHT * 4, rgbScaleBuffer.data()); - encoder->encode_frame_rgba(rgbScaleBuffer.data(), SCREEN_WIDTH, SCREEN_HEIGHT, (previousTimestamp + currentTimestamp) / 2); + std::memcpy(rgbScaleBuffer.data(), syntheticImage.bits(), frameBufferSize); + encoder->encode_frame_rgba(rgbScaleBuffer.data(), recordingWidth, recordingHeight, (previousTimestamp + currentTimestamp) / 2); } - std::copy(image.bits(), image.bits() + SCREEN_WIDTH * SCREEN_HEIGHT * 4, rgbScaleBuffer.data()); - encoder->encode_frame_rgba(rgbScaleBuffer.data(), SCREEN_WIDTH, SCREEN_HEIGHT, currentTimestamp); + std::memcpy(rgbScaleBuffer.data(), image.bits(), frameBufferSize); + encoder->encode_frame_rgba(rgbScaleBuffer.data(), recordingWidth, recordingHeight, currentTimestamp); previousImage = image; previousTimestamp = currentTimestamp; diff --git a/frogpilot/ui/screenrecorder/screenrecorder.h b/frogpilot/ui/screenrecorder/screenrecorder.h index 02b11dec9..b55efa1cc 100644 --- a/frogpilot/ui/screenrecorder/screenrecorder.h +++ b/frogpilot/ui/screenrecorder/screenrecorder.h @@ -23,20 +23,25 @@ private slots: private: void encodeImage(); void updateState(); + bool prepareForRecording(QImage frame); - bool recording; + bool recording = false; - int frameCount; + int frameCount = 0; - qint64 startedTime; + qint64 startedTime = 0; std::thread encodingThread; std::unique_ptr encoder; std::vector rgbScaleBuffer; + size_t frameBufferSize = 0; + int recordingWidth = 0; + int recordingHeight = 0; - BlockingQueue imageQueue{UI_FREQ}; + static constexpr int kMaxBufferedFrames = 3; + BlockingQueue imageQueue{kMaxBufferedFrames}; QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index ef9a32789..60fbd0c85 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -237,6 +237,8 @@ class CarController(CarControllerBase): # === Off-pulse scheduling on regen release === if not raw_regen_active and self.last_regen_active: + # Clear any pending paddle commands when regen stops + self.paddle_queue.clear() # schedule two off-slots at 1/3 and 2/3 of the last steer interval if self.prev_steer_ts_ns and self.last_steer_ts_ns: intv = self.last_steer_ts_ns - self.prev_steer_ts_ns diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index da09e2bc0..822f22aa4 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,15 +1,12 @@ #!/usr/bin/env python3 -import gc - import cereal.messaging as messaging from openpilot.common.params import Params -from openpilot.common.realtime import set_realtime_priority +from openpilot.common.realtime import config_realtime_process from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): - gc.disable() - set_realtime_priority(2) + config_realtime_process([0, 1, 2, 3], 5) params = Params() pm = messaging.PubMaster(['driverMonitoringState']) diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index aaee4fa1e..9974a3888 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -1,9 +1,9 @@ from math import atan2 +import numpy as np from cereal import car import cereal.messaging as messaging from openpilot.selfdrive.controls.lib.events import Events -from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.stat_live import RunningStatFilter @@ -33,8 +33,8 @@ class DRIVER_MONITOR_SETTINGS: self._SG_THRESHOLD = 0.9 self._BLINK_THRESHOLD = 0.865 - self._EE_THRESH11 = 0.25 - self._EE_THRESH12 = 7.5 + self._EE_THRESH11 = 0.4 + self._EE_THRESH12 = 15.0 self._EE_MAX_OFFSET1 = 0.06 self._EE_MIN_OFFSET1 = 0.025 self._EE_THRESH21 = 0.01 @@ -57,7 +57,7 @@ class DRIVER_MONITOR_SETTINGS: self._POSESTD_THRESHOLD = 0.3 self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz - self._ALWAYS_ON_ALERT_MIN_SPEED = 7 + self._ALWAYS_ON_ALERT_MIN_SPEED = 11 self._POSE_CALIB_MIN_SPEED = 13 # 30 mph self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative @@ -205,10 +205,10 @@ class DriverMonitoring: bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) bp_normal = max(min(bp / k1, 0.5),0) - self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5], + self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5], [self.settings._POSE_PITCH_THRESHOLD_SLACK, self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD - self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5], + self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5], [self.settings._POSE_YAW_THRESHOLD_SLACK, self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 7cf87e48c..08bef264e 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -163,7 +163,12 @@ void MapWindow::updateState(const UIState &s) { return; } const SubMaster &sm = *(s.sm); - update(); + const bool map_visible = isVisible(); + const bool locationd_updated = sm.updated("liveLocationKalman"); + + if (map_visible) { + update(); + } // on rising edge of a valid system time, reinitialize the map to set a new token if (sm.valid("clocks") && !prev_time_valid) { @@ -188,7 +193,7 @@ void MapWindow::updateState(const UIState &s) { uiState()->scene.navigate_on_openpilot = nav_enabled; } - if (sm.updated("liveLocationKalman")) { + if (locationd_updated) { auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); auto locationd_pos = locationd_location.getPositionGeodetic(); auto locationd_orientation = locationd_location.getCalibratedOrientationNED(); @@ -223,11 +228,34 @@ void MapWindow::updateState(const UIState &s) { } } + if (sm.updated("navInstruction")) { + routing_problem = !sm.valid("navInstruction") && coordinate_from_param("NavDestination").has_value(); + + if (sm.valid("navInstruction")) { + auto i = sm["navInstruction"].getNavInstruction(); + map_eta->updateETA(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); + + if (map_visible && locationd_valid) { + m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance + map_instructions->updateInstructions(i); + } + } else { + clearRoute(); + } + } + loaded_once = loaded_once || (m_map && m_map->isFullyLoaded()); if (!loaded_once) { - setError(tr("Map Loading")); + if (map_visible) { + setError(tr("Map Loading")); + } return; } + + if (!map_visible) { + return; + } + initLayers(); if (!locationd_valid) { @@ -238,7 +266,7 @@ void MapWindow::updateState(const UIState &s) { setError(""); } - if (locationd_valid) { + if (locationd_valid && locationd_updated) { // Update current location marker auto point = coordinate_to_collection(*last_position); QMapLibre::Feature feature1(QMapLibre::Feature::PointType, point, {}, {}); @@ -254,33 +282,15 @@ void MapWindow::updateState(const UIState &s) { } if (interaction_counter == 0) { - if (last_position) m_map->setCoordinate(*last_position); - if (last_bearing) m_map->setBearing(*last_bearing); + if (locationd_updated && last_position) m_map->setCoordinate(*last_position); + if (locationd_updated && last_bearing) m_map->setBearing(*last_bearing); + if (locationd_updated) { m_map->setZoom(util::map_val(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM)); + } } else { interaction_counter--; } - if (sm.updated("navInstruction")) { - // an invalid navInstruction packet with a nav destination is only possible if: - // - API exception/no internet - // - route response is empty - // - any time navd is waiting for recompute_countdown - routing_problem = !sm.valid("navInstruction") && coordinate_from_param("NavDestination").has_value(); - - if (sm.valid("navInstruction")) { - auto i = sm["navInstruction"].getNavInstruction(); - map_eta->updateETA(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); - - if (locationd_valid) { - m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance - map_instructions->updateInstructions(i); - } - } else { - clearRoute(); - } - } - if (sm.rcv_frame("navRoute") != route_rcv_frame) { qWarning() << "Updating navLayer with new route"; auto route = sm["navRoute"].getNavRoute(); diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index 9cd270ccc..bfed4e6a5 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -1,5 +1,6 @@ #include "selfdrive/ui/qt/sidebar.h" +#include #include #include "selfdrive/ui/qt/util.h" diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index 7c55fbb2e..bb791fd5a 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -2,6 +2,7 @@ #include +#include #include #include #include @@ -92,4 +93,12 @@ private: QSharedPointer flag_gif; QSharedPointer home_gif; QSharedPointer settings_gif; + + QString current_home_asset_path; + QString current_flag_asset_path; + QString current_settings_asset_path; + + QDateTime home_asset_last_modified; + QDateTime flag_asset_last_modified; + QDateTime settings_asset_last_modified; }; diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 9d82284d9..3cd4b5808 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -43,8 +43,8 @@ public: CL_CHECK(clSetKernelArg(krnl_, 2, sizeof(cl_int), &expo_time)); const size_t globalWorkSize[] = {size_t(width / 2), size_t(height / 2)}; - const int imgproc_local_worksize = 16; - const size_t localWorkSize[] = {imgproc_local_worksize, imgproc_local_worksize}; + const int imgproc_local_worksize = 8; // Reduced from 16 to lessen GPU load + const size_t localWorkSize[] = {size_t(imgproc_local_worksize), size_t(imgproc_local_worksize)}; CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, imgproc_event)); } @@ -91,7 +91,8 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, imgproc = new ImgProc(device_id, context, this, s, nv12_width, nv12_uv_offset); - const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + // Use default queue priority to balance camera processing with tinygrad workloads + const cl_queue_properties props[] = { 0 }; q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); } @@ -317,8 +318,8 @@ std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, pro void camerad_thread() { cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); #ifdef QCOM2 - const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; - cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); + // Use default context priority (no hint) to avoid starving camera or tinygrad + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); #else cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); #endif diff --git a/system/camerad/main.cc b/system/camerad/main.cc index b86b4f57f..208b273b8 100644 --- a/system/camerad/main.cc +++ b/system/camerad/main.cc @@ -12,7 +12,7 @@ int main(int argc, char *argv[]) { return 0; } - int ret = util::set_realtime_priority(53); + int ret = util::set_realtime_priority(52); // Lower priority to yield more to modeld assert(ret == 0); ret = util::set_core_affinity({6}); assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores