diff options
Diffstat (limited to 'controller/fw/src')
-rw-r--r-- | controller/fw/src/freq_meas.c | 69 | ||||
-rw-r--r-- | controller/fw/src/freq_meas.h | 2 | ||||
-rw-r--r-- | controller/fw/src/simulation.h | 14 |
3 files changed, 70 insertions, 15 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; } diff --git a/controller/fw/src/freq_meas.h b/controller/fw/src/freq_meas.h index 1c083f8..4786c0e 100644 --- a/controller/fw/src/freq_meas.h +++ b/controller/fw/src/freq_meas.h @@ -2,6 +2,6 @@ #ifndef __FREQ_MEAS_H__ #define __FREQ_MEAS_H__ -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); #endif /* __FREQ_MEAS_H__ */ diff --git a/controller/fw/src/simulation.h b/controller/fw/src/simulation.h new file mode 100644 index 0000000..e813de7 --- /dev/null +++ b/controller/fw/src/simulation.h @@ -0,0 +1,14 @@ +#ifndef __SIMULATION_H__ +#define __SIMULATION_H__ + +#ifdef SIMULATION +#include <stdio.h> +#define DEBUG_PRINTN(...) fprintf(stderr, __VA_ARGS__) +#define DEBUG_PRINTNF(fmt, ...) DEBUG_PRINTN("%s:%d: " fmt, __FILE__, __LINE__, ##__VA_ARGS__) +#define DEBUG_PRINT(fmt, ...) DEBUG_PRINTNF(fmt "\n", ##__VA_ARGS__) +#else +#define DEBUG_PRINT(...) ((void)0) +#define DEBUG_PRINTN(...) ((void)0) +#endif + +#endif /* __SIMULATION_H__ */ |