114 lines
2.8 KiB
C
114 lines
2.8 KiB
C
#include <stddef.h>
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
|
|
#include "system/task_fft.h"
|
|
#include "system/data_channel.h"
|
|
#include "system/Complex.h"
|
|
#include "system/float_word.h"
|
|
|
|
static void bit_reverse(Complex *x, size_t n);
|
|
static void fft_inplace(Complex *x, size_t n);
|
|
|
|
#define FFT_SIZE 1024
|
|
|
|
int task_fft_run(void *task)
|
|
{
|
|
fft_config *config = (fft_config *)task;
|
|
|
|
Complex buffer[FFT_SIZE];
|
|
|
|
// 1) 1024 Werte aus dem Eingangskanal lesen
|
|
for (size_t i = 0; i < FFT_SIZE; ++i)
|
|
{
|
|
float sample = 0.0f;
|
|
data_channel_read(config->base.sources[0], (uint32_t *)&sample);
|
|
|
|
buffer[i].re = sample;
|
|
buffer[i].im = 0.0f;
|
|
}
|
|
|
|
// 2) FFT berechnen
|
|
fft_inplace(buffer, FFT_SIZE);
|
|
|
|
// 3) Magnitude berechnen und skalieren
|
|
for (size_t i = 0; i < FFT_SIZE; ++i)
|
|
{
|
|
float_word w;
|
|
float mag = sqrtf(buffer[i].re * buffer[i].re + buffer[i].im * buffer[i].im);
|
|
|
|
if (i == 0)
|
|
mag *= 1.0f / FFT_SIZE; // DC-Komponente
|
|
else
|
|
mag *= 2.0f / FFT_SIZE; // alle anderen Frequenzen
|
|
|
|
w.value = mag;
|
|
data_channel_write(config->base.sink, w.word);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
// --------------------------------------------------
|
|
// Hilfsfunktionen: Bit-Reversal + FFT
|
|
// --------------------------------------------------
|
|
static void bit_reverse(Complex *x, size_t n)
|
|
{
|
|
size_t j = 0;
|
|
for (size_t i = 0; i < n; ++i)
|
|
{
|
|
if (i < j)
|
|
{
|
|
Complex tmp = x[i];
|
|
x[i] = x[j];
|
|
x[j] = tmp;
|
|
}
|
|
size_t bit = n >> 1;
|
|
while (bit & j)
|
|
{
|
|
j ^= bit;
|
|
bit >>= 1;
|
|
}
|
|
j |= bit;
|
|
}
|
|
}
|
|
|
|
static void fft_inplace(Complex *x, size_t n)
|
|
{
|
|
bit_reverse(x, n);
|
|
|
|
for (size_t len = 2; len <= n; len <<= 1)
|
|
{
|
|
float ang = -2.0f * (float)M_PI / (float)len;
|
|
float wlen_re = cosf(ang);
|
|
float wlen_im = sinf(ang);
|
|
|
|
for (size_t i = 0; i < n; i += len)
|
|
{
|
|
float w_re = 1.0f;
|
|
float w_im = 0.0f;
|
|
|
|
for (size_t j = 0; j < (len >> 1); ++j)
|
|
{
|
|
Complex u = x[i + j];
|
|
|
|
Complex v;
|
|
v.re = w_re * x[i + j + (len >> 1)].re - w_im * x[i + j + (len >> 1)].im;
|
|
v.im = w_re * x[i + j + (len >> 1)].im + w_im * x[i + j + (len >> 1)].re;
|
|
|
|
x[i + j].re = u.re + v.re;
|
|
x[i + j].im = u.im + v.im;
|
|
x[i + j + (len >> 1)].re = u.re - v.re;
|
|
x[i + j + (len >> 1)].im = u.im - v.im;
|
|
|
|
float tmp_re = w_re * wlen_re - w_im * wlen_im;
|
|
float tmp_im = w_re * wlen_im + w_im * wlen_re;
|
|
w_re = tmp_re;
|
|
w_im = tmp_im;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|