150 lines
3.6 KiB
C
150 lines
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 "lut.h"
|
|
#include "fft.h"
|
|
|
|
value_type 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(value_type *re, value_type *im, value_type *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, value_type *resultRe, value_type *resultIm) {
|
|
int i;
|
|
int layer, part, element;
|
|
int num_parts, num_elements;
|
|
|
|
int left, right;
|
|
|
|
value_type x_left_re, x_left_im, x_right_re, x_right_im;
|
|
value_type 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];
|
|
|
|
// use lookup table to get sinus and cosinus values for param
|
|
//param = -M_PI * element / (1 << layer);
|
|
sinval = lookup_sin(layer, element);
|
|
cosval = lookup_cos(layer, element);
|
|
|
|
// 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(value_type *absFFT) {
|
|
int maxPos = 0;
|
|
value_type maxVal = 0;
|
|
int i;
|
|
|
|
for(i = 0; i < BLOCK_LEN; i++) {
|
|
if(absFFT[i] > maxVal) {
|
|
maxPos = i;
|
|
maxVal = absFFT[i];
|
|
}
|
|
}
|
|
|
|
return (value_type)maxPos * SAMPLE_RATE / BLOCK_LEN;
|
|
}
|
|
|
|
value_type get_energy_in_band(value_type *fft, uint32_t minFreq, uint32_t maxFreq) {
|
|
int firstBlock = minFreq * BLOCK_LEN / SAMPLE_RATE;
|
|
int lastBlock = maxFreq * BLOCK_LEN / SAMPLE_RATE;
|
|
int i;
|
|
|
|
value_type energy = 0;
|
|
for(i = firstBlock; i < lastBlock; i++) {
|
|
energy += fft[i];
|
|
}
|
|
|
|
return energy;
|
|
}
|