Primeira versao
Dependencies: mbed EthernetInterface mbed-rtos
Diff: calculos.c
- Revision:
- 1:6c73db131ebc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/calculos.c Thu Mar 05 20:38:46 2015 +0000 @@ -0,0 +1,220 @@ +/* +* Arquivo calculos.c +* Conten rotinas para calculo do RMS, DFT e FFT +*/ + +#include "Definitions.h" + +#define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr +#define PI 3.14159265358979323846F + +float RMS(float *vet, int amostras); +void CalculateFFT(float *buffer, float *sen, float *cos, float *vm, int sign, int ch); +float* ComplexFFT(float* data, int sign, int ch); +float DFT(float *data, float *seno, float *coss); + +float RMS(float *vet, int amostras){ + int i; + float aux, soma=0; + + for(i=0; i < amostras; i++){ + aux = vet[i] * vet[i]; + soma += aux; + } + soma = (float) soma / amostras; + return (sqrt(soma)); +} + +void CalculateFFT(float *buffer, float *sen, float *cos, float *vm, int sign, int ch) +{ + int i; + //float value[256]; + /* + printf("Tamanho float %lu\n", sizeof(float)); + printf("Tamanho double %lu\n", sizeof(double)); + printf("Tamanho unsigned short int %lu\n", sizeof(unsigned short int)); + printf("Tamanho unsigned long %lu\n", sizeof(unsigned long)); + printf("Tamanho unsigned long long %lu\n", sizeof(unsigned long long)); + + + for(int i=0; i < Settings::get_Samples(); i++) + printf("%d*",buffer[i]); + printf("\n"); + */ + //printf("[0] %d %d %d %d\n", buffer[0], buffer[100], buffer[200], buffer[255]); + /* + for(i=0; i<Settings::get_Samples();i++) + value[i]= (float) ( (buffer[i] - Settings::get_Offset(ch)) / Settings::get_Gain(ch) ); + */ + + printf("Chegou chamada\n"); + float* fft = ComplexFFT(buffer,1, 0); //deve desalocar memoria do ptr retornado + + /* + Mapa do vetor fft. + O vetor tem 2 vezes o no. de amostras. Cada par de valores (portanto n e n+1), representam, respectivamente + COS e SEN. + Os dois primeiros valores reprensetam a frequencia 0Hz, portanto sao atribuidas ao valor medio. + Os demais pares de valores representam a fundamental e suas harmonicas, + sendo que se a fundamental for 60Hz, teremos: 60,120,180,240... + Para a nossa aplicacao apenas as 12 primeiras harmonicas serao utilizadas (720Hz) + */ + + //*vm = DFT(value, sen, cos); + *vm = fft[0]; + + for(int i=1;i<AMOSTRAS/2;i++) + { + cos[i-1] = fft[i*2]; + sen[i-1] = fft[i*2+1]; + } + + for(int i=0;i<AMOSTRAS;i++) + { + printf("[%dHz]\tsen %.4f\tcos %.4f\n", (i+1)*60, sen[i], cos[i]); + if (i > 100) + break; + } + + free(fft); + //printf("[3] %d %d %d %d\n", buffer[0], buffer[100], buffer[200], buffer[255]); +} + + +float* ComplexFFT(float* data, int sign, int ch) +{ + + //variables for the fft + unsigned long n,mmax,m,j,istep,i; + //double wtemp,wr,wpr,wpi,wi,theta,tempr,tempi; + float wtemp,wr,wpr,wpi,wi,theta,tempr,tempi; + float *vector; + //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] + + //new complex array of size n=2*sample_rate + //if(vector==0) + //vector=(float*)malloc(2*SAMPLE_RATE*sizeof(float)); era assim, define estava em Capture.h + + printf("Chegou compex\n"); + + vector=(float*)malloc(2*AMOSTRAS*sizeof(float)); + if (vector == NULL) + printf("Sem memoria\n"); + memset(vector,0,2*AMOSTRAS*sizeof(float)); + + //put the real array in a complex array + //the complex part is filled with 0's + //the remaining vector with no data is filled with 0's + //for(n=0; n<SAMPLE_RATE;n++)era assim, define estava em Capture.h + + printf("Alocou\n"); + + for(n=0; n<AMOSTRAS;n++) + { + if(n<AMOSTRAS){ + vector[2*n]= (float) data[n] ; + // printf("%.4f$", vector[2*n]); + } + else + vector[2*n]=0; + vector[2*n+1]=0; + } + + printf("\n"); + + //printf("[1] %d %d %d %d\n", data[0], data[100], data[200], data[255]); + + //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) + //n=SAMPLE_RATE << 1; //multiply by 2era assim, define estava em Capture.h + n=AMOSTRAS << 1; //multiply by 2 + j=0; + for (i=0;i<n/2;i+=2) { + if (j > i) { + SWAP(vector[j],vector[i]); + SWAP(vector[j+1],vector[i+1]); + if((j/2)<(n/4)){ + SWAP(vector[(n-(i+2))],vector[(n-(j+2))]); + SWAP(vector[(n-(i+2))+1],vector[(n-(j+2))+1]); + } + } + m=n >> 1; + while (m >= 2 && j >= m) { + j -= m; + m >>= 1; + } + j += m; + } + //end of the bit-reversed order algorithm + + //Danielson-Lanzcos routine + mmax=2; + while (n > mmax) { + istep=mmax << 1; + theta=sign*(2*PI/mmax); + wtemp=sin(0.5*theta); + wpr = -2.0*wtemp*wtemp; + wpi=sin(theta); + wr=1.0; + wi=0.0; + for (m=1;m<mmax;m+=2) { + for (i=m;i<=n;i+=istep) { + j=i+mmax; + tempr=wr*vector[j-1]-wi*vector[j]; + tempi=wr*vector[j]+wi*vector[j-1]; + vector[j-1]=vector[i-1]-tempr; + vector[j]=vector[i]-tempi; + vector[i-1] += tempr; + vector[i] += tempi; + } + wr=(wtemp=wr)*wpr-wi*wpi+wr; + wi=wi*wpr+wtemp*wpi+wi; + } + mmax=istep; + } + //end of the algorithm + + // Ajustes a FFT + for(i = 0; i < AMOSTRAS; i++ ){ + vector[i] = (float) ((2 * vector[i]) / AMOSTRAS ); + /* + if (i % 2 == 1) + vector[i] = vector[i] * -1; + */ + } + + //printf("[2] %d %d %d %d\n", data[0], data[100], data[200], data[255]); + + return vector; +} + +float DFT(float *data, float *seno, float *coss){ + int i, j; + + printf("Entrou DFT\n"); + + for(i=0; i < AMOSTRAS; i++) + seno[i] = coss[i] = 0; + + for(i=0; i < AMOSTRAS; i++){ + for(j = 0; j < AMOSTRAS; j++ ){ + coss[j] += (data[i] * (cos( (2 * PI * i * j) / AMOSTRAS ) ) ) ; + seno[j] += (data[i] * (sin( (2 * PI * i * j) / AMOSTRAS ) ) ) ; + } + } + printf("Primeiro Laco\n"); + for(j = 1; j < AMOSTRAS; j++ ){ + coss[j] = 2 * coss[j] / AMOSTRAS; + seno[j] = 2 * seno[j] / AMOSTRAS ; + } + + printf("Segundo Laco\n"); + coss[0] = coss[0] / AMOSTRAS; + seno[0] = seno[0] / AMOSTRAS; + return (float) (coss[0] + seno[0] ); +}