[elektro-etc] periodicitas detektor

[Andras Huszti]

Hali!

Ha nem tudod a periodicitas frekvenciajat akkor FFT, ha tudod akkor sima
korrelacio. Pl DTMF-nel a korrelacio gyorsabb lehet.

En ezt hasznaltam egy PIC-es projectemben, de nem tudom 100%-ra hogy
mukodik-e:


#include <math.h>
#include "fft.h"	

#include "lcd.h"

#define SWAP(a,b)tempr=(a);(a)=(b);(b)=tempr

float PI = 3.14159265354;

static void DanielsonLanzcosRoutine(FftInputType *data, unsigned long
sizeofData, int isign);
static void BitReversal(FftInputType *data, unsigned long sizeofData);

void FFT (FftInputType *data, unsigned long sizeofData) {
    //the complex array is real + complex so the array 
    //as a size n = 2 * number of complex samples
    //real part is the data[index] and 
    //the complex part is the data[index+1]
	BitReversal(data, sizeofData);
	DanielsonLanzcosRoutine(data, sizeofData, 1);
}

void IFFT (FftInputType *data, unsigned long sizeofData) {
    unsigned long i = 0;
    unsigned long n = sizeofData / sizeof(FftInputType);
	BitReversal(data, sizeofData);
	DanielsonLanzcosRoutine(data, sizeofData, -1);
	for (i = 0; i < n / 2; i++) {
		data[i * 2] /= (n / 2);
	}
}

void calculateMagnitueAndPhaseFromComplex(FftInputType *data, unsigned
long sizeofData) {
	unsigned long x;
	unsigned long freqs = (sizeofData / sizeof(FftInputType)) / 2;
	float ampl = 0;
	float fiPI = 0;
	float fiC = 0;
	for (x = 0; x < freqs; x++) {
		ampl = sqrt((pow((float)data[x * 2], 2)) + (pow((float)data[(x * 2) +
1], 2)));
		fiPI = tan((float)data[(x * 2) + 1] / (float)data[x * 2]);
		fiC = (180 * fiPI / (2 * PI));
		if ((ampl < 0.0001) && (ampl > -0.0001)) {
			fiC = 0;
		}
		while (fiC > 360) {
			fiC -= 360;
		}
		while (fiC < -360) {
			fiC += 360;
		}
		data[x * 2] = (FftInputType)ampl;
		data[(x * 2) + 1] = (FftInputType)fiC ;
	}
}


static void BitReversal(FftInputType *data, unsigned long sizeofData) {
    //binary inversion (note that the indexes 
    //start from 0 witch means that the
    //real part of the complex is on the even-indexes 
    //and the complex part is on the odd-indexes
    unsigned long i = 0;
    unsigned long j = 0;
    unsigned long n = 0;
	double tempr = 0;
	n = (sizeofData / sizeof(FftInputType));
    for (i = 0; i < n / 2; i += 2) {
		unsigned long m = 0;
        if (j > i) {
            //swap the real part
            SWAP(data[j], data[i]);
            //swap the complex part
            SWAP(data[j + 1], data[i + 1]);
            // checks if the changes occurs in the first half
            // and use the mirrored effect on the second half
            if ((j / 2) < (n / 4)) {
                //swap the real part
                SWAP(data[(n - (i + 2))] , data[(n - (j + 2))]);
                //swap the complex part
                SWAP(data[(n - (i + 2)) + 1] , data[(n - (j + 2)) + 1]);
            }
        }
        m = n / 2;
        while (m >= 2 && j >= m) {
            j -= m;
            m = m / 2;
        }
        j += m;
    }
}

static void DanielsonLanzcosRoutine(FftInputType *data, unsigned long
sizeofData, int isign) {
	unsigned long n = 0;
    unsigned long mmax = 0;
    //Danielson-Lanzcos routine 
	n = (sizeofData / sizeof(FftInputType));
    mmax = 2;
    //external loop
    while (n > mmax) {
		double wtemp = 0;
		double theta = 0;
		double wi = 0;
		double wpi = 0;
		double wr = 0;
		double wpr = 0;
		unsigned long m = 0;
		unsigned long istep = 0;
		istep = mmax <<  1;
        theta = isign * (2 * PI / mmax);
        wtemp = sin(0.5 * theta);
        wpr = -2.0 * wtemp * wtemp;
        wpi = sin(theta);
        wr = 1.0;
        wi = 0.0;
        //internal loops
        for (m = 1; m < mmax; m += 2) {
			unsigned long i = 0;
			unsigned long j = 0;
            for (i = m; i <= n; i += istep) {
				double tempi = 0;
				double tempr = 0;
				j = i + mmax;
                tempr = wr * (float)data[j - 1] - wi * (float)data[j + 1
- 1];
                tempi = wr * (float)data[j + 1 - 1] + wi * (float)data[j
- 1];
                data[j - 1] = (float)data[i - 1] - tempr;
                data[j + 1 - 1] = (float)data[i + 1 - 1] - tempi;
                data[i - 1] += tempr;
                data[i + 1 - 1] += tempi;
            }
            wr = (wtemp = wr) * wpr - wi * wpi + wr;
            wi = wi * wpr + wtemp * wpi + wi;
        }
        mmax = istep;
    }
} 

#ifndef _FFT_H_
#define _FFT_H_

typedef float FftInputType;

extern float PI;

extern void FFT (FftInputType *data, unsigned long sizeofData);
extern void IFFT (FftInputType *data, unsigned long sizeofData);
extern void calculateMagnitueAndPhaseFromComplex(FftInputType *data,
unsigned long sizeofData);

#endif