diff options
Diffstat (limited to 'controller/fw/src/freq_meas.c')
| -rw-r--r-- | controller/fw/src/freq_meas.c | 24 |
1 files changed, 12 insertions, 12 deletions
diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c index e6b3976..627af34 100644 --- a/controller/fw/src/freq_meas.c +++ b/controller/fw/src/freq_meas.c @@ -26,7 +26,7 @@ extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S); #define CONCAT(A, B, C) A ## B ## C #define arm_rfft_init_name(nbits) CONCAT(arm_rfft_, nbits, _fast_init_f32) -float func_gauss_grad(float *out, float *params, int x, void *userdata); +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) { @@ -42,12 +42,12 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) { arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0); -#define FMEAS_FFT_WINDOW_MIN_F 30.0f -#define FMEAS_FFT_WINDOW_MAX_F 70.0f - const float binsize = (float)FMEAS_ADC_SAMPLING_RATE / FMEAS_FFT_LEN; - const int first_bin = (int)(FMEAS_FFT_WINDOW_MIN_F / binsize); - const int last_bin = (int)(FMEAS_FFT_WINDOW_MAX_F / binsize + 0.5f); - const int nbins = last_bin - first_bin + 1; +#define FMEAS_FFT_WINDOW_MIN_F_HZ 30.0f +#define FMEAS_FFT_WINDOW_MAX_F_HZ 70.0f + const float binsize_hz = (float)FMEAS_ADC_SAMPLING_RATE / FMEAS_FFT_LEN; + const size_t first_bin = (int)(FMEAS_FFT_WINDOW_MIN_F_HZ / binsize_hz); + 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; /* Copy real values of target data to front of output buffer */ for (size_t i=0; i<nbins; i++) @@ -69,10 +69,10 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) { a_max, i_max, 1.0f }; - if (levmarq(3, ¶ms, 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)) return -1; - *out = (params[1] + first_bin) * binsize; + *out = (par[1] + first_bin) * binsize_hz; return 0; } @@ -82,13 +82,13 @@ float func_gauss(float *params, int x, void *userdata) { float a = params[0]; float mu = params[1]; float sigma = params[2]; - return a*expf(-arm_power_f32((x-mu), 2.0f/(2.0f*(sigma*sigma)))); + return a*expf(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma)))); } -float func_gauss_grad(float *out, float *params, int x, void *userdata) { +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]; - return -(x-mu) / ( sigma*sigma*sigma * 2.5066282746310002f) * a*expf(-arm_power_f32((x-mu), 2.0f/(2.0f*(sigma*sigma)))); + *out = -(x-mu) / ( sigma*sigma*sigma * 2.5066282746310002f) * a*expf(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma)))); } |