Marcelo Rebonatto / Mbed 2 deprecated aquisicao

Primeira versao

Dependencies:   mbed EthernetInterface mbed-rtos

calculos.c@1:6c73db131ebc, 2015-03-05 (annotated)

Committer:
rebonatto
Date:
Thu Mar 05 20:38:46 2015 +0000
Revision:
1:6c73db131ebc
Valor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rebonatto 1:6c73db131ebc 1 /*
rebonatto 1:6c73db131ebc 2 * Arquivo calculos.c
rebonatto 1:6c73db131ebc 3 * Conten rotinas para calculo do RMS, DFT e FFT
rebonatto 1:6c73db131ebc 4 */
rebonatto 1:6c73db131ebc 5
rebonatto 1:6c73db131ebc 6 #include "Definitions.h"
rebonatto 1:6c73db131ebc 7
rebonatto 1:6c73db131ebc 8 #define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
rebonatto 1:6c73db131ebc 9 #define PI 3.14159265358979323846F
rebonatto 1:6c73db131ebc 10
rebonatto 1:6c73db131ebc 11 float RMS(float *vet, int amostras);
rebonatto 1:6c73db131ebc 12 void CalculateFFT(float *buffer, float *sen, float *cos, float *vm, int sign, int ch);
rebonatto 1:6c73db131ebc 13 float* ComplexFFT(float* data, int sign, int ch);
rebonatto 1:6c73db131ebc 14 float DFT(float *data, float *seno, float *coss);
rebonatto 1:6c73db131ebc 15
rebonatto 1:6c73db131ebc 16 float RMS(float *vet, int amostras){
rebonatto 1:6c73db131ebc 17 int i;
rebonatto 1:6c73db131ebc 18 float aux, soma=0;
rebonatto 1:6c73db131ebc 19
rebonatto 1:6c73db131ebc 20 for(i=0; i < amostras; i++){
rebonatto 1:6c73db131ebc 21 aux = vet[i] * vet[i];
rebonatto 1:6c73db131ebc 22 soma += aux;
rebonatto 1:6c73db131ebc 23 }
rebonatto 1:6c73db131ebc 24 soma = (float) soma / amostras;
rebonatto 1:6c73db131ebc 25 return (sqrt(soma));
rebonatto 1:6c73db131ebc 26 }
rebonatto 1:6c73db131ebc 27
rebonatto 1:6c73db131ebc 28 void CalculateFFT(float *buffer, float *sen, float *cos, float *vm, int sign, int ch)
rebonatto 1:6c73db131ebc 29 {
rebonatto 1:6c73db131ebc 30 int i;
rebonatto 1:6c73db131ebc 31 //float value[256];
rebonatto 1:6c73db131ebc 32 /*
rebonatto 1:6c73db131ebc 33 printf("Tamanho float %lu\n", sizeof(float));
rebonatto 1:6c73db131ebc 34 printf("Tamanho double %lu\n", sizeof(double));
rebonatto 1:6c73db131ebc 35 printf("Tamanho unsigned short int %lu\n", sizeof(unsigned short int));
rebonatto 1:6c73db131ebc 36 printf("Tamanho unsigned long %lu\n", sizeof(unsigned long));
rebonatto 1:6c73db131ebc 37 printf("Tamanho unsigned long long %lu\n", sizeof(unsigned long long));
rebonatto 1:6c73db131ebc 38
rebonatto 1:6c73db131ebc 39
rebonatto 1:6c73db131ebc 40 for(int i=0; i < Settings::get_Samples(); i++)
rebonatto 1:6c73db131ebc 41 printf("%d*",buffer[i]);
rebonatto 1:6c73db131ebc 42 printf("\n");
rebonatto 1:6c73db131ebc 43 */
rebonatto 1:6c73db131ebc 44 //printf("[0] %d %d %d %d\n", buffer[0], buffer[100], buffer[200], buffer[255]);
rebonatto 1:6c73db131ebc 45 /*
rebonatto 1:6c73db131ebc 46 for(i=0; i<Settings::get_Samples();i++)
rebonatto 1:6c73db131ebc 47 value[i]= (float) ( (buffer[i] - Settings::get_Offset(ch)) / Settings::get_Gain(ch) );
rebonatto 1:6c73db131ebc 48 */
rebonatto 1:6c73db131ebc 49
rebonatto 1:6c73db131ebc 50 printf("Chegou chamada\n");
rebonatto 1:6c73db131ebc 51 float* fft = ComplexFFT(buffer,1, 0); //deve desalocar memoria do ptr retornado
rebonatto 1:6c73db131ebc 52
rebonatto 1:6c73db131ebc 53 /*
rebonatto 1:6c73db131ebc 54 Mapa do vetor fft.
rebonatto 1:6c73db131ebc 55 O vetor tem 2 vezes o no. de amostras. Cada par de valores (portanto n e n+1), representam, respectivamente
rebonatto 1:6c73db131ebc 56 COS e SEN.
rebonatto 1:6c73db131ebc 57 Os dois primeiros valores reprensetam a frequencia 0Hz, portanto sao atribuidas ao valor medio.
rebonatto 1:6c73db131ebc 58 Os demais pares de valores representam a fundamental e suas harmonicas,
rebonatto 1:6c73db131ebc 59 sendo que se a fundamental for 60Hz, teremos: 60,120,180,240...
rebonatto 1:6c73db131ebc 60 Para a nossa aplicacao apenas as 12 primeiras harmonicas serao utilizadas (720Hz)
rebonatto 1:6c73db131ebc 61 */
rebonatto 1:6c73db131ebc 62
rebonatto 1:6c73db131ebc 63 //*vm = DFT(value, sen, cos);
rebonatto 1:6c73db131ebc 64 *vm = fft[0];
rebonatto 1:6c73db131ebc 65
rebonatto 1:6c73db131ebc 66 for(int i=1;i<AMOSTRAS/2;i++)
rebonatto 1:6c73db131ebc 67 {
rebonatto 1:6c73db131ebc 68 cos[i-1] = fft[i*2];
rebonatto 1:6c73db131ebc 69 sen[i-1] = fft[i*2+1];
rebonatto 1:6c73db131ebc 70 }
rebonatto 1:6c73db131ebc 71
rebonatto 1:6c73db131ebc 72 for(int i=0;i<AMOSTRAS;i++)
rebonatto 1:6c73db131ebc 73 {
rebonatto 1:6c73db131ebc 74 printf("[%dHz]\tsen %.4f\tcos %.4f\n", (i+1)*60, sen[i], cos[i]);
rebonatto 1:6c73db131ebc 75 if (i > 100)
rebonatto 1:6c73db131ebc 76 break;
rebonatto 1:6c73db131ebc 77 }
rebonatto 1:6c73db131ebc 78
rebonatto 1:6c73db131ebc 79 free(fft);
rebonatto 1:6c73db131ebc 80 //printf("[3] %d %d %d %d\n", buffer[0], buffer[100], buffer[200], buffer[255]);
rebonatto 1:6c73db131ebc 81 }
rebonatto 1:6c73db131ebc 82
rebonatto 1:6c73db131ebc 83
rebonatto 1:6c73db131ebc 84 float* ComplexFFT(float* data, int sign, int ch)
rebonatto 1:6c73db131ebc 85 {
rebonatto 1:6c73db131ebc 86
rebonatto 1:6c73db131ebc 87 //variables for the fft
rebonatto 1:6c73db131ebc 88 unsigned long n,mmax,m,j,istep,i;
rebonatto 1:6c73db131ebc 89 //double wtemp,wr,wpr,wpi,wi,theta,tempr,tempi;
rebonatto 1:6c73db131ebc 90 float wtemp,wr,wpr,wpi,wi,theta,tempr,tempi;
rebonatto 1:6c73db131ebc 91 float *vector;
rebonatto 1:6c73db131ebc 92 //the complex array is real+complex so the array
rebonatto 1:6c73db131ebc 93 //as a size n = 2* number of complex samples
rebonatto 1:6c73db131ebc 94 //real part is the data[index] and
rebonatto 1:6c73db131ebc 95 //the complex part is the data[index+1]
rebonatto 1:6c73db131ebc 96
rebonatto 1:6c73db131ebc 97 //new complex array of size n=2*sample_rate
rebonatto 1:6c73db131ebc 98 //if(vector==0)
rebonatto 1:6c73db131ebc 99 //vector=(float*)malloc(2*SAMPLE_RATE*sizeof(float)); era assim, define estava em Capture.h
rebonatto 1:6c73db131ebc 100
rebonatto 1:6c73db131ebc 101 printf("Chegou compex\n");
rebonatto 1:6c73db131ebc 102
rebonatto 1:6c73db131ebc 103 vector=(float*)malloc(2*AMOSTRAS*sizeof(float));
rebonatto 1:6c73db131ebc 104 if (vector == NULL)
rebonatto 1:6c73db131ebc 105 printf("Sem memoria\n");
rebonatto 1:6c73db131ebc 106 memset(vector,0,2*AMOSTRAS*sizeof(float));
rebonatto 1:6c73db131ebc 107
rebonatto 1:6c73db131ebc 108 //put the real array in a complex array
rebonatto 1:6c73db131ebc 109 //the complex part is filled with 0's
rebonatto 1:6c73db131ebc 110 //the remaining vector with no data is filled with 0's
rebonatto 1:6c73db131ebc 111 //for(n=0; n<SAMPLE_RATE;n++)era assim, define estava em Capture.h
rebonatto 1:6c73db131ebc 112
rebonatto 1:6c73db131ebc 113 printf("Alocou\n");
rebonatto 1:6c73db131ebc 114
rebonatto 1:6c73db131ebc 115 for(n=0; n<AMOSTRAS;n++)
rebonatto 1:6c73db131ebc 116 {
rebonatto 1:6c73db131ebc 117 if(n<AMOSTRAS){
rebonatto 1:6c73db131ebc 118 vector[2*n]= (float) data[n] ;
rebonatto 1:6c73db131ebc 119 // printf("%.4f$", vector[2*n]);
rebonatto 1:6c73db131ebc 120 }
rebonatto 1:6c73db131ebc 121 else
rebonatto 1:6c73db131ebc 122 vector[2*n]=0;
rebonatto 1:6c73db131ebc 123 vector[2*n+1]=0;
rebonatto 1:6c73db131ebc 124 }
rebonatto 1:6c73db131ebc 125
rebonatto 1:6c73db131ebc 126 printf("\n");
rebonatto 1:6c73db131ebc 127
rebonatto 1:6c73db131ebc 128 //printf("[1] %d %d %d %d\n", data[0], data[100], data[200], data[255]);
rebonatto 1:6c73db131ebc 129
rebonatto 1:6c73db131ebc 130 //binary inversion (note that the indexes
rebonatto 1:6c73db131ebc 131 //start from 0 witch means that the
rebonatto 1:6c73db131ebc 132 //real part of the complex is on the even-indexes
rebonatto 1:6c73db131ebc 133 //and the complex part is on the odd-indexes)
rebonatto 1:6c73db131ebc 134 //n=SAMPLE_RATE << 1; //multiply by 2era assim, define estava em Capture.h
rebonatto 1:6c73db131ebc 135 n=AMOSTRAS << 1; //multiply by 2
rebonatto 1:6c73db131ebc 136 j=0;
rebonatto 1:6c73db131ebc 137 for (i=0;i<n/2;i+=2) {
rebonatto 1:6c73db131ebc 138 if (j > i) {
rebonatto 1:6c73db131ebc 139 SWAP(vector[j],vector[i]);
rebonatto 1:6c73db131ebc 140 SWAP(vector[j+1],vector[i+1]);
rebonatto 1:6c73db131ebc 141 if((j/2)<(n/4)){
rebonatto 1:6c73db131ebc 142 SWAP(vector[(n-(i+2))],vector[(n-(j+2))]);
rebonatto 1:6c73db131ebc 143 SWAP(vector[(n-(i+2))+1],vector[(n-(j+2))+1]);
rebonatto 1:6c73db131ebc 144 }
rebonatto 1:6c73db131ebc 145 }
rebonatto 1:6c73db131ebc 146 m=n >> 1;
rebonatto 1:6c73db131ebc 147 while (m >= 2 && j >= m) {
rebonatto 1:6c73db131ebc 148 j -= m;
rebonatto 1:6c73db131ebc 149 m >>= 1;
rebonatto 1:6c73db131ebc 150 }
rebonatto 1:6c73db131ebc 151 j += m;
rebonatto 1:6c73db131ebc 152 }
rebonatto 1:6c73db131ebc 153 //end of the bit-reversed order algorithm
rebonatto 1:6c73db131ebc 154
rebonatto 1:6c73db131ebc 155 //Danielson-Lanzcos routine
rebonatto 1:6c73db131ebc 156 mmax=2;
rebonatto 1:6c73db131ebc 157 while (n > mmax) {
rebonatto 1:6c73db131ebc 158 istep=mmax << 1;
rebonatto 1:6c73db131ebc 159 theta=sign*(2*PI/mmax);
rebonatto 1:6c73db131ebc 160 wtemp=sin(0.5*theta);
rebonatto 1:6c73db131ebc 161 wpr = -2.0*wtemp*wtemp;
rebonatto 1:6c73db131ebc 162 wpi=sin(theta);
rebonatto 1:6c73db131ebc 163 wr=1.0;
rebonatto 1:6c73db131ebc 164 wi=0.0;
rebonatto 1:6c73db131ebc 165 for (m=1;m<mmax;m+=2) {
rebonatto 1:6c73db131ebc 166 for (i=m;i<=n;i+=istep) {
rebonatto 1:6c73db131ebc 167 j=i+mmax;
rebonatto 1:6c73db131ebc 168 tempr=wr*vector[j-1]-wi*vector[j];
rebonatto 1:6c73db131ebc 169 tempi=wr*vector[j]+wi*vector[j-1];
rebonatto 1:6c73db131ebc 170 vector[j-1]=vector[i-1]-tempr;
rebonatto 1:6c73db131ebc 171 vector[j]=vector[i]-tempi;
rebonatto 1:6c73db131ebc 172 vector[i-1] += tempr;
rebonatto 1:6c73db131ebc 173 vector[i] += tempi;
rebonatto 1:6c73db131ebc 174 }
rebonatto 1:6c73db131ebc 175 wr=(wtemp=wr)*wpr-wi*wpi+wr;
rebonatto 1:6c73db131ebc 176 wi=wi*wpr+wtemp*wpi+wi;
rebonatto 1:6c73db131ebc 177 }
rebonatto 1:6c73db131ebc 178 mmax=istep;
rebonatto 1:6c73db131ebc 179 }
rebonatto 1:6c73db131ebc 180 //end of the algorithm
rebonatto 1:6c73db131ebc 181
rebonatto 1:6c73db131ebc 182 // Ajustes a FFT
rebonatto 1:6c73db131ebc 183 for(i = 0; i < AMOSTRAS; i++ ){
rebonatto 1:6c73db131ebc 184 vector[i] = (float) ((2 * vector[i]) / AMOSTRAS );
rebonatto 1:6c73db131ebc 185 /*
rebonatto 1:6c73db131ebc 186 if (i % 2 == 1)
rebonatto 1:6c73db131ebc 187 vector[i] = vector[i] * -1;
rebonatto 1:6c73db131ebc 188 */
rebonatto 1:6c73db131ebc 189 }
rebonatto 1:6c73db131ebc 190
rebonatto 1:6c73db131ebc 191 //printf("[2] %d %d %d %d\n", data[0], data[100], data[200], data[255]);
rebonatto 1:6c73db131ebc 192
rebonatto 1:6c73db131ebc 193 return vector;
rebonatto 1:6c73db131ebc 194 }
rebonatto 1:6c73db131ebc 195
rebonatto 1:6c73db131ebc 196 float DFT(float *data, float *seno, float *coss){
rebonatto 1:6c73db131ebc 197 int i, j;
rebonatto 1:6c73db131ebc 198
rebonatto 1:6c73db131ebc 199 printf("Entrou DFT\n");
rebonatto 1:6c73db131ebc 200
rebonatto 1:6c73db131ebc 201 for(i=0; i < AMOSTRAS; i++)
rebonatto 1:6c73db131ebc 202 seno[i] = coss[i] = 0;
rebonatto 1:6c73db131ebc 203
rebonatto 1:6c73db131ebc 204 for(i=0; i < AMOSTRAS; i++){
rebonatto 1:6c73db131ebc 205 for(j = 0; j < AMOSTRAS; j++ ){
rebonatto 1:6c73db131ebc 206 coss[j] += (data[i] * (cos( (2 * PI * i * j) / AMOSTRAS ) ) ) ;
rebonatto 1:6c73db131ebc 207 seno[j] += (data[i] * (sin( (2 * PI * i * j) / AMOSTRAS ) ) ) ;
rebonatto 1:6c73db131ebc 208 }
rebonatto 1:6c73db131ebc 209 }
rebonatto 1:6c73db131ebc 210 printf("Primeiro Laco\n");
rebonatto 1:6c73db131ebc 211 for(j = 1; j < AMOSTRAS; j++ ){
rebonatto 1:6c73db131ebc 212 coss[j] = 2 * coss[j] / AMOSTRAS;
rebonatto 1:6c73db131ebc 213 seno[j] = 2 * seno[j] / AMOSTRAS ;
rebonatto 1:6c73db131ebc 214 }
rebonatto 1:6c73db131ebc 215
rebonatto 1:6c73db131ebc 216 printf("Segundo Laco\n");
rebonatto 1:6c73db131ebc 217 coss[0] = coss[0] / AMOSTRAS;
rebonatto 1:6c73db131ebc 218 seno[0] = seno[0] / AMOSTRAS;
rebonatto 1:6c73db131ebc 219 return (float) (coss[0] + seno[0] );
rebonatto 1:6c73db131ebc 220 }