From de6126ba06fc27978237a70049e444412ed22fce Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Tue, 28 Dec 2021 18:37:33 +0800
Subject: [PATCH] replay: add flag REPLAY_FLAG_FULL_SPEED to play at full speed
 (#23324)

* add flag REPLAY_FLAG_FULL_SPEED

* use hasFlag
---
 selfdrive/ui/replay/main.cc   |  8 ++++++++
 selfdrive/ui/replay/replay.cc | 13 ++++++++-----
 selfdrive/ui/replay/replay.h  |  6 +++++-
 3 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/selfdrive/ui/replay/main.cc b/selfdrive/ui/replay/main.cc
index ccc85d4b727006..82f9c4e4af8506 100644
--- a/selfdrive/ui/replay/main.cc
+++ b/selfdrive/ui/replay/main.cc
@@ -65,6 +65,14 @@ void keyboardThread(Replay *replay_) {
       replay_->seekTo(-10, true);
     } else if (c == 'G') {
       replay_->seekTo(0, true);
+    } else if (c == 'x') {
+      if (replay_->hasFlag(REPLAY_FLAG_FULL_SPEED)) {
+        replay_->removeFlag(REPLAY_FLAG_FULL_SPEED);
+        qInfo() << "replay at normal speed";
+      } else {
+        replay_->addFlag(REPLAY_FLAG_FULL_SPEED);
+        qInfo() << "replay at full speed";
+      }
     } else if (c == ' ') {
       replay_->pause(!replay_->isPaused());
     }
diff --git a/selfdrive/ui/replay/replay.cc b/selfdrive/ui/replay/replay.cc
index d48df96874456c..5aea11bb2c19d8 100644
--- a/selfdrive/ui/replay/replay.cc
+++ b/selfdrive/ui/replay/replay.cc
@@ -224,7 +224,7 @@ void Replay::startStream(const Segment *cur_segment) {
       camera_size[type] = {fr->width, fr->height};
     }
   }
-  camera_server_ = std::make_unique<CameraServer>(camera_size, flags_ & REPLAY_FLAG_SEND_YUV);
+  camera_server_ = std::make_unique<CameraServer>(camera_size, hasFlag(REPLAY_FLAG_SEND_YUV));
 
   // start stream thread
   stream_thread_ = new QThread();
@@ -252,8 +252,8 @@ void Replay::publishFrame(const Event *e) {
       {cereal::Event::DRIVER_ENCODE_IDX, DriverCam},
       {cereal::Event::WIDE_ROAD_ENCODE_IDX, WideRoadCam},
   };
-  if ((e->which == cereal::Event::DRIVER_ENCODE_IDX && !(flags_ & REPLAY_FLAG_DCAM)) ||
-      (e->which == cereal::Event::WIDE_ROAD_ENCODE_IDX && !(flags_ & REPLAY_FLAG_ECAM))) {
+  if ((e->which == cereal::Event::DRIVER_ENCODE_IDX && !hasFlag(REPLAY_FLAG_DCAM)) ||
+      (e->which == cereal::Event::WIDE_ROAD_ENCODE_IDX && !hasFlag(REPLAY_FLAG_ECAM))) {
     return;
   }
   auto eidx = capnp::AnyStruct::Reader(e->event).getPointerSection()[0].getAs<cereal::EncodeIndex>();
@@ -314,11 +314,14 @@ void Replay::stream() {
           // reset start times
           evt_start_ts = cur_mono_time_;
           loop_start_ts = nanos_since_boot();
-        } else if (behind_ns > 0) {
+        } else if (behind_ns > 0 && !hasFlag(REPLAY_FLAG_FULL_SPEED)) {
           precise_nano_sleep(behind_ns);
         }
 
         if (evt->frame) {
+          if (hasFlag(REPLAY_FLAG_FULL_SPEED)) {
+            camera_server_->waitFinish();
+          }
           publishFrame(evt);
         } else {
           publishMessage(evt);
@@ -328,7 +331,7 @@ void Replay::stream() {
     // wait for frame to be sent before unlock.(frameReader may be deleted after unlock)
     camera_server_->waitFinish();
 
-    if (eit == events_->end() && !(flags_ & REPLAY_FLAG_NO_LOOP)) {
+    if (eit == events_->end() && !hasFlag(REPLAY_FLAG_NO_LOOP)) {
       int last_segment = segments_.rbegin()->first;
       if (current_segment_ >= last_segment && isSegmentMerged(last_segment)) {
         qInfo() << "reaches the end of route, restart from beginning";
diff --git a/selfdrive/ui/replay/replay.h b/selfdrive/ui/replay/replay.h
index e482d8d96b3816..4a3fab2981546a 100644
--- a/selfdrive/ui/replay/replay.h
+++ b/selfdrive/ui/replay/replay.h
@@ -17,6 +17,7 @@ enum REPLAY_FLAGS {
   REPLAY_FLAG_QCAMERA = 0x0040,
   REPLAY_FLAG_SEND_YUV = 0x0080,
   REPLAY_FLAG_NO_CUDA = 0x0100,
+  REPLAY_FLAG_FULL_SPEED = 0x0200,
 };
 
 class Replay : public QObject {
@@ -30,6 +31,9 @@ class Replay : public QObject {
   void start(int seconds = 0);
   void pause(bool pause);
   bool isPaused() const { return paused_; }
+  inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; }
+  inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; }
+  inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; }
 
 signals:
   void segmentChanged();
@@ -78,5 +82,5 @@ protected slots:
   std::vector<const char*> sockets_;
   std::unique_ptr<Route> route_;
   std::unique_ptr<CameraServer> camera_server_;
-  uint32_t flags_ = REPLAY_FLAG_NONE;
+  std::atomic<uint32_t> flags_ = REPLAY_FLAG_NONE;
 };