musiclight2/fft.c

152 regels
3.6 KiB
C

/*
* vim: sw=2 ts=2 expandtab
*
* THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"):
* <cfr34k@tkolb.de> wrote this file. As long as you retain this notice you can
* do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a pizza in return. - Thomas Kolb
*/
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <stdint.h>
#include "config.h"
#include "fft.h"
double hanning_buffer[BLOCK_LEN];
int lookup_table[BLOCK_LEN];
void init_fft(void) {
int i = 0;
int ri, b;
for(i = 0; i < BLOCK_LEN; i++) {
hanning_buffer[i] = 0.5 * (1 - cos(2 * M_PI * i / BLOCK_LEN));
}
for(i = 0; i < BLOCK_LEN; i++) {
ri = 0;
for(b = 0; b < FFT_EXPONENT; b++)
ri |= ((i >> b) & 1) << (FFT_EXPONENT - b - 1);
lookup_table[i] = ri;
}
}
void complex_to_absolute(double *re, double *im, double *result) {
int i;
for(i = 0; i < DATALEN; i++)
{
result[i] = sqrt( re[i]*re[i] + im[i]*im[i] );
}
}
void apply_hanning(sample *dftinput) {
int i;
for(i = 0; i < BLOCK_LEN; i++)
dftinput[i] *= hanning_buffer[i];
}
void fft_transform(sample *samples, double *resultRe, double *resultIm) {
int i;
int layer, part, element;
int num_parts, num_elements;
int left, right;
double x_left_re, x_left_im, x_right_re, x_right_im;
double param;
double sinval, cosval;
// re-arrange the input array according to the lookup table
// and store it into the real output array (as the input is obviously real).
// zero the imaginary output
for(i = 0; i < BLOCK_LEN; i++)
{
resultRe[lookup_table[i]] = samples[i];
resultIm[i] = 0;
}
// walk layers
for(layer = 0; layer < FFT_EXPONENT; layer++)
{
// number of parts in current layer
num_parts = 1 << (FFT_EXPONENT - layer - 1);
// walk parts of layer
for(part = 0; part < num_parts; part++)
{
// number of elements in current part
num_elements = (1 << layer);
// walk elements in part
for(element = 0; element < num_elements; element++)
{
// calculate index of element in left and right half of part
left = (1 << (layer + 1)) * part + element;
right = left + (1 << layer);
// buffer the elements for the calculation
x_left_re = resultRe[left];
x_left_im = resultIm[left];
x_right_re = resultRe[right];
x_right_im = resultIm[right];
// precalculate the parameter for sin and cos
param = -M_PI * element / (1 << layer);
// precalculate sinus and cosinus values for param
sinval = sin(param);
cosval = cos(param);
// combine the values according to a butterfly diagram
resultRe[left] = x_right_re + x_left_re * cosval - x_left_im * sinval;
resultIm[left] = x_right_im + x_left_im * cosval + x_left_re * sinval;
resultRe[right] = x_right_re - x_left_re * cosval + x_left_im * sinval;
resultIm[right] = x_right_im - x_left_im * cosval - x_left_re * sinval;
}
}
}
}
uint32_t find_loudest_frequency(double *absFFT) {
int maxPos = 0;
double maxVal = 0;
int i;
for(i = 0; i < BLOCK_LEN; i++) {
if(absFFT[i] > maxVal) {
maxPos = i;
maxVal = absFFT[i];
}
}
return (double)maxPos * SAMPLE_RATE / BLOCK_LEN;
}
double get_energy_in_band(double *fft, uint32_t minFreq, uint32_t maxFreq) {
int firstBlock = minFreq * BLOCK_LEN / SAMPLE_RATE;
int lastBlock = maxFreq * BLOCK_LEN / SAMPLE_RATE;
int i;
double energy = 0;
for(i = firstBlock; i < lastBlock; i++) {
energy += fft[i];
}
return energy;
}