From ad8ae87ac8920d57c9416076131b3b67d4a8280b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Bourdeauducq?= <sb@m-labs.hk>
Date: Sat, 18 Jan 2025 15:46:45 +0800
Subject: [PATCH] clocker, servo threads

---
 dsp_lib.h   | 22 ++++++++++++++++++++++
 sndlock.cpp | 19 +++++++++++++++++++
 2 files changed, 41 insertions(+)

diff --git a/dsp_lib.h b/dsp_lib.h
index 73ae85b..85d0550 100644
--- a/dsp_lib.h
+++ b/dsp_lib.h
@@ -2,6 +2,8 @@
 
 #include <cstdint>
 #include <complex>
+#include <chrono>
+#include <thread>
 
 typedef uint32_t phase_t;
 #define PHASE_MAX UINT32_MAX
@@ -71,3 +73,23 @@ class Lockin {
             return phase;
         }
 };
+
+class Clocker {
+    private:
+        std::chrono::milliseconds period;
+        std::chrono::time_point<std::chrono::steady_clock> next_tick = std::chrono::time_point<std::chrono::steady_clock>::min();
+    public:
+        Clocker(std::chrono::milliseconds period): period(period) {};
+        void tick() {
+            if(next_tick == std::chrono::time_point<std::chrono::steady_clock>::min()) {
+                next_tick = std::chrono::steady_clock::now() + period;
+            } else {
+                auto duration = next_tick - std::chrono::steady_clock::now();
+                if(duration >= duration.zero())
+                    std::this_thread::sleep_for(duration);
+                else
+                    std::cerr << "missed tick" << std::endl;
+                next_tick += period;
+            }
+        }
+};
diff --git a/sndlock.cpp b/sndlock.cpp
index 188bcd8..9128b62 100644
--- a/sndlock.cpp
+++ b/sndlock.cpp
@@ -3,6 +3,7 @@
 #include <cstdint>
 #include <complex>
 #include <optional>
+#include <chrono>
 
 #include <GLFW/glfw3.h>
 #include <sndio.h>
@@ -228,6 +229,15 @@ static void dsp_thread()
     sio_close(hdl);
 }
 
+static void servo_thread(int channel)
+{
+    Clocker clocker = Clocker(std::chrono::milliseconds(100));
+    while(!shutdown_threads) {
+        clocker.tick();
+        std::cout << "servo thread tick " << channel << std::endl;
+    }
+}
+
 int main(int argc, char* argv[])
 {
     if(!glfwInit()) {
@@ -276,6 +286,15 @@ int main(int argc, char* argv[])
     };
     std::atexit(JoinDSP);
 
+    static std::thread servo_thread_h[SND_PCHAN];
+    for(int i=0;i<SND_PCHAN;i++)
+        servo_thread_h[i] = std::thread(servo_thread, i);
+    static auto JoinServo = []() {
+        for(int i=0;i<SND_PCHAN;i++)
+            servo_thread_h[i].join();
+    };
+    std::atexit(JoinServo);
+
     shutdown_threads = false;
     static auto SetShutdown = []() {
         shutdown_threads = true;