feat:data slove and update heatmap

components/ffmsep/cpstream_core.cc

@@ -62,6 +62,9 @@ struct CPStreamCore::Impl {
         if (config_.frame_queue_capacity == 0U) {
             config_.frame_queue_capacity = 1U;
         }
+        if (config_.slave_request_interval.count() < 0) {
+            config_.slave_request_interval = std::chrono::milliseconds{0};
+        }
         frame_queue_capacity_ = config_.frame_queue_capacity;
     }
 
@@ -106,7 +109,9 @@ struct CPStreamCore::Impl {
                 config_.parity,
                 config_.stopbits,
                 config_.flowcontrol);
-            serial->open();
+            if (!serial->isOpen()) {
+                serial->open();
+            }
             serial->flush();
 
             {
@@ -213,6 +218,9 @@ struct CPStreamCore::Impl {
 
         reader_thread_ = std::thread(&Impl::reader_loop, this);
         decoder_thread_ = std::thread(&Impl::decoder_loop, this);
+        if (!config_.slave_request_command.empty()) {
+            slave_thread_ = std::thread(&Impl::slave_loop, this);
+        }
         return true;
     }
 
@@ -227,6 +235,9 @@ struct CPStreamCore::Impl {
         if (reader_thread_.joinable()) {
             reader_thread_.join();
         }
+        if (slave_thread_.joinable()) {
+            slave_thread_.join();
+        }
 
         signal_decoder_flush(true);
         packet_cv_.notify_all();
@@ -399,6 +410,33 @@ struct CPStreamCore::Impl {
         }
     }
 
+    void slave_loop() {
+        const auto command = config_.slave_request_command;
+        auto interval = config_.slave_request_interval;
+        if (interval.count() < 0) {
+            interval = std::chrono::milliseconds{0};
+        }
+        const bool repeat = interval.count() > 0;
+
+        while (!stop_requested_.load(std::memory_order_acquire)) {
+            const bool success = send(command);
+            if (!success) {
+                std::this_thread::sleep_for(kReaderIdleSleep);
+                continue;
+            }
+            if (!repeat) {
+                break;
+            }
+
+            auto remaining = interval;
+            while (remaining.count() > 0 && !stop_requested_.load(std::memory_order_acquire)) {
+                const auto step = std::min(remaining, kReaderIdleSleep);
+                std::this_thread::sleep_for(step);
+                remaining -= step;
+            }
+        }
+    }
+
     void decoder_loop() {
         while (true) {
             Packet packet;
@@ -450,6 +488,15 @@ struct CPStreamCore::Impl {
                     decoded.pts = frame.pts;
                     decoded.received_at = std::chrono::steady_clock::now();
                     decoded.frame = std::move(frame);
+                    if (codec_descriptor_ && codec_descriptor_->id == CPCodecID::Tactile) {
+                        if (auto parsed = tactile::parse_frame(decoded.frame)) {
+                            decoded.tactile = parsed;
+                            decoded.tactile_pressures = tactile::parse_pressure_values(*parsed);
+                            if (auto matrix = tactile::parse_matrix_size_payload(*parsed)) {
+                                decoded.tactile_matrix_size = matrix;
+                            }
+                        }
+                    }
 
                     FrameCallback callback_copy;
                     {
@@ -520,6 +567,7 @@ struct CPStreamCore::Impl {
     CPCodecContext* codec_ctx_ = nullptr;
 
     std::thread reader_thread_;
+    std::thread slave_thread_;
     std::thread decoder_thread_;
 
     std::mutex packet_mutex_;