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?
User | Revision | Line number | New 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 | } |