From a854b35a6a76934e0a24e0c51cc9813853692e7e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Bourdeauducq?= <sb@m-labs.hk>
Date: Wed, 22 Jan 2025 14:31:45 +0800
Subject: [PATCH] principal computation

---
 sndlock.cpp | 86 +++++++++++++++++++++++++++++++++--------------------
 1 file changed, 53 insertions(+), 33 deletions(-)

diff --git a/sndlock.cpp b/sndlock.cpp
index a291bb2..a2ee555 100644
--- a/sndlock.cpp
+++ b/sndlock.cpp
@@ -39,12 +39,14 @@ static float in_wave[HIST_DEPTH];
 
 static std::atomic<int> multiplier[SND_PCHAN];
 static std::atomic<double> lpf_bandwidth[SND_PCHAN];
-static std::atomic<double> li_mag[SND_PCHAN];
-static std::atomic<double> li_phase[SND_PCHAN];
-static std::mutex li_hist_mutex;
+static std::atomic<double> principal_angle[SND_PCHAN];
+static std::atomic<double> li_principal[SND_PCHAN];
+
 static std::atomic<bool> li_hist_hold[SND_PCHAN];
+static std::mutex li_hist_mutex;
 static float li_hist_mag[SND_PCHAN][HIST_DEPTH];
 static float li_hist_phase[SND_PCHAN][HIST_DEPTH];
+static float li_hist_principal[SND_PCHAN][HIST_DEPTH];
 
 enum class InWaveState { delay, wait_trigger, capturing };
 
@@ -215,20 +217,20 @@ static void dsp_thread()
 
                 for(int j=0;j<SND_PCHAN;j++) {
                     std::complex<double> lockin_out = lockin[j].update(sample);
-                    double mag = std::abs(lockin_out);
-                    double phase = std::arg(lockin_out);
+                    double principal = std::real(lockin_out*std::polar(1.0, (double)principal_angle[j]));
 
-                    li_mag[j] = mag;
-                    li_phase[j] = phase;
+                    li_principal[j] = principal;
                     if(!li_hist_hold[j]) {
                         li_count[j]++;
                         if(li_count[j] == 200) {
                             li_count[j] = 0;
                             std::lock_guard<std::mutex> guard(li_hist_mutex);
                             std::memmove(&li_hist_mag[j][0], &li_hist_mag[j][1], (HIST_DEPTH-1)*sizeof(float));
-                            li_hist_mag[j][HIST_DEPTH-1] = mag;
+                            li_hist_mag[j][HIST_DEPTH-1] = std::abs(lockin_out);
                             std::memmove(&li_hist_phase[j][0], &li_hist_phase[j][1], (HIST_DEPTH-1)*sizeof(float));
-                            li_hist_phase[j][HIST_DEPTH-1] = phase;
+                            li_hist_phase[j][HIST_DEPTH-1] = std::arg(lockin_out);
+                            std::memmove(&li_hist_principal[j][0], &li_hist_principal[j][1], (HIST_DEPTH-1)*sizeof(float));
+                            li_hist_principal[j][HIST_DEPTH-1] = principal;
                         }
                     }
                 }
@@ -258,7 +260,7 @@ static std::atomic<float> init_current_cooling[SND_PCHAN];
 static std::atomic<float> init_current_heating[SND_PCHAN];
 static std::atomic<float> init_temp[SND_PCHAN];
 static std::atomic<float> leadin_current[SND_PCHAN];
-static std::atomic<float> leadin_mag[SND_PCHAN];
+static std::atomic<float> leadin_thr[SND_PCHAN];
 
 static void servo_thread(int channel)
 {
@@ -293,7 +295,7 @@ static void servo_thread(int channel)
             continue;
 
         servo_state[channel] = "LEAD-IN";
-        while(servo_enable[channel] && li_mag[channel] < leadin_mag[channel]) {
+        while(servo_enable[channel] && li_principal[channel] < leadin_thr[channel]) {
             kirdy.set_tec_current(leadin_current[channel]);
             clocker.tick();
             laser_temp[channel] = temp = kirdy.get_laser_temp();
@@ -347,12 +349,13 @@ int main(int argc, char* argv[])
         amplitude[i] = 1.0;
         multiplier[i] = 1;
         lpf_bandwidth[i] = 10.0;
+        principal_angle[i] = M_PI/4.0;
         servo_state[i] = "DISABLED";
         init_current_cooling[i] = 270.0f;
         init_current_heating[i] = -30.0f;
         init_temp[i] = 16.0f;
         leadin_current[i] = 230.0f;
-        leadin_mag[i] = 0.007f;
+        leadin_thr[i] = 0.007f;
     }
 
     static std::thread dsp_thread_h = std::thread(dsp_thread);
@@ -378,9 +381,7 @@ int main(int argc, char* argv[])
     };
     std::atexit(SetShutdown);
 
-    float plot_scale[SND_PCHAN];
-    for(int i=0;i<SND_PCHAN;i++)
-        plot_scale[i] = 0.05f;
+    int plot_sel[SND_PCHAN] = { 0 };
     while(!glfwWindowShouldClose(window)) {
         glfwPollEvents();
         glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
@@ -454,30 +455,49 @@ int main(int argc, char* argv[])
                 sprintf(str, "3f##%d", i);
                 ImGui::RadioButton(str, &multiplier_l, 3);
                 multiplier[i] = multiplier_l;
-                ImGui::SameLine();
-                ImGui::Text("/");
-                bool hold_l = li_hist_hold[i];
-                ImGui::SameLine();
-                sprintf(str, "hold plot##%d", i);
-                ImGui::Checkbox(str, &hold_l);
-                li_hist_hold[i] = hold_l;
                 sprintf(str, "LPF BW##%d", i);
                 float lpf_bandwidth_l = lpf_bandwidth[i];
                 ImGui::SliderFloat(str, &lpf_bandwidth_l, 0.5f, 200.0f);
                 lpf_bandwidth[i] = lpf_bandwidth_l;
+                sprintf(str, "principal angle##%d", i);
+                float principal_angle_l = principal_angle[i];
+                ImGui::SliderFloat(str, &principal_angle_l, 0.0f, M_PI/2.0f);
+                principal_angle[i] = principal_angle_l;
+
+                ImGui::AlignTextToFramePadding();
+                ImGui::Text("plot:");
+                ImGui::SameLine();
+                bool hold_l = li_hist_hold[i];
+                sprintf(str, "hold##%d", i);
+                ImGui::Checkbox(str, &hold_l);
+                li_hist_hold[i] = hold_l;
+                ImGui::SameLine();
+                sprintf(str, "magnitude##%d", i);
+                ImGui::RadioButton(str, &plot_sel[i], 0);
+                ImGui::SameLine();
+                sprintf(str, "phase##%d", i);
+                ImGui::RadioButton(str, &plot_sel[i], 1);
+                ImGui::SameLine();
+                sprintf(str, "principal##%d", i);
+                ImGui::RadioButton(str, &plot_sel[i], 2);
 
-                sprintf(str, "mag scale##%d", i);
-                ImGui::SliderFloat(str, &plot_scale[i], 0.01f, 0.1f);
                 ImGui::PushItemWidth(ImGui::GetContentRegionAvail().x);
+                sprintf(str, "##plot%d", i);
                 {
                     std::lock_guard<std::mutex> guard(li_hist_mutex);
-                    sprintf(str, "##magnitude%d", i);
-                    ImGui::PlotLines(str, li_hist_mag[i], HIST_DEPTH, 0, 0, -0.0f, 0.1f*plot_scale[i], ImVec2(0.0f, 200.0f));
-                    sprintf(str, "##phase%d", i);
-                    ImGui::PlotLines(str, li_hist_phase[i], HIST_DEPTH, 0, 0, -M_PI, M_PI, ImVec2(0.0f, 200.0f));
+                    switch(plot_sel[i]) {
+                        case 0:
+                            ImGui::PlotLines(str, li_hist_mag[i], HIST_DEPTH, 0, 0, -0.0f, 0.02f, ImVec2(0.0f, 200.0f));
+                            break;
+                        case 1:
+                            ImGui::PlotLines(str, li_hist_phase[i], HIST_DEPTH, 0, 0, -M_PI, M_PI, ImVec2(0.0f, 200.0f));
+                            break;
+                        case 2:
+                            ImGui::PlotLines(str, li_hist_principal[i], HIST_DEPTH, 0, 0, -0.02f, 0.02f, ImVec2(0.0f, 200.0f));
+                            break;
+                    }
                 }
                 ImGui::PopItemWidth();
-                ImGui::Text("values: %8.5f %8.5f rad", (double)li_mag[i], (double)li_phase[i]);
             }
         }
 
@@ -522,10 +542,10 @@ int main(int argc, char* argv[])
                 ImGui::InputFloat(str, &leadin_current_l, 1.0f, 500.0f, "%.3f");
                 leadin_current[i] = leadin_current_l;
                 ImGui::SameLine();
-                sprintf(str, "magnitude##%d", i);
-                float leadin_mag_l = leadin_mag[i];
-                ImGui::InputFloat(str, &leadin_mag_l, 0.0f, 0.001f, "%.6f");
-                leadin_mag[i] = leadin_mag_l;
+                sprintf(str, "threshold##%d", i);
+                float leadin_thr_l = leadin_thr[i];
+                ImGui::InputFloat(str, &leadin_thr_l, 0.0f, 0.001f, "%.6f");
+                leadin_thr[i] = leadin_thr_l;
                 ImGui::PopItemWidth();
             }
         }