Diff: Codes/SignalProcessor.cpp

Revision:

0:fac116e94d44

diff -r 000000000000 -r fac116e94d44 Codes/SignalProcessor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Codes/SignalProcessor.cpp	Tue Jan 05 11:47:35 2016 +0000
@@ -0,0 +1,452 @@
+/*
+ * SignalProcessor.cpp
+ *
+ *  Created on: 
+ *      Author: 
+ */
+ 
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "SignalProcessor.h"
+#include "limites.h"
+
+#define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
+#define PI 3.14159265358979323846F
+// 3.141597653564793332212487132
+//              3.14159265358979323846
+                
+
+/* Elementos vm2, under, over adicionados em 20/05/2014 por Rebonatto */
+/* vm2 eh o calculo do valor medio, under eh a cotagem dos valores do AD com 0 */
+/* over e a contagem do AD com 4095 */
+/* over e under sao para verificar se o processo de ajuste dos dados esta Ok e vm2 para conferir o vm da fft */ 
+void SignalProcessor::CalculateRMSBulk(float *result, float *vm2, int *under, int *over)
+{
+    int nChannel,nSample;    
+    short int v;
+    float val;
+    
+    // initialized vectors of results
+    for(nChannel=0;nChannel<NUMBER_OF_CHANNELS;nChannel++)
+        result[nChannel] = vm2[nChannel] = under[nChannel] = over[nChannel] = 0;
+    
+    // procedd calculus    
+    for(nChannel=0;nChannel<NUMBER_OF_CHANNELS;nChannel++)
+    {        
+        for(nSample=0;nSample<NUMBER_OF_SAMPLES;nSample++)
+        {            
+            v = Capture::GetValue(nSample, nChannel);
+            /* novos calculos  */    
+            vm2[nChannel] += v;
+            if (v <= 20)
+                under[nChannel] = under[nChannel] + 1;
+            if (v >= 4075)    
+                over[nChannel] = over[nChannel] + 1;
+            
+            val = (float)v;
+            
+            val -= Settings::get_Offset(nChannel);
+            val /= Settings::get_Gain(nChannel);
+            val *= val;
+            result[nChannel] += val;
+        }
+        result[nChannel] /= (float)NUMBER_OF_SAMPLES;
+        result[nChannel] = sqrt(result[nChannel]);
+        
+        /* novos calculos */
+        vm2[nChannel] /= (float)NUMBER_OF_SAMPLES;        
+    }
+}
+
+float SignalProcessor::CalculateRMS(int nChannel)
+{
+    float val, result=0;
+    int nSample;
+    short int v;
+    
+    for(nSample=0;nSample<NUMBER_OF_SAMPLES;nSample++)
+    {        
+        v = Capture::GetValue(nSample, nChannel);
+        val = (float)v;
+                                               // cada ponto   
+        //val -= Settings::get_Offset(nChannel); // diminui o offset
+        val /= Settings::get_Gain(nChannel);   // divide pelo ganhp
+        val *= val;                            // eleva ao quadrado
+        result += val;                         // soma  
+    }
+    result /= (float)NUMBER_OF_SAMPLES;  // divide pelo numero de amostras (256)
+    result = sqrt(result);
+    return result;
+}
+
+float SignalProcessor::CalculateRMSFloat( float *buffer,int nChannel)
+{
+    float result=0, val;
+    int nSample;
+    
+    for(nSample=0;nSample<NUMBER_OF_SAMPLES;nSample++)
+    {
+        val = buffer[nSample];
+        /*
+        short int v = buffer[nSample];
+        float val = (float)v;
+        */                                        // cada ponto   
+        //val -= Settings::get_Offset(nChannel);  // diminui o offset
+        val /= Settings::get_Gain(nChannel);      // divide pelo ganho
+        val *= val;                               // eleva ao quadrado
+        result += val;                            // soma  
+    }
+    result /= NUMBER_OF_SAMPLES;  // divide pelo numero de amostras (256)
+    result = sqrt(result);
+    return result;
+}
+
+
+void SignalProcessor::CalculateFFT(float *buffer,float *sen,float *cos,float *vm,int sign, int ch)
+{    
+    /*
+    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) );    
+    */
+    
+    //float* fft = ComplexFFT(buffer,1, ch);  //deve desalocar memoria do ptr retornado
+    ComplexFFT(buffer,1, ch);  //deve desalocar memoria do ptr retornado
+    
+    /* usado em teste
+    *vm = ComplexFFT(buffer, fft, 1, ch);  //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 = buffer[0];
+    //printf("Valor medio da FFT %.4f \n", buffer[0]);
+    
+    for(int i=1;i<Settings::get_MaxHarmonics()+1;i++)
+    {
+        cos[i-1] = buffer[i*2];
+        sen[i-1] = buffer[i*2+1];
+    }
+    
+    /*
+    for(int i=0;i<Settings::get_MaxHarmonics();i++)
+    {
+        printf("[%dHz]\tsen %.4f\tcos %.4f\n", (i+1)*60, sen[i], cos[i]);
+    }
+    */
+    
+    //free(fft);    
+    
+    //printf("[3] %d %d %d %d\n", buffer[0], buffer[100], buffer[200], buffer[255]);
+}
+
+float* SignalProcessor::ComplexFFT(float* data, int sign, int ch)
+{
+
+    //variables for the fft 
+    unsigned long n,mmax,m,j,istep,i, met;
+    //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]
+
+    /*
+    printf("Original\n");
+    for(int i=0; i < NUMBER_OF_SAMPLES; i++)
+        printf("%.2f ", data[i]);
+    printf("\n");
+    */
+    
+    //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("Antes malloc\n");
+    /*
+    vector=(float*)malloc(2*NUMBER_OF_SAMPLES*sizeof(float));
+    memset(vector,0,2*NUMBER_OF_SAMPLES*sizeof(float));
+    */
+    //printf("DEpois malloc\n");    
+    
+    // DisplayRAMBanks();
+    
+    //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       
+            
+    /*
+    for(n=0; n<NUMBER_OF_SAMPLES;n++)
+    {
+        if(n<NUMBER_OF_SAMPLES){
+            //vector[2*n]= (float) ( (data[n] - Settings::get_Offset(ch)) / Settings::get_Gain(ch) );            
+            vector[2*n]= (float) data[n] ;            
+            //  printf("%.4f$", vector[2*n]);
+            }
+        else
+            vector[2*n]=0;
+        vector[2*n+1]=0;
+    }    
+    */
+    /* trazendo o vetor em float, com metade ocupada, mudança dos valores reais e complexos 
+    //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
+    */
+    /*    
+    printf("Original\n");
+    for(int i=0; i < NUMBER_OF_SAMPLES*2; i++)
+        printf("%.2f ", data[i]);
+    printf("\n");
+    */    
+    met=NUMBER_OF_SAMPLES-1;    
+    for(n=NUMBER_OF_SAMPLES*2-1; n > 0; n--){
+        if (n % 2 == 0){
+            data[n] = data[met];
+            met--;
+        }
+        else
+            data[n] = 0;
+    }    
+    
+    /*
+    printf("Modificado\n");
+    for(int i=0; i < NUMBER_OF_SAMPLES*2; i++)
+        printf("%.2f ", data[i]);
+    printf("\n");
+    */
+    
+    //printf("[0] %.2f [100] %.2f [201] %.2f [255] %.2f\n", data[0], data[100], data[201], 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=NUMBER_OF_SAMPLES << 1; //multiply by 2
+    j=0;
+    for (i=0;i<n/2;i+=2) {
+        if (j > i) {
+            SWAP(data[j],data[i]);
+            SWAP(data[j+1],data[i+1]);
+            if((j/2)<(n/4)){
+                SWAP(data[(n-(i+2))],data[(n-(j+2))]);
+                SWAP(data[(n-(i+2))+1],data[(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*data[j-1]-wi*data[j];
+                tempi=wr*data[j]+wi*data[j-1];
+                data[j-1]=data[i-1]-tempr;
+                data[j]=data[i]-tempi;
+                data[i-1] += tempr;
+                data[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 < NUMBER_OF_SAMPLES*2; i++ ){          
+        data[i] = (float) ((2 * data[i]) / NUMBER_OF_SAMPLES );
+        
+        if (i % 2 == 1)
+            data[i] = data[i] * -1;        
+    }
+    
+    //printf("[2] %d %d %d %d\n", data[0], data[100], data[200], data[255]);
+    /*
+    printf("Na funcao\n");
+    for(int i=1;i<Settings::get_MaxHarmonics()*2+1;i++)
+    {
+        printf("[%dHz]\tsen %.4f\tcos %.4f\n", i*60, data[i*2+1], data[i*2]);
+    }          
+    */
+    return NULL;
+}
+
+float SignalProcessor::ComplexFFTTeste(unsigned short int* data, float *vector, 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("Antes malloc\n");
+    //vector=(float*)malloc(2*Settings::get_Samples()*sizeof(float));
+    memset(vector,0,2*NUMBER_OF_SAMPLES*sizeof(float));
+    //printf("DEpois memset\n");
+    
+    
+    DisplayRAMBanks();
+    
+    //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
+            
+    for(n=0; n<NUMBER_OF_SAMPLES;n++)
+    {
+        if(n<NUMBER_OF_SAMPLES){
+            //vector[2*n]= (float) ( (data[n] - Settings::get_Offset(ch)) / Settings::get_Gain(ch) );            
+            vector[2*n]= (float) data[n] ;            
+            //  printf("%.4f$", vector[2*n]);
+            }
+        else
+            vector[2*n]=0;
+        vector[2*n+1]=0;
+    }
+
+    //printf("Passou primeiro lcao\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=NUMBER_OF_SAMPLES << 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
+    
+    printf("Passou Segundo lcao\n");    
+
+    //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
+
+    printf("Fim FFT\n");    
+    /*
+    // Ajustes a FFT
+    for(i = 0; i < Settings::get_Samples()*2; i++ ){          
+        vector[i] = (float) ((2 * vector[i]) / Settings::get_Samples() );
+        
+        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[0];
+}
+
+
+/*
+float SignalProcessor::DFT(float *data, float *seno, float *coss){
+    int i, j;
+    
+    for(i=0; i < Settings::get_MaxHarmonics()+1; i++)
+        seno[i] = coss[i] = 0;
+    
+    for(i=0; i < Settings::get_Samples(); i++){
+        for(j = 0; j < Settings::get_MaxHarmonics()+1; j++ ){          
+            coss[j] += (data[i] * (cos( (2 * PI * i * j) / Settings::get_Samples() ) ) ) ;
+            seno[j] += (data[i] * (sin( (2 * PI * i * j) / Settings::get_Samples() ) ) ) ;
+        }
+    }
+       
+    for(j = 1; j < Settings::get_MaxHarmonics()+1; j++ ){          
+        coss[j] = 2 * coss[j] / Settings::get_Samples();
+        seno[j] = 2 * seno[j] / Settings::get_Samples() ;
+    }
+    return (float) (coss[0] / Settings::get_Samples()) + (seno[0] / Settings::get_Samples());
+}
+*/
+