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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
|
#ifndef SIMULATION
#include <stm32f407xx.h>
#endif
#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");
*/
#ifndef SIMULATION
GPIOA->BSRR = 1<<12;
#endif
arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0);
#ifndef SIMULATION
GPIOA->BSRR = 1<<12<<16;
#endif
#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]);
*/
#ifndef SIMULATION
GPIOA->BSRR = 1<<12;
#endif
if (levmarq(3, par, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat) < 0) {
#ifndef SIMULATION
GPIOA->BSRR = 1<<12<<16;
#endif
*out = NAN;
return -1;
}
#ifndef SIMULATION
GPIOA->BSRR = 1<<12<<16;
#endif
/*
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;
}
|
