69 lines
1.5 KiB
C
69 lines
1.5 KiB
C
#include "system/task_fft.h"
|
|
#include "system/data_channel.h"
|
|
#include "system/Complex.h"
|
|
#include "system/float_word.h"
|
|
#include "math.h"
|
|
#include "complex.h"
|
|
|
|
void cooley_tukey(float complex *X, const uint32_t N) {
|
|
if (N >= 2) {
|
|
float complex tmp [N / 2];
|
|
for (uint32_t i = 0; i < N / 2; ++i) {
|
|
tmp[i] = X[2*i + 1];
|
|
X[i] = X[2*i];
|
|
}
|
|
for (uint32_t i = 0; i < N / 2; ++i) {
|
|
X[i + N / 2] = tmp[i];
|
|
}
|
|
|
|
cooley_tukey(X, N / 2);
|
|
cooley_tukey(X + N / 2, N / 2);
|
|
|
|
for (uint32_t i = 0; i < N / 2; ++i) {
|
|
X[i + N / 2] = X[i] - cexp(-2.0 * I * M_PI * (float)i / (float)N) * X[i + N / 2];
|
|
X[i] -= (X[i + N / 2]-X[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
int task_fft_run( void * task ) {
|
|
|
|
fft_config * config = (fft_config*)task;
|
|
|
|
Complex input[DATA_CHANNEL_DEPTH];
|
|
|
|
// Daten lesen
|
|
for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
|
|
float a;
|
|
data_channel_read(config->base.sources[0], (uint32_t *)&a);
|
|
input[i] = (Complex){a, 0};
|
|
}
|
|
|
|
// FFT berechnen
|
|
cooley_tukey(input, DATA_CHANNEL_DEPTH);
|
|
|
|
// Ergebnisse in data channel schreiben
|
|
for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
|
|
|
|
float_word c;
|
|
c.value = complex_abs(&input[i]);
|
|
|
|
|
|
if (i > (uint32_t)0)
|
|
{
|
|
c.value = c.value * (2.0/DATA_CHANNEL_DEPTH);
|
|
}
|
|
else
|
|
{
|
|
c.value = c.value * (1.0/DATA_CHANNEL_DEPTH);
|
|
}
|
|
|
|
|
|
data_channel_write(config->base.sink, c.word);
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|