working on testing

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, &params, 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))));
 }