Marcelo Rebonatto / Mbed 2 deprecated PMED_Calibra

First Version

Dependencies:   EthernetInterface mbed-rtos mbed

Codes/SignalProcessor.cpp@3:94a128e0f316, 2016-03-07 (annotated)

Committer:
rebonatto
Date:
Mon Mar 07 19:20:49 2016 +0000
Revision:
3:94a128e0f316
Parent:
0:9df41090ba33 
Vers?o est?vel com nova forma de aquisi??o de valores e limpeza de c?digo desnecess?rio.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rebonatto 0:9df41090ba33 1 /*
rebonatto 0:9df41090ba33 2 * SignalProcessor.cpp
rebonatto 0:9df41090ba33 3 *
rebonatto 0:9df41090ba33 4 * Created on:
rebonatto 0:9df41090ba33 5 * Author:
rebonatto 0:9df41090ba33 6 */
rebonatto 0:9df41090ba33 7
rebonatto 0:9df41090ba33 8 #include <math.h>
rebonatto 0:9df41090ba33 9 #include <stdlib.h>
rebonatto 0:9df41090ba33 10 #include <stdio.h>
rebonatto 0:9df41090ba33 11 #include <string.h>
rebonatto 0:9df41090ba33 12
rebonatto 0:9df41090ba33 13 #include "SignalProcessor.h"
rebonatto 0:9df41090ba33 14
rebonatto 0:9df41090ba33 15 #define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
rebonatto 0:9df41090ba33 16 #define PI 3.14159265358979323846F
rebonatto 0:9df41090ba33 17 // 3.141597653564793332212487132
rebonatto 0:9df41090ba33 18 // 3.14159265358979323846
rebonatto 0:9df41090ba33 19
rebonatto 0:9df41090ba33 20
rebonatto 0:9df41090ba33 21 /* Elementos vm2, under, over adicionados em 20/05/2014 por Rebonatto */
rebonatto 0:9df41090ba33 22 /* vm2 eh o calculo do valor medio, under eh a cotagem dos valores do AD com 0 */
rebonatto 0:9df41090ba33 23 /* over e a contagem do AD com 4095 */
rebonatto 0:9df41090ba33 24 /* over e under sao para verificar se o processo de ajuste dos dados esta Ok e vm2 para conferir o vm da fft */
rebonatto 0:9df41090ba33 25 void SignalProcessor::CalculateRMSBulk(float *result, float *vm2, int *under, int *over)
rebonatto 0:9df41090ba33 26 {
rebonatto 0:9df41090ba33 27 int nChannel,nSample;
rebonatto 0:9df41090ba33 28
rebonatto 0:9df41090ba33 29 for(nChannel=0;nChannel<Settings::get_MaxChannels();nChannel++)
rebonatto 0:9df41090ba33 30 result[nChannel] = vm2[nChannel] = under[nChannel] = over[nChannel] = 0;
rebonatto 0:9df41090ba33 31
rebonatto 0:9df41090ba33 32 for(nChannel=0;nChannel<Settings::get_MaxChannels();nChannel++)
rebonatto 0:9df41090ba33 33 {
rebonatto 0:9df41090ba33 34 for(nSample=0;nSample<Settings::get_Samples();nSample++)
rebonatto 0:9df41090ba33 35 {
rebonatto 0:9df41090ba33 36 unsigned short int v = Capture::GetValue(nSample, nChannel);
rebonatto 0:9df41090ba33 37 /* novos calculos */
rebonatto 0:9df41090ba33 38 vm2[nChannel] += v;
rebonatto 0:9df41090ba33 39 if (v <= 20)
rebonatto 0:9df41090ba33 40 under[nChannel] = under[nChannel] + 1;
rebonatto 0:9df41090ba33 41 if (v >= 4075)
rebonatto 0:9df41090ba33 42 over[nChannel] = over[nChannel] + 1;
rebonatto 0:9df41090ba33 43 float val = (float)v;
rebonatto 0:9df41090ba33 44
rebonatto 0:9df41090ba33 45 val -= Settings::get_Offset(nChannel);
rebonatto 0:9df41090ba33 46 val /= Settings::get_Gain(nChannel);
rebonatto 0:9df41090ba33 47 val *= val;
rebonatto 0:9df41090ba33 48 result[nChannel] += val;
rebonatto 0:9df41090ba33 49 }
rebonatto 0:9df41090ba33 50 result[nChannel] /= (float)Settings::get_Samples();
rebonatto 0:9df41090ba33 51 result[nChannel] = sqrt(result[nChannel]);
rebonatto 0:9df41090ba33 52
rebonatto 0:9df41090ba33 53 /* novos calculos */
rebonatto 0:9df41090ba33 54 vm2[nChannel] /= (float)Settings::get_Samples();
rebonatto 0:9df41090ba33 55 }
rebonatto 0:9df41090ba33 56 }
rebonatto 0:9df41090ba33 57
rebonatto 0:9df41090ba33 58 float SignalProcessor::CalculateRMS(unsigned short int *buffer,int nChannel)
rebonatto 0:9df41090ba33 59 {
rebonatto 0:9df41090ba33 60 float result=0;
rebonatto 0:9df41090ba33 61 int nSample;
rebonatto 0:9df41090ba33 62
rebonatto 0:9df41090ba33 63 for(nSample=0;nSample<Settings::get_Samples();nSample++)
rebonatto 0:9df41090ba33 64 {
rebonatto 0:9df41090ba33 65
rebonatto 0:9df41090ba33 66 unsigned short int v = buffer[nSample];
rebonatto 0:9df41090ba33 67 float val = (float)v;
rebonatto 0:9df41090ba33 68 // cada ponto
rebonatto 0:9df41090ba33 69 val -= Settings::get_Offset(nChannel); // diminui o offset
rebonatto 0:9df41090ba33 70 val /= Settings::get_Gain(nChannel); // divide pelo ganhp
rebonatto 0:9df41090ba33 71 val *= val; // eleva ao quadrado
rebonatto 0:9df41090ba33 72 result += val; // soma
rebonatto 0:9df41090ba33 73 }
rebonatto 0:9df41090ba33 74 result /= (float)Settings::get_Samples(); // divide pelo numero de amostras (256)
rebonatto 0:9df41090ba33 75 result = sqrt(result);
rebonatto 0:9df41090ba33 76 return result;
rebonatto 0:9df41090ba33 77 }
rebonatto 0:9df41090ba33 78
rebonatto 0:9df41090ba33 79 void SignalProcessor::CalculateFFT(unsigned short int *buffer,float *sen,float *cos,float *vm,int sign, int ch)
rebonatto 0:9df41090ba33 80 {
rebonatto 0:9df41090ba33 81 //int i;
rebonatto 0:9df41090ba33 82 //float value[256];
rebonatto 0:9df41090ba33 83 /*
rebonatto 0:9df41090ba33 84 printf("Tamanho float %lu\n", sizeof(float));
rebonatto 0:9df41090ba33 85 printf("Tamanho double %lu\n", sizeof(double));
rebonatto 0:9df41090ba33 86 printf("Tamanho unsigned short int %lu\n", sizeof(unsigned short int));
rebonatto 0:9df41090ba33 87 printf("Tamanho unsigned long %lu\n", sizeof(unsigned long));
rebonatto 0:9df41090ba33 88 printf("Tamanho unsigned long long %lu\n", sizeof(unsigned long long));
rebonatto 0:9df41090ba33 89 */
rebonatto 0:9df41090ba33 90
rebonatto 0:9df41090ba33 91 /*
rebonatto 0:9df41090ba33 92 for(int i=0; i < Settings::get_Samples(); i++)
rebonatto 0:9df41090ba33 93 printf("%d*",buffer[i]);
rebonatto 0:9df41090ba33 94 printf("\n");
rebonatto 0:9df41090ba33 95 */
rebonatto 0:9df41090ba33 96
rebonatto 0:9df41090ba33 97 //printf("[0] %d %d %d %d\n", buffer[0], buffer[100], buffer[200], buffer[255]);
rebonatto 0:9df41090ba33 98 /*
rebonatto 0:9df41090ba33 99 for(i=0; i<Settings::get_Samples();i++)
rebonatto 0:9df41090ba33 100 value[i]= (float) ( (buffer[i] - Settings::get_Offset(ch)) / Settings::get_Gain(ch) );
rebonatto 0:9df41090ba33 101 */
rebonatto 0:9df41090ba33 102 printf("Antes ComplexFFT\n");
rebonatto 0:9df41090ba33 103 float* fft = ComplexFFT(buffer,1, ch); //deve desalocar memoria do ptr retornado
rebonatto 0:9df41090ba33 104 printf("Passou ComplexFFT\n");
rebonatto 0:9df41090ba33 105
rebonatto 0:9df41090ba33 106 /*
rebonatto 0:9df41090ba33 107 Mapa do vetor fft.
rebonatto 0:9df41090ba33 108 O vetor tem 2 vezes o no. de amostras. Cada par de valores (portanto n e n+1), representam, respectivamente
rebonatto 0:9df41090ba33 109 COS e SEN.
rebonatto 0:9df41090ba33 110 Os dois primeiros valores reprensetam a frequencia 0Hz, portanto sao atribuidas ao valor medio.
rebonatto 0:9df41090ba33 111 Os demais pares de valores representam a fundamental e suas harmonicas,
rebonatto 0:9df41090ba33 112 sendo que se a fundamental for 60Hz, teremos: 60,120,180,240...
rebonatto 0:9df41090ba33 113 Para a nossa aplicacao apenas as 12 primeiras harmonicas serao utilizadas (720Hz)
rebonatto 0:9df41090ba33 114 */
rebonatto 0:9df41090ba33 115
rebonatto 0:9df41090ba33 116 //*vm = DFT(value, sen, cos);
rebonatto 0:9df41090ba33 117 *vm = fft[0];
rebonatto 0:9df41090ba33 118
rebonatto 0:9df41090ba33 119 for(int i=1;i<Settings::get_MaxHarmonics()+1;i++)
rebonatto 0:9df41090ba33 120 {
rebonatto 0:9df41090ba33 121 cos[i-1] = fft[i*2];
rebonatto 0:9df41090ba33 122 sen[i-1] = fft[i*2+1];
rebonatto 0:9df41090ba33 123 }
rebonatto 0:9df41090ba33 124
rebonatto 0:9df41090ba33 125 for(int i=0;i<Settings::get_MaxHarmonics();i++)
rebonatto 0:9df41090ba33 126 {
rebonatto 0:9df41090ba33 127 printf("[%dHz]\tsen %.4f\tcos %.4f\n", (i+1)*60, sen[i], cos[i]);
rebonatto 0:9df41090ba33 128 }
rebonatto 0:9df41090ba33 129
rebonatto 0:9df41090ba33 130 free(fft);
rebonatto 0:9df41090ba33 131 //printf("[3] %d %d %d %d\n", buffer[0], buffer[100], buffer[200], buffer[255]);
rebonatto 0:9df41090ba33 132 }
rebonatto 0:9df41090ba33 133
rebonatto 0:9df41090ba33 134
rebonatto 0:9df41090ba33 135 float* SignalProcessor::ComplexFFT(unsigned short int* data, int sign, int ch)
rebonatto 0:9df41090ba33 136 {
rebonatto 0:9df41090ba33 137
rebonatto 0:9df41090ba33 138 //variables for the fft
rebonatto 0:9df41090ba33 139 unsigned long n,mmax,m,j,istep,i;
rebonatto 0:9df41090ba33 140 //double wtemp,wr,wpr,wpi,wi,theta,tempr,tempi;
rebonatto 0:9df41090ba33 141 float wtemp,wr,wpr,wpi,wi,theta,tempr,tempi;
rebonatto 0:9df41090ba33 142 float *vector;
rebonatto 0:9df41090ba33 143 //the complex array is real+complex so the array
rebonatto 0:9df41090ba33 144 //as a size n = 2* number of complex samples
rebonatto 0:9df41090ba33 145 //real part is the data[index] and
rebonatto 0:9df41090ba33 146 //the complex part is the data[index+1]
rebonatto 0:9df41090ba33 147
rebonatto 0:9df41090ba33 148 //new complex array of size n=2*sample_rate
rebonatto 0:9df41090ba33 149 //if(vector==0)
rebonatto 0:9df41090ba33 150 //vector=(float*)malloc(2*SAMPLE_RATE*sizeof(float)); era assim, define estava em Capture.h
rebonatto 0:9df41090ba33 151
rebonatto 0:9df41090ba33 152 printf("Antes malloc\n");
rebonatto 0:9df41090ba33 153 vector=(float*)malloc(2*Settings::get_Samples()*sizeof(float));
rebonatto 0:9df41090ba33 154 memset(vector,0,2*Settings::get_Samples()*sizeof(float));
rebonatto 0:9df41090ba33 155 printf("DEpois malloc\n");
rebonatto 0:9df41090ba33 156 //put the real array in a complex array
rebonatto 0:9df41090ba33 157 //the complex part is filled with 0's
rebonatto 0:9df41090ba33 158 //the remaining vector with no data is filled with 0's
rebonatto 0:9df41090ba33 159 //for(n=0; n<SAMPLE_RATE;n++)era assim, define estava em Capture.h
rebonatto 0:9df41090ba33 160
rebonatto 0:9df41090ba33 161 for(n=0; n<Settings::get_Samples();n++)
rebonatto 0:9df41090ba33 162 {
rebonatto 0:9df41090ba33 163 if(n<Settings::get_Samples()){
rebonatto 0:9df41090ba33 164 //vector[2*n]= (float) ( (data[n] - Settings::get_Offset(ch)) / Settings::get_Gain(ch) );
rebonatto 0:9df41090ba33 165 vector[2*n]= (float) data[n] ;
rebonatto 0:9df41090ba33 166 // printf("%.4f$", vector[2*n]);
rebonatto 0:9df41090ba33 167 }
rebonatto 0:9df41090ba33 168 else
rebonatto 0:9df41090ba33 169 vector[2*n]=0;
rebonatto 0:9df41090ba33 170 vector[2*n+1]=0;
rebonatto 0:9df41090ba33 171 }
rebonatto 0:9df41090ba33 172
rebonatto 0:9df41090ba33 173 //printf("\n");
rebonatto 0:9df41090ba33 174
rebonatto 0:9df41090ba33 175 //printf("[1] %d %d %d %d\n", data[0], data[100], data[200], data[255]);
rebonatto 0:9df41090ba33 176
rebonatto 0:9df41090ba33 177 //binary inversion (note that the indexes
rebonatto 0:9df41090ba33 178 //start from 0 witch means that the
rebonatto 0:9df41090ba33 179 //real part of the complex is on the even-indexes
rebonatto 0:9df41090ba33 180 //and the complex part is on the odd-indexes)
rebonatto 0:9df41090ba33 181 //n=SAMPLE_RATE << 1; //multiply by 2era assim, define estava em Capture.h
rebonatto 0:9df41090ba33 182 n=Settings::get_Samples() << 1; //multiply by 2
rebonatto 0:9df41090ba33 183 j=0;
rebonatto 0:9df41090ba33 184 for (i=0;i<n/2;i+=2) {
rebonatto 0:9df41090ba33 185 if (j > i) {
rebonatto 0:9df41090ba33 186 SWAP(vector[j],vector[i]);
rebonatto 0:9df41090ba33 187 SWAP(vector[j+1],vector[i+1]);
rebonatto 0:9df41090ba33 188 if((j/2)<(n/4)){
rebonatto 0:9df41090ba33 189 SWAP(vector[(n-(i+2))],vector[(n-(j+2))]);
rebonatto 0:9df41090ba33 190 SWAP(vector[(n-(i+2))+1],vector[(n-(j+2))+1]);
rebonatto 0:9df41090ba33 191 }
rebonatto 0:9df41090ba33 192 }
rebonatto 0:9df41090ba33 193 m=n >> 1;
rebonatto 0:9df41090ba33 194 while (m >= 2 && j >= m) {
rebonatto 0:9df41090ba33 195 j -= m;
rebonatto 0:9df41090ba33 196 m >>= 1;
rebonatto 0:9df41090ba33 197 }
rebonatto 0:9df41090ba33 198 j += m;
rebonatto 0:9df41090ba33 199 }
rebonatto 0:9df41090ba33 200 //end of the bit-reversed order algorithm
rebonatto 0:9df41090ba33 201
rebonatto 0:9df41090ba33 202 //Danielson-Lanzcos routine
rebonatto 0:9df41090ba33 203 mmax=2;
rebonatto 0:9df41090ba33 204 while (n > mmax) {
rebonatto 0:9df41090ba33 205 istep=mmax << 1;
rebonatto 0:9df41090ba33 206 theta=sign*(2*PI/mmax);
rebonatto 0:9df41090ba33 207 wtemp=sin(0.5*theta);
rebonatto 0:9df41090ba33 208 wpr = -2.0*wtemp*wtemp;
rebonatto 0:9df41090ba33 209 wpi=sin(theta);
rebonatto 0:9df41090ba33 210 wr=1.0;
rebonatto 0:9df41090ba33 211 wi=0.0;
rebonatto 0:9df41090ba33 212 for (m=1;m<mmax;m+=2) {
rebonatto 0:9df41090ba33 213 for (i=m;i<=n;i+=istep) {
rebonatto 0:9df41090ba33 214 j=i+mmax;
rebonatto 0:9df41090ba33 215 tempr=wr*vector[j-1]-wi*vector[j];
rebonatto 0:9df41090ba33 216 tempi=wr*vector[j]+wi*vector[j-1];
rebonatto 0:9df41090ba33 217 vector[j-1]=vector[i-1]-tempr;
rebonatto 0:9df41090ba33 218 vector[j]=vector[i]-tempi;
rebonatto 0:9df41090ba33 219 vector[i-1] += tempr;
rebonatto 0:9df41090ba33 220 vector[i] += tempi;
rebonatto 0:9df41090ba33 221 }
rebonatto 0:9df41090ba33 222 wr=(wtemp=wr)*wpr-wi*wpi+wr;
rebonatto 0:9df41090ba33 223 wi=wi*wpr+wtemp*wpi+wi;
rebonatto 0:9df41090ba33 224 }
rebonatto 0:9df41090ba33 225 mmax=istep;
rebonatto 0:9df41090ba33 226 }
rebonatto 0:9df41090ba33 227 //end of the algorithm
rebonatto 0:9df41090ba33 228
rebonatto 0:9df41090ba33 229 /*
rebonatto 0:9df41090ba33 230 // Ajustes a FFT
rebonatto 0:9df41090ba33 231 for(i = 0; i < Settings::get_Samples()*2; i++ ){
rebonatto 0:9df41090ba33 232 vector[i] = (float) ((2 * vector[i]) / Settings::get_Samples() );
rebonatto 0:9df41090ba33 233 if (i % 2 == 1)
rebonatto 0:9df41090ba33 234 vector[i] = vector[i] * -1;
rebonatto 0:9df41090ba33 235
rebonatto 0:9df41090ba33 236 }
rebonatto 0:9df41090ba33 237 */
rebonatto 0:9df41090ba33 238 //printf("[2] %d %d %d %d\n", data[0], data[100], data[200], data[255]);
rebonatto 0:9df41090ba33 239
rebonatto 0:9df41090ba33 240 return vector;
rebonatto 0:9df41090ba33 241 }
rebonatto 0:9df41090ba33 242
rebonatto 0:9df41090ba33 243 /*
rebonatto 0:9df41090ba33 244 float SignalProcessor::DFT(float *data, float *seno, float *coss){
rebonatto 0:9df41090ba33 245 int i, j;
rebonatto 0:9df41090ba33 246
rebonatto 0:9df41090ba33 247 for(i=0; i < Settings::get_MaxHarmonics()+1; i++)
rebonatto 0:9df41090ba33 248 seno[i] = coss[i] = 0;
rebonatto 0:9df41090ba33 249
rebonatto 0:9df41090ba33 250 for(i=0; i < Settings::get_Samples(); i++){
rebonatto 0:9df41090ba33 251 for(j = 0; j < Settings::get_MaxHarmonics()+1; j++ ){
rebonatto 0:9df41090ba33 252 coss[j] += (data[i] * (cos( (2 * PI * i * j) / Settings::get_Samples() ) ) ) ;
rebonatto 0:9df41090ba33 253 seno[j] += (data[i] * (sin( (2 * PI * i * j) / Settings::get_Samples() ) ) ) ;
rebonatto 0:9df41090ba33 254 }
rebonatto 0:9df41090ba33 255 }
rebonatto 0:9df41090ba33 256
rebonatto 0:9df41090ba33 257 for(j = 1; j < Settings::get_MaxHarmonics()+1; j++ ){
rebonatto 0:9df41090ba33 258 coss[j] = 2 * coss[j] / Settings::get_Samples();
rebonatto 0:9df41090ba33 259 seno[j] = 2 * seno[j] / Settings::get_Samples() ;
rebonatto 0:9df41090ba33 260 }
rebonatto 0:9df41090ba33 261 return (float) (coss[0] / Settings::get_Samples()) + (seno[0] / Settings::get_Samples());
rebonatto 0:9df41090ba33 262 }
rebonatto 0:9df41090ba33 263 */