sync camera frame with model frame

commaai · Oct 12, 2024 · 31287e3 · 31287e3
1 parent 354e909
commit 31287e3
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 9 deletions.
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
@@ -19,12 +19,14 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par
 
  experimental_btn = new ExperimentalButton(this);
  main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight);
+ setUpdateOnFrame(false); // Disable frame-triggered updates
 }
 
 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
@@ -201,11 +201,14 @@ 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;
+ }
+ if (frames[frame_idx].first != draw_frame_id) {
+ qDebug() << "Camera frame not sync with model frame." << frames[frame_idx].first << draw_frame_id;
+ }
+ }
 
  // Log duplicate/dropped frames
  if (frames[frame_idx].first == prev_frame_id) {
@@ -306,7 +309,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
@@ -34,6 +34,7 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
  using QOpenGLWidget::QOpenGLWidget;
  explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, QWidget* parent = nullptr);
  ~CameraWidget();
+ void setUpdateOnFrame(bool v) { update_on_frame = v; }
  void setBackgroundColor(const QColor &color) { bg = color; }
  void setFrameId(int frame_id) { draw_frame_id = frame_id; }
  void setStreamType(VisionStreamType type) { requested_stream_type = 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
@@ -15,7 +15,8 @@
 #define BACKLIGHT_TS 10.00
 
 static void update_sockets(UIState *s) {
- s->sm->update(0);
+ // Wait for 10 ms to ensure modelV2 is updated when onroad.
+ s->sm->update(s->scene.started ? 10 : 0);
 }
 
 static void update_state(UIState *s) {
@@ -94,7 +95,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
  "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
  "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
  "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
- });
+ }, std::vector<const char*>{"modelV2"});
  prime_state = new PrimeState(this);
  language = QString::fromStdString(Params().get("LanguageSetting"));