sb10q/sndlock
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

principal computation

Browse Source
This commit is contained in:
Sébastien Bourdeauducq 2025-01-22 14:31:45 +08:00
parent 4437d9bde7
commit a854b35a6a
1 changed files with 53 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
sndlock.cpp
View File

@ -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();
}
}