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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
|
#include <unistd.h>
#include <math.h>
#include <arm_math.h>
#include <levmarq.h>
#include "freq_meas.h"
#include "sr_global.h"
#include "simulation.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)
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 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 - 0.5) * 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) {
*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
#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;
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++) {
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);
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
};
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) < 0) {
*out = NAN;
return -1;
}
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], 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], 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;
}
|
