1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
|
#include <unistd.h>
#include <math.h>
#include <arm_math.h>
#include <levmarq.h>
#include "freq_meas.h"
#include "sr_global.h"
/* FTT window lookup table defined in generated/fmeas_fft_window.c */
extern const float * const fmeas_fft_window_table;
/* jury-rig some definitions for these functions since the ARM headers only export an over-generalized variable bin size
* variant. */
extern arm_status arm_rfft_32_fast_init_f32(arm_rfft_fast_instance_f32 * S);
extern arm_status arm_rfft_64_fast_init_f32(arm_rfft_fast_instance_f32 * S);
extern arm_status arm_rfft_128_fast_init_f32(arm_rfft_fast_instance_f32 * S);
extern arm_status arm_rfft_256_fast_init_f32(arm_rfft_fast_instance_f32 * S);
extern arm_status arm_rfft_512_fast_init_f32(arm_rfft_fast_instance_f32 * S);
extern arm_status arm_rfft_1024_fast_init_f32(arm_rfft_fast_instance_f32 * S);
extern arm_status arm_rfft_2048_fast_init_f32(arm_rfft_fast_instance_f32 * S);
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);
float func_gauss(float *params, int x, void *userdata);
int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
int rc;
float in_buf[FMEAS_FFT_LEN];
float out_buf[FMEAS_FFT_LEN];
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];
arm_rfft_fast_instance_f32 fft_inst;
if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS)
return rc;
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;
/* 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)];
LMstat lmstat;
levmarq_init(&lmstat);
float a_max = 0.0f;
int i_max = 0;
for (size_t i=0; i<nbins; i++) {
if (out_buf[i] > a_max) {
a_max = out_buf[i];
i_max = i;
}
}
float par[3] = {
a_max, i_max, 1.0f
};
if (levmarq(3, ¶ms, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat))
return -1;
*out = (params[1] + first_bin) * binsize;
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(-arm_power_f32((x-mu), 2.0f/(2.0f*(sigma*sigma))));
}
float 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))));
}
|
