From a2cff8a2d57a02f314d9f6adb7221b193a150e4c Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Tue, 17 May 2022 15:25:37 -0700
Subject: [PATCH] frame offset

---
 selfdrive/ui/qt/onroad.cc             |  1 +
 selfdrive/ui/qt/widgets/cameraview.cc |  8 ++++++--
 selfdrive/ui/qt/widgets/cameraview.h  | 10 ++++++++--
 3 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index 0292020a01..4f6187656b 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -375,6 +375,7 @@ void NvgWindow::paintGL() {
   UIState *s = uiState();
   const cereal::ModelDataV2::Reader &model = (*s->sm)["modelV2"].getModelV2();
   CameraViewWidget::setFrameId(model.getFrameId());
+  qDebug() << "NvgWindow::paintGL: frame to draw:" << model.getFrameId();
   CameraViewWidget::paintGL();
 
   QPainter painter(this);
diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index a66270175f..29b1f3e93c 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -55,7 +55,7 @@ const mat4 device_transform = {{
   0.0,  0.0, 0.0, 1.0,
 }};
 
-const int FRAME_BUFFER_SIZE = 4;
+const int FRAME_BUFFER_SIZE = 3;
 static_assert(FRAME_BUFFER_SIZE <= YUV_BUFFER_COUNT);
 
 mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) {
@@ -221,10 +221,14 @@ void CameraViewWidget::paintGL() {
   if (frames.size() == 0) return;
 
   auto it = std::find_if(frames.begin(), frames.end(), [this](const std::pair<uint32_t, VisionBuf*>& element) {
-    return element.first == draw_frame_id;
+    return element.first == (draw_frame_id + frame_offset);
   });
   int frame_idx = (it == frames.end()) ? (frames.size() - 1) : (it - frames.begin());
+
   VisionBuf *frame = frames[frame_idx].second;
+  qDebug() << "Latest frame:" << frames[frames.size() - 1].first;
+  qDebug() << "Drawing frame:" << frames[frame_idx].first;
+  qDebug() << "CameraViewWidget::paintGL: frame to draw:" << draw_frame_id;
 
   glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
   glViewport(0, 0, width(), height());
diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
index d995fa8c4d..51373fb752 100644
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@@ -6,6 +6,7 @@
 #include <QOpenGLShaderProgram>
 #include <QOpenGLWidget>
 #include <QThread>
+#include <QDebug>
 #include "cereal/visionipc/visionipc_client.h"
 #include "selfdrive/camerad/cameras/camera_common.h"
 #include "selfdrive/ui/ui.h"
@@ -19,7 +20,11 @@ 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; }
+  void setFrameId(int frame_id) {
+    frame_offset = frame_id == draw_frame_id ? frame_offset + 1 : 0;
+    if (frame_id > draw_frame_id)
+      draw_frame_id = frame_id;
+  }
 
 signals:
   void clicked();
@@ -50,7 +55,8 @@ protected:
   QThread *vipc_thread = nullptr;
 
   std::deque<std::pair<uint32_t, VisionBuf*>> frames;
-  uint32_t draw_frame_id;
+  uint32_t draw_frame_id = 0;
+  uint32_t frame_offset = 0;
 
 protected slots:
   void vipcConnected(VisionIpcClient *vipc_client);