1 files changed, 55 insertions, 14 deletions

diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c
index 627af34..ba7801e 100644
--- a/controller/fw/src/freq_meas.c
+++ b/controller/fw/src/freq_meas.c
@@ -7,6 +7,7 @@
 
 #include "freq_meas.h"
 #include "sr_global.h"
+#include "simulation.h"
 
 
 /* FTT window lookup table defined in generated/fmeas_fft_window.c */
@@ -29,17 +30,33 @@ extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S);
 void func_gauss_grad(float *out, float *params, int x, void *userdata);
 float func_gauss(float *params, int x, void *userdata);
 
-int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
+int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     int rc;
     float in_buf[FMEAS_FFT_LEN];
     float out_buf[FMEAS_FFT_LEN];
+    /*
+    DEBUG_PRINTN("    [emulated adc buf] ");
+    for (size_t i=0; i<FMEAS_FFT_LEN; i++)
+        DEBUG_PRINTN("%5d, ", adc_buf[i]);
+    DEBUG_PRINTN("\n");
+    */
+    //DEBUG_PRINT("Applying window function");
     for (size_t i=0; i<FMEAS_FFT_LEN; i++)
         in_buf[i] = (float)adc_buf[i] / (float)FMEAS_ADC_MAX * fmeas_fft_window_table[i];
 
+    //DEBUG_PRINT("Running FFT");
     arm_rfft_fast_instance_f32 fft_inst;
-    if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS)
+    if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS) {
+        *out = NAN;
         return rc;
+    }
 
+    /*
+    DEBUG_PRINTN("    [input] ");
+    for (size_t i=0; i<FMEAS_FFT_LEN; i++)
+        DEBUG_PRINTN("%010f, ", in_buf[i]);
+    DEBUG_PRINTN("\n");
+    */
     arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0);
 
 #define FMEAS_FFT_WINDOW_MIN_F_HZ 30.0f
@@ -49,10 +66,24 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     const size_t last_bin = (int)(FMEAS_FFT_WINDOW_MAX_F_HZ / binsize_hz + 0.5f);
     const size_t nbins = last_bin - first_bin + 1;
 
+    DEBUG_PRINT("binsize_hz=%f first_bin=%zd last_bin=%zd nbins=%zd", binsize_hz, first_bin, last_bin, nbins);
+    DEBUG_PRINTN("    [bins real] ");
+    for (size_t i=0; i<FMEAS_FFT_LEN/2; i+=2)
+        DEBUG_PRINTN("%010f, ", out_buf[i]);
+    DEBUG_PRINTN("\n    [bins imag] ");
+    for (size_t i=1; i<FMEAS_FFT_LEN/2; i+=2)
+        DEBUG_PRINTN("%010f, ", out_buf[i]);
+    DEBUG_PRINT("\n");
+
+    DEBUG_PRINT("Repacking FFT results");
     /* Copy real values of target data to front of output buffer */
-    for (size_t i=0; i<nbins; i++)
-        out_buf[i] = out_buf[2 * (first_bin + i)];
+    for (size_t i=0; i<nbins; i++) {
+        float real = out_buf[2 * (first_bin + i)];
+        float imag = out_buf[2 * (first_bin + i) + 1];
+        out_buf[i] = sqrtf(real*real + imag*imag);
+    }
 
+    DEBUG_PRINT("Running Levenberg-Marquardt");
     LMstat lmstat;
     levmarq_init(&lmstat);
 
@@ -68,27 +99,37 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     float par[3] = {
         a_max, i_max, 1.0f
     };
+    DEBUG_PRINT("    par_pre={%010f, %010f, %010f}", par[0], par[1], par[2]);
 
-    if (levmarq(3, par, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat))
+    if (levmarq(3, par, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat) < 0) {
+        *out = NAN;
         return -1;
+    }
 
-    *out = (par[1] + first_bin) * binsize_hz;
+    DEBUG_PRINT("    par_post={%010f, %010f, %010f}", par[0], par[1], par[2]);
 
+    DEBUG_PRINT("done.");
+    *out = (par[1] + first_bin) * binsize_hz;
     return 0;
 }
 
 float func_gauss(float *params, int x, void *userdata) {
     UNUSED(userdata);
-    float a = params[0];
-    float mu = params[1];
-    float sigma = params[2];
-    return a*expf(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+    float a = params[0], b = params[1], c = params[2];
+    float n = x-b;
+    return a*expf(-n*n / (2.0f* c*c));
 }
 
 void func_gauss_grad(float *out, float *params, int x, void *userdata) {
     UNUSED(userdata);
-    float a = params[0];
-    float mu = params[1];
-    float sigma = params[2];
-    *out = -(x-mu) / (  sigma*sigma*sigma * 2.5066282746310002f) * a*expf(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+    float a = params[0], b = params[1], c = params[2];
+    float n = x-b;
+    float e = expf(-n*n / (2.0f * c*c));
+    
+    /* d/da */
+    out[0] = e;
+    /* d/db */
+    out[1] = a*n/(c*c) * e;
+    /* d/dc */
+    out[2] = a*n*n/(c*c*c) * e;
 }