DSPRelated.com
   Code Snippets

Block processing adaptative notch filter

Hernán Ordiales ● December 17, 2010 ● Coded in C++ 
bool Do(const Audio& input, const Audio& reference, Audio& output)
{
	const unsigned size = input.GetSize();
	const DataArray& in = input.GetBuffer();
	const DataArray& ref = reference.GetBuffer();
	DataArray& out = output.GetBuffer();

	const unsigned M = mConfig.GetFilterLength();
	const TData mu = mConfig.GetStep();
	const unsigned L = M-1;
	
	std::vector<TData> &wn = _filterCoef;

	TData r[M];
	for (unsigned k=0;k<size;k++)
	{
		TData acum = 0.;
		for (unsigned n=0, i=k;n<M;n++,i++)
		{
			if (i<L)
				r[n] = _oldRef[i];
			else
				r[n] = ref[i-L];
			acum += wn[n]*r[n]; // interference estimation
		}
		
		if (k<L)
			out[k] = _oldIn[k]-acum; // ECG signal estimation
		else
			out[k] = in[k-L]-acum; // ECG signal estimation
			
		for (unsigned n=0;n<M;n++) wn[n]+=mu*r[n]*out[k]; // new filter taps
	}

	for (unsigned k=0;k<L;k++)
	{
		_oldRef[k] = ref[size-L+k];
		_oldIn[k] = in[size-L+k];
	}

	return true;
}

Correlation on DSP KIT TMS320C6713 Simulator

● December 15, 2010 ● Coded in C++ for the TI C67x
#include <stdio.h>
#include <math.h>

double pi=3.14159;

float x[50],y[50];
float r[100];

void correlation(float *x,float *y, int nx, int ny);

int main()
{   
   

int ii;
for(ii=0;ii<50  ;ii++)
 {
	 x[ii]=sin(2.0*pi*ii*3/50);
 	 y[ii]=sin(2.0*pi*ii*3/50);

 }

correlation(x,y,50,50);

printf("Complete.\n");

return 0;
}

void correlation(float *x,float *y, int nx, int ny)
{
	int n=50,delay=0,maxdelay=50;
	int i,j;
   double mx,my,sx,sy,sxy,denom;
  
   
   /* Calculate the mean of the two series x[], y[] */
   mx = 0;
   my = 0;   
	   for (i=0;i<n;i++) 
		   {
			  mx += x[i];
			  my += y[i];
		   }
   mx /= n;
   my /= n;

   /* Calculate the denominator */
   sx = 0;
   sy = 0;
	   for (i=0;i<n;i++) 
		   {
			  sx += (x[i] - mx) * (x[i] - mx);
			  sy += (y[i] - my) * (y[i] - my);
		   }
   denom = sqrt(sx*sy);

   /* Calculate the correlation series */
   for (delay=-maxdelay;delay<maxdelay;delay++) 
	   {
		  sxy = 0;
			  for (i=0;i<n;i++) 
				  {
					 j = i + delay;
	 if (j < 0 || j >= n)
						continue;
	 else
						sxy += (x[i] - mx) * (y[j] - my);
					 // Or should it be (?)
					/* if (j < 0 || j >= n)
						sxy += (x[i] - mx) * (-my);
					 else
						sxy += (x[i] - mx) * (y[j] - my);*/
					 
				  }
r[delay+maxdelay]= ( sxy / denom);

Discrete Fourier Transform

Miguel De Jesus Rosario ● December 14, 2010●3 comments ● Coded in C++ 
#include<iostream>
#include<iomanip>
#include<cmath>
using namespace std;

#define pi 3.14159265

// class complex
class complex
{
public:
	//This class member function set the values of the class members real and imag
	void setvals(double r, double i) 
	{
		real=r;
		imag=i;
	}

	//This function get the values (by reference) of the class members real and imag
	void getvals(double &r, double &i) 
	{
		r=real;
		i=imag;
	}

private: 
	/* The class members real and imag represents the real and imaginary parts 
       of a complex number, respectively.*/
	double real;
	double imag;

}; // end of class complex

//**********************************************************************************************
/* function dft receives two parameters x and N. x is the input sequence containing 
the discrete-time samples and N is the number of DFT points.  The complex type paramater
xk receives a sequence of N-points frequency samples (this is the DFT of the input sequence*/

void dft(double x[], int N, complex xk[])
{
	double r,i;
	
	for(int k=0;k<N;k++) //k is the index of the N-point frequency sequence
	{
		xk[k].setvals(0,0); //initialize the values of the counters to zero  
		
		for(int n=0;n<N;n++) //n is the index of the discrete-time sequence
		{
			xk[k].getvals(r,i); // get the values of real and imag

			/*This is the computation of the real and imaginary parts of the DFT.
			The class member function setvals is used to update the value of the accumulators
			contaning the real and imaginary parts of the DFT samples*/

			xk[k].setvals(r + x[n]*cos(2*pi*k/N*n),i + x[n]*sin(2*pi*k/N*n));	
		} //end inner for-loop

	} //end outer for-loop

}//end dft function

//************************************************************************************************
//Declaration of the main() function
int main()
{
	int size; // The number of DFT samples 
	double r,i; // r = real part of the DFT; i = imaginary part of the DFT 

cout<<"Enter the desired number of DFT samples: ";
cin>>size;

double *x = new double[size];  //dynamic array x represent the discrete-time samples
complex *xk = new complex[size]; // dynamic array xk represent the DFT samples

cout<<"\nEnter the sequence x[n]: ";
for(int h=0;h<size;h++)
{
	cin>>x[h];
}

//Computation of the DFT of x
dft(x, size, xk);

system("CLS"); //clear screen

cout<<"Discrete Fourier Transform: "<<endl<<endl;

for(int h=0;h<size;h++)
{
	cout.precision(3); // display three (3) decimal places
	xk[h].getvals(r,i); //get the DFT samples values
	cout<<"xk["<<h<<"] = "<<fixed<< r <<" + "<<fixed<< i <<"j"<<endl;  // print the DFT samples
}

cout<<endl;

return 0;
}// end main function

FFT with down-sampling in time domain for C++

Emmanuel ● November 10, 2010●3 comments ● Coded in C++ 
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

#define N 128
#define PI 3.14159265358979

struct COMPLEX
{float real,imag;};

void qfft(struct COMPLEX *X, int n);   //FFT function
int q_log2(int n);
int qflip(int i, int M);     //self developed function like fliplr() in Matlab for bits.

struct COMPLEX w[N/2];       	    //twiddle factors
struct COMPLEX samples[N];          //input signal
float x1[N];                        //output signal
int i;
float s;		           

main(void)
{	
	for (i = 0 ; i<N/2 ; i++)	          // set up twiddle constants in w
    {
      	   w[i].real = cos(2*PI*i/N);  //Re component of twiddle constants
      	   w[i].imag =-sin(2*PI*i/N);  //Im component of twiddle constants
    }
	
/*Generating test signal******************/	
    for (i=0; i<N; i++)
	{
        s=0+i;
		samples[i].real = -1+(s*(2/128));
		samples[i].imag = 0; 		
	}
	printf("%f,%f",samples[50].real,s);
	
/************************************************/	
	system("PAUSE");
	//fft calculation
	qfft(samples,N);
        //magnitude calculation
	x1[i] = sqrt(samples[i].real*samples[i].real
	         + samples[i].imag*samples[i].imag);
	
    printf("El resultado es:\n");
	for (i=0; i<N; i++)
	{
		printf("%f\t%fi\n",samples[i].real,samples[i].imag);
	}
	
    system("PAUSE");
}

void qfft(struct COMPLEX *X, int n) 
{
	int k,l,m,k2,k21,Mk,index;
	struct COMPLEX op1,op2;
	struct COMPLEX X1;
	//nuber of stages
	int M=q_log2(n);
	//to array elements of the input signal
	for (k=0; k<N/2; k++)
	{
        index=qflip(k,M);
        X1=X[k];
        X[k]=X[index];
        X[index]=X1;
        printf("%d\n",index);
        }
	//entering to graph
	//k holds the stage
	for (k=1; k<=M; k++)
	{
		k2=(int)pow(2,k);
		k21=(int)pow(2,k-1);
		Mk=(int)pow(2,M-k);
		//l holds the butterfly
		for (l=1; l <= (n/k2); l++)
		{
			//m holds the upper element of the butterfly
			for (m=1; m<=k21; m++)
			{
				op1=X[(l-1)*k2+m-1];
				op2=X[(l-1)*k2+m+k21-1];
				//Butterfly equations
				//Complex product
				//(a+jb)(c+jd)=ac+j(ad+bc)-bd
				X[(l-1)*k2+m-1].real = op1.real + w[Mk*(m-1)].real*op2.real 
									 - w[Mk*(m-1)].imag*op2.imag ;
				X[(l-1)*k2+m-1].imag = op1.imag + w[Mk*(m-1)].real*op2.imag
									 + w[Mk*(m-1)].imag*op2.real ;
				//su elemento complemento:
				X[(l-1)*k2+m+k21-1].real = op1.real - w[Mk*(m-1)].real*op2.real 
									     + w[Mk*(m-1)].imag*op2.imag ;
				X[(l-1)*k2+m+k21-1].imag = op1.imag - w[Mk*(m-1)].real*op2.imag
									     - w[Mk*(m-1)].imag*op2.real ;
			}//end m
		}//end l
	}//end k
	return;
}//end function

int q_log2(int n)
{
	int res=0;
	while(n>1)
	{
		n=n/2;
		res=res+1;
	}
	return res;
}

/***Implementation of the qflip function*****/

/**************************************************/

Inplace matrix transposition

● November 3, 2010 ● Coded in C++ 
/* ------------------------------------------------------------------------- */
//
// This C++ code intents to do inplace transposition for matrix.
// It is more efficient for huge matrix, and quite generic as it uses classes 
// and templates.
//
// Feel free to use it and to modify it.
// Allocation can be improved according to your system.
// Compiles with g++
/* ------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <alloca.h>
#include <iostream>

using std::swap;
using std::cout;

template <typename T>
class matrix_t
{
   public:
      unsigned int row;
      unsigned int col;
      T *addr;

      /* ------------------------------------------------------------------------- */
      // FUNCTION: matrix_t 
      /* ------------------------------------------------------------------------- */
      matrix_t()
      {
         row = 0;
         col = 0;
         addr = NULL;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: init 
      // 
      // PARAM: _row
      // PARAM: _col
      // 
      // Init with allocation
      //
      // RETURN:
      /* ------------------------------------------------------------------------- */
      int init(unsigned int _row, unsigned int _col)
      {
         row = _row;
         col = _col;
         addr = (T *) calloc(sizeof(T), row * col);
         if (addr == NULL)
         {
            return -1;
         }
         return 0;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: term 
      // 
      // Terminate
      //
      // RETURN:
      /* ------------------------------------------------------------------------- */
      int term()
      {
         if (addr)
         {
            free(addr);
         }
         addr = NULL;
         return 0;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: fill 
      //
      // Fill with values
      /* ------------------------------------------------------------------------- */
      void fill()
      {
         unsigned int x = 0;
         int c = 0;
         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               addr[x * col + y] = (T) c++;
            }
         }
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: print 
      //
      // Print to standard output
      /* ------------------------------------------------------------------------- */
      void print()
      {
         unsigned int x = 0;
         cout << "row: " << row << "\n";
         cout << "col: " << col << "\n";
         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               cout.width(3);
               cout << addr[x * col + y] << " ";
            }
            cout << "\n";
         }
         cout << "\n";
         cout << "\n";
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: operator== 
      // 
      // PARAM: mat2
      //
      // Comparaison of two matrices
      // 
      // RETURN: 1 if equals; 0 if differents
      /* ------------------------------------------------------------------------- */
      int operator==(matrix_t mat2)
      {
         unsigned int x = 0;

         if (row != mat2.row)
         {
            fprintf(stderr, "Error : different number of rows (%d != %d)\n", row, mat2.row);
            return 0;
         }

         if (col != mat2.col)
         {
            fprintf(stderr, "Error : different number of columns (%d != %d)\n", col, mat2.col);
            return 0;
         }
         
         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               unsigned int pos = x * col + y;
               if (addr[pos] != mat2.addr[pos])
               {
                  return 0;
               }
            }
         }
         return 1;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: transpose 
      // 
      // Transposition
      /* ------------------------------------------------------------------------- */
      int transpose()
      {
         unsigned int x = 0;
         T *addr2 = (T *) calloc(sizeof(T), row * col);
         
         if (addr2 == NULL)
         {
            return -1;
         }

         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               int s = x * col + y;
               int d = y * row + x;
               addr2[d] = addr[s];
            }
         }

         free(addr);
         addr = addr2;
         swap<unsigned int>(row, col);
         return 0;
      }

#define BITSET_POS(ptr,x)   ((int)(((unsigned char*)(ptr))[(x)/8]))
#define BITSET_IS(ptr,x)   ((int)(((unsigned char*)(ptr))[(x)/8]) & (int)(1<<((x)&7)))
#define BITSET_SET(ptr,x)   ((unsigned char*)(ptr))[(x)/8] |= (unsigned char)(1<<((x)&7))

      /* ------------------------------------------------------------------------- */
      // FUNCTION: transpose_inplace 
      //  
      // Inplace transposition
      /* ------------------------------------------------------------------------- */
      int transpose_inplace()
      {
         unsigned int len = row * col;
         unsigned int size = (len/8) + 1;
         unsigned int i = 0;
         unsigned char *data = (unsigned char*) calloc(1, size);
         if (addr == NULL)
         {
            return -1;
         }
         
         while (i < len)
         {
            if (BITSET_POS(data, i) == 0xff)
            {
               i += 8;
               continue;
            }

            if (BITSET_IS(data, i))
            {
               i++;
               continue;
            }

            unsigned int begin = i;
            BITSET_SET(data, i);
            T tmp = (T)0;
            do
            {
               swap<T>(addr[i], tmp);
               unsigned int _row = i / col;
               unsigned int _col = i % col;
               i = _col * row + _row;
               BITSET_SET(data, i);
            }
            while (i != begin);

            swap<T>(addr[i], tmp);
            i = begin + 1;
         }

         free(data);
         swap<unsigned int>(row, col);
         return 0;
      }
}; 

/* ------------------------------------------------------------------------- */
// FUNCTION: main 
// 
// Test case : 
//    transpose a 9x23 matrix
//    transpose inplace a 9x23 matrix
//    and compare
//
/* ------------------------------------------------------------------------- */
int main()
{
   matrix_t<double> m1, m1_ref;

   m1.init(9, 23);
   m1_ref.init(9, 23);

   m1.fill();
   m1_ref.fill();

   m1.print();

   m1.transpose_inplace();
   m1_ref.transpose();

   m1.print();

   if (m1 == m1_ref)
   {
      printf("==\n");
   }
   else
   {
      printf("!=\n");
   }
   
   m1.term();
   m1_ref.term();

   return 0;
}