101 lines
3.1 KiB
C
101 lines
3.1 KiB
C
#include "fft/fft.h"
|
|
#include "fft/table.h"
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
|
|
#define Re(x) 2 * x
|
|
#define Im(x) 2 * x + 1
|
|
|
|
static q15_t res[FFT_N * 2];
|
|
|
|
static void applyWindow(q15_t *data, uint16_t len) {
|
|
for (uint16_t i = len; i < FFT_N; i++) data[i] = 0;
|
|
for (uint16_t i = 0; i < len; i++) data[i] = (q15_t)(((q31_t)data[i] * hanning[i]) >> 15);
|
|
}
|
|
|
|
static inline void bitreversal(q31_t *data) {
|
|
q31_t tmp;
|
|
uint16_t n1 = FFT_N >> 1;
|
|
uint16_t n2 = n1 + 1;
|
|
uint16_t *bitrev = (uint16_t *)&bitrevTable[0];
|
|
|
|
for (uint16_t i = 0, j = 0; i <= (n1 - 2); i += 2) {
|
|
if (i < j) {
|
|
tmp = data[i];
|
|
data[i] = data[j];
|
|
data[j] = tmp;
|
|
|
|
tmp = data[i + n2];
|
|
data[i + n2] = data[j + n2];
|
|
data[j + n2] = tmp;
|
|
}
|
|
tmp = data[i + 1];
|
|
data[i + 1] = data[j + n1];
|
|
data[j + n1] = tmp;
|
|
|
|
j = *bitrev;
|
|
bitrev++;
|
|
}
|
|
}
|
|
|
|
static inline void butterfly(q15_t *data) {
|
|
uint16_t step = 1;
|
|
q15_t xt, yt, cosv, sinv;
|
|
|
|
for (uint16_t m = FFT_N; m > 1; m = m >> 1) {
|
|
uint16_t n1 = m;
|
|
uint16_t n2 = m >> 1;
|
|
|
|
for (uint16_t i = 0, ia = 0; i < n2; i++) {
|
|
cosv = rotCoef[ia * 2];
|
|
sinv = rotCoef[ia * 2 + 1];
|
|
ia = ia + step;
|
|
|
|
for (uint16_t j = i, k; j < FFT_N; j += n1) {
|
|
k = j + n2;
|
|
if (m == FFT_N) {
|
|
xt = (data[Re(j)] >> 2u) - (data[Re(k)] >> 2u);
|
|
yt = (data[Im(j)] >> 2u) - (data[Im(k)] >> 2u);
|
|
|
|
data[Re(j)] = (q15_t)(((data[Re(j)] >> 2u) + (data[Re(k)] >> 2u)) >> 1);
|
|
data[Im(j)] = (q15_t)(((data[Im(k)] >> 2u) + (data[Im(j)] >> 2u)) >> 1);
|
|
|
|
data[Re(k)] = ((((q31_t)xt * cosv) >> 16)) + ((((q31_t)yt * sinv) >> 16));
|
|
data[Im(k)] = ((((q31_t)yt * cosv) >> 16)) - ((((q31_t)xt * sinv) >> 16));
|
|
} else if (m == 2) {
|
|
xt = data[Re(j)] - data[Re(k)];
|
|
yt = data[Im(j)] - data[Im(k)];
|
|
|
|
data[Re(j)] = data[Re(j)] + data[Re(k)];
|
|
data[Im(j)] = data[Im(k)] + data[Im(j)];
|
|
|
|
data[Re(k)] = xt;
|
|
data[Im(k)] = yt;
|
|
} else {
|
|
xt = data[Re(j)] - data[Re(k)];
|
|
yt = data[Im(j)] - data[Im(k)];
|
|
|
|
data[Re(j)] = (q15_t)((data[Re(j)] + data[Re(k)]) >> 1);
|
|
data[Im(j)] = (q15_t)((data[Im(k)] + data[Im(j)]) >> 1);
|
|
|
|
data[Re(k)] = ((((q31_t)xt * cosv) >> 16)) + ((((q31_t)yt * sinv) >> 16));
|
|
data[Im(k)] = ((((q31_t)yt * cosv) >> 16)) - ((((q31_t)xt * sinv) >> 16));
|
|
}
|
|
}
|
|
}
|
|
step = step << 1;
|
|
}
|
|
}
|
|
|
|
void FFT(q15_t *data, uint16_t len) {
|
|
applyWindow(data, len);
|
|
for (uint16_t i = 0; i < FFT_N; i++) {
|
|
res[Re(i)] = data[i]; // real
|
|
res[Im(i)] = 0; // imaginary
|
|
}
|
|
|
|
butterfly(res);
|
|
bitreversal((q31_t *)res);
|
|
for (uint16_t i = 0; i < FFT_N; i++) data[i] = (q15_t)abs(res[Re(i)]);
|
|
}
|