diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 91e2a76f02..bedb570235 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -372,19 +372,21 @@ void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV } void NvgWindow::paintGL() { + UIState *s = uiState(); + const cereal::ModelDataV2::Reader &model = (*s->sm)["modelV2"].getModelV2(); + CameraViewWidget::setFrameId(model.getFrameId()); CameraViewWidget::paintGL(); QPainter painter(this); painter.setRenderHint(QPainter::Antialiasing); painter.setPen(Qt::NoPen); - UIState *s = uiState(); if (s->worldObjectsVisible()) { drawLaneLines(painter, s); if (s->scene.longitudinal_control) { - auto leads = (*s->sm)["modelV2"].getModelV2().getLeadsV3(); + const auto leads = model.getLeadsV3(); if (leads[0].getProb() > .5) { drawLead(painter, leads[0], s->scene.lead_vertices[0]); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 646d286ee7..a0b3c43cde 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -163,7 +163,7 @@ void CameraViewWidget::initializeGL() { } void CameraViewWidget::showEvent(QShowEvent *event) { - latest_frame = nullptr; + frames.clear(); if (!vipc_thread) { vipc_thread = new QThread(); connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); @@ -214,14 +214,20 @@ void CameraViewWidget::paintGL() { glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); - if (latest_frame == nullptr) return; + if (frames.empty()) return; + + int frame_idx; + for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) { + if (frames[frame_idx].first == draw_frame_id) break; + } + VisionBuf *frame = frames[frame_idx].second; glPixelStorei(GL_UNPACK_ALIGNMENT, 1); glViewport(0, 0, width(), height()); glBindVertexArray(frame_vao); glUseProgram(program->programId()); - uint8_t *address[3] = {latest_frame->y, latest_frame->u, latest_frame->v}; + uint8_t *address[3] = {frame->y, frame->u, frame->v}; for (int i = 0; i < 3; ++i) { glActiveTexture(GL_TEXTURE0 + i); glBindTexture(GL_TEXTURE_2D, textures[i]); @@ -244,7 +250,7 @@ void CameraViewWidget::paintGL() { void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { makeCurrent(); - latest_frame = nullptr; + frames.clear(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; @@ -263,19 +269,23 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { updateFrameMat(width(), height()); } -void CameraViewWidget::vipcFrameReceived(VisionBuf *buf) { - latest_frame = buf; +void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) { + frames.push_back(std::make_pair(frame_id, buf)); + while (frames.size() > FRAME_BUFFER_SIZE) { + frames.pop_front(); + } update(); } void CameraViewWidget::vipcThread() { VisionStreamType cur_stream_type = stream_type; std::unique_ptr vipc_client; + VisionIpcBufExtra meta_main = {0}; while (!QThread::currentThread()->isInterruptionRequested()) { if (!vipc_client || cur_stream_type != stream_type) { cur_stream_type = stream_type; - vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, true)); + vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, false)); } if (!vipc_client->connected) { @@ -286,8 +296,8 @@ void CameraViewWidget::vipcThread() { emit vipcThreadConnected(vipc_client.get()); } - if (VisionBuf *buf = vipc_client->recv(nullptr, 1000)) { - emit vipcThreadFrameReceived(buf); + if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) { + emit vipcThreadFrameReceived(buf, meta_main.frame_id); } } } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 6873e8e991..65d5edc221 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -10,6 +10,9 @@ #include "selfdrive/camerad/cameras/camera_common.h" #include "selfdrive/ui/ui.h" +const int FRAME_BUFFER_SIZE = 5; +static_assert(FRAME_BUFFER_SIZE <= YUV_BUFFER_COUNT); + class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { Q_OBJECT @@ -19,11 +22,12 @@ public: ~CameraViewWidget(); void setStreamType(VisionStreamType type) { stream_type = type; } void setBackgroundColor(const QColor &color) { bg = color; } + void setFrameId(int frame_id) { draw_frame_id = frame_id; } signals: void clicked(); void vipcThreadConnected(VisionIpcClient *); - void vipcThreadFrameReceived(VisionBuf *); + void vipcThreadFrameReceived(VisionBuf *, quint32); protected: void paintGL() override; @@ -36,8 +40,8 @@ protected: void vipcThread(); bool zoomed_view; - VisionBuf *latest_frame = nullptr; GLuint frame_vao, frame_vbo, frame_ibo; + GLuint textures[3]; mat4 frame_mat; std::unique_ptr program; QColor bg = QColor("#000000"); @@ -48,9 +52,10 @@ protected: std::atomic stream_type; QThread *vipc_thread = nullptr; - GLuint textures[3]; + std::deque> frames; + uint32_t draw_frame_id = 0; protected slots: void vipcConnected(VisionIpcClient *vipc_client); - void vipcFrameReceived(VisionBuf *vipc_client); + void vipcFrameReceived(VisionBuf *vipc_client, uint32_t frame_id); };