From 822febdbc8935fb71fada177f1a459c9482bbfac Mon Sep 17 00:00:00 2001
From: deanlee <deanlee3@gmail.com>
Date: Tue, 3 Sep 2024 22:15:31 +0800
Subject: [PATCH] synchronize model frame with camera frame

---
 selfdrive/ui/qt/onroad/annotated_camera.cc |  4 ++++
 selfdrive/ui/qt/widgets/cameraview.cc      | 12 ++++++------
 selfdrive/ui/qt/widgets/cameraview.h       |  4 +++-
 selfdrive/ui/ui.cc                         | 22 ++++++++++++++++++----
 selfdrive/ui/ui.h                          |  7 +++++--
 5 files changed, 36 insertions(+), 13 deletions(-)

diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
index fd8ad19c2e8875..20d33f468d0244 100644
--- a/selfdrive/ui/qt/onroad/annotated_camera.cc
+++ b/selfdrive/ui/qt/onroad/annotated_camera.cc
@@ -19,12 +19,16 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par
 
   experimental_btn = new ExperimentalButton(this);
   main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight);
+  // Disable automatic UI updates on frame received
+  setUpdateOnFrame(false);
 }
 
 void AnnotatedCameraWidget::updateState(const UIState &s) {
   // update engageability/experimental mode button
   experimental_btn->updateState(s);
   dmon.updateState(s);
+
+  update();
 }
 
 void AnnotatedCameraWidget::initializeGL() {
diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index 674e5e999cb2cd..196c0bef3714bc 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -201,11 +201,11 @@ void CameraWidget::paintGL() {
   if (frames.empty()) return;
 
   int frame_idx = frames.size() - 1;
-
-  // Always draw latest frame until sync logic is more stable
-  // for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) {
-  //   if (frames[frame_idx].first == draw_frame_id) break;
-  // }
+  if (draw_frame_id >= 0) {
+    for (frame_idx = 0; frame_idx < frames.size() - 1; ++frame_idx) {
+      if (frames[frame_idx].first == draw_frame_id) break;
+    }
+  }
 
   // Log duplicate/dropped frames
   if (frames[frame_idx].first == prev_frame_id) {
@@ -306,7 +306,7 @@ void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) {
 }
 
 void CameraWidget::vipcFrameReceived() {
-  update();
+  if (update_on_frame) update();
 }
 
 void CameraWidget::vipcThread() {
diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
index 29aa8493c72dfa..8d689d353fb78e 100644
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@@ -35,6 +35,7 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
   explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, QWidget* parent = nullptr);
   ~CameraWidget();
   void setBackgroundColor(const QColor &color) { bg = color; }
+  void setUpdateOnFrame(bool v) { update_on_frame = v; }
   void setFrameId(int frame_id) { draw_frame_id = frame_id; }
   void setStreamType(VisionStreamType type) { requested_stream_type = type; }
   VisionStreamType getStreamType() { return active_stream_type; }
@@ -77,7 +78,8 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
   QThread *vipc_thread = nullptr;
   std::recursive_mutex frame_lock;
   std::deque<std::pair<uint32_t, VisionBuf*>> frames;
-  uint32_t draw_frame_id = 0;
+  bool update_on_frame = true;
+  uint32_t draw_frame_id = -1;
   uint32_t prev_frame_id = 0;
 
 protected slots:
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 852980d54e8290..3f3fc6285d5d3d 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -191,10 +191,24 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   prime_state = new PrimeState(this);
   language = QString::fromStdString(Params().get("LanguageSetting"));
 
-  // update timer
-  timer = new QTimer(this);
-  QObject::connect(timer, &QTimer::timeout, this, &UIState::update);
-  timer->start(1000 / UI_FREQ);
+  thread = new QThread();
+  QObject::connect(thread, &QThread::started, [this]() { sheduleUpdate(); });
+  thread->start();
+}
+
+UIState::~UIState() {
+  thread->requestInterruption();
+  thread->quit();
+  thread->wait();
+  delete thread;
+}
+
+void UIState::sheduleUpdate() {
+  SubMaster sub_master({"modelV2"});
+  while (!QThread::currentThread()->isInterruptionRequested()) {
+    sub_master.update(100);
+    QMetaObject::invokeMethod(this, std::bind(&UIState::update, this), Qt::QueuedConnection);
+  }
 }
 
 void UIState::update() {
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 8a06a4cccb7713..51c461902b8810 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -4,10 +4,10 @@
 #include <memory>
 #include <string>
 
-#include <QTimer>
 #include <QColor>
 #include <QFuture>
 #include <QPolygonF>
+#include <QThread>
 
 #include "cereal/messaging/messaging.h"
 #include "common/mat.h"
@@ -81,6 +81,7 @@ class UIState : public QObject {
 
 public:
   UIState(QObject* parent = 0);
+  ~UIState();
   void updateStatus();
   inline bool engaged() const {
     return scene.started && (*sm)["selfdriveState"].getSelfdriveState().getEnabled();
@@ -104,8 +105,10 @@ private slots:
   void update();
 
 private:
-  QTimer *timer;
+  void sheduleUpdate();
+
   bool started_prev = false;
+  QThread *thread = nullptr;
 };
 
 UIState *uiState();