dsp_filter.cpp

一个开源的心电图测量仪驱动和应用软件,可记录

// DSP_Filter.cpp: implementation of the DSP_Filter class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "ECG_1.h"
#include "DSP_Filter.h"
#include "math.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

DSP_Filter::DSP_Filter()
{
	PI = 3.141592;

}

DSP_Filter::~DSP_Filter()
{

}

void DSP_Filter::RFilter_Low(double *dest, int lenght, int fsmpl, double fk)
{
	//Low filter 1 stage
	int i=0;
	int lenght_1 = lenght + 100;
	//resize X and Y arrays
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	//Move array from dest in to X array
	MoveArr(X,dest,lenght);


	//Calculate filter coefficients recursive form
	double x = fk;
	double A0 = pow((1-x),4);
	double B1 = 4*x;
	double B2 = -6*x*x;
	double B3 = 4*x*x*x;
	double B4 = -x*x*x*x;
	//i rins from 2!!!
	for(i=4;i<lenght_1;i++)
	{
		*(Y+i) = (*(X+i)*A0 + *(Y+i-1)*B1 + *(Y+i-2)*B2
			+ *(Y+i-3)*B3 + *(Y+i-4)*B4)*1.005;
	}
	MoveArr(dest,Y,lenght);
	//Free the used memory
	Delete_Arrs();
}

void DSP_Filter::ZeroArrs(int lenght)
{
	for(int i = 0;i<lenght;i++)
	{
		*(Y+i) = 0;//Zero the filter array
		*(X+i) = 0;//Zero the remaine X array
	}

}

void DSP_Filter::MoveArr(double *dest, double *source, int lenght)
{
	//Move the array in to he *dest position
	for(int i=0;i<lenght;i++)
	{	
		*(dest+i) = *(source+i);
	}

}

void DSP_Filter::Message(double var)
{
	CString str;
	str.Format("%lf",var);
	MessageBox(NULL,str,"Message",MB_OK);

}

void DSP_Filter::RFilter_Noht(double *dest, int lenght, int fsmpl, double fcut,double bw)
{
	//Do not work good at cut of frequency =<0.1 fsmpl!!!
	int lenght_1 = lenght+100;
	Greate_Arrs(lenght_1);
	//Zerro the new maked arrays
	ZeroArrs(lenght_1);
	MoveArr(X,dest,lenght);
	//Calculate noht koefizients
	double K,R;
	fcut = fcut/double(fsmpl);
	R = 1-3*bw;
	K = (1-2*R*cos(2*PI*fcut)+R*R)/(2-2*cos(2*PI*fcut));
	
	double A0 = K;
	double A1 = -2*K*cos(2*PI*fcut);
	double A2 = K;
	double B1 = 2*R*cos(2*PI*fcut);
	double B2 = -R*R;
	//i rins from 2!!!
	for(int i=2;i<lenght_1;i++)
	{
		*(Y+i) = (*(X+i)*A0 + *(X+i-1)*A1 + *(X+i-2)*A2
			+ *(Y+i-1)*B1 + *(Y+i-2)*B2);
	}
	MoveArr(dest,Y,lenght);
	//Free memory
	Delete_Arrs();
}

void DSP_Filter::WFilter_High(double *dest, double *source, int lenght,double amp, int fsmpl, double fk)
{
	//Win=-sync filter hight > 0.67hz
	int lenght_1 = lenght+400;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,dest,lenght);
	int M = 200;
	int corect=0;
	if(fsmpl==250)
		corect=0;
	if(fsmpl==500)
		corect=10;

	SlideArrRight(Y,X,lenght_1,(M/2));
	MoveArr(X,Y,lenght_1);
	for(int i=0;i<lenght_1;i++)
	{
		*(Y+i)=0;
	}
	for(int j=M;j<lenght_1-M;j++)
	{
		for(i=0;i<M;i++)
		{
			*(Y+j)=*(Y+j)+*(X+j-i)*H[i];
		//	*(Y+j) = *(Y+j)+*(X+j+i)*H[i];
		}
	}
	SlideArrLeft(Y,Y,lenght_1,M-corect);//
//	FilterLowFromTo(Y,Y,0,M,lenght);
	MoveArr(dest,Y,lenght);
	//Free memory
	Delete_Arrs();

}

void DSP_Filter::InvertArr(double *dest, double *source, int lenght)
{
	//Invert the array
	//Array source first nust be normalized to 0+-
	int lenght_1 = lenght+100;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,dest,lenght);
	for(int i=0;i<lenght_1;i++)
	{
		*(Y+i) = *(X+i)*-1; 

	}
	MoveArr(dest,Y,lenght);
	Delete_Arrs();
}

void DSP_Filter::Delta1(double *dest, double *source, int lenght)
{
	int lenght_1 = lenght+100;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,source,lenght);

	//finding the destination array 1delta
	for(int i=0;i<lenght;i++)
	{
		*(Y+i) = *(X+i+1) - *(X+i);
	}
	MoveArr(dest,Y,lenght);
	Delete_Arrs();
}

void DSP_Filter::Greate_Arrs(int new_lenght)
{
	//Creates new arr X&Y with lenght = mew_lenght
	b_arr_flag = 1;
	X = new double [new_lenght];
	Y = new double [new_lenght];
}

void DSP_Filter::Delete_Arrs()
{
	//deletes the created arrs
	if(b_arr_flag == 1)
	{
		delete X;
		delete Y;
		b_arr_flag = 0;
	}
}

double DSP_Filter::Mean(double *source, int lenght)
{
	//finding the mean
	double MEAN=0;
	for(int i=0;i<lenght;i++)
	{
		MEAN = MEAN + *(source+i);
	}
	MEAN = MEAN/lenght;
//	CString str;
//	str.Format("%lf",MEAN);
//	MessageBox(str,"Mean",MB_OK);
	return MEAN;
}

void DSP_Filter::NormalizeArr(double *ar, double mean, int lenght)
{
	double temp = 0;
	for(int i=0;i<lenght;i++)
	{
		if(*(ar+i)>mean)
			temp = *(ar+i)-mean;
		if(*(ar+i)==mean)
			temp = 0;
		if(*(ar+i)<mean)
			temp = *(ar+i)-mean;
		*(ar+i) = temp;
	}
}

void DSP_Filter::SetArrToNormalize(double *dest,int lenght)
{
	NormalizeArr(dest,Mean(dest,lenght),lenght);

}

void DSP_Filter::DeltaReverse1(double *dest, double *source, int lenght)
{
	int lenght_1 = lenght+100;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,source,lenght);

	//finding the destination array reverse delta
	for(int i=0;i<lenght;i++)
	{
		*(Y+i+1) = *(Y+i) + *(X+i+1);
	}
	MoveArr(dest,Y,lenght);
	Delete_Arrs();

}

void DSP_Filter::Delta2(double *dest, double *source, int lenght)
{
	//Finding delta2
	int lenght_1 = lenght+100;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,source,lenght);
	
	//finding the destination array 1delta
	for(int i=0;i<lenght;i++)
	{
		*(Y+i) = *(X+i+2) - 2*(*(X+i+1)) + *(X+i);
	}
	MoveArr(dest,Y,lenght);
	Delete_Arrs();

}

void DSP_Filter::DeltaReverse2(double *dest, double *source, int lenght)
{
	//Finding reverce delta2
	DeltaReverse1(dest,source,lenght);
	DeltaReverse1(dest,source,lenght);
}

double DSP_Filter::StandardDeviation(double *source, int lenght)
{
	double dev = 0;
	double mean = Mean(source,lenght);
	for(int i=0;i<lenght;i++)
	{
		dev = dev + (*(source+i)-mean)*(*(source+i)-mean);
	}
	dev = dev/(lenght-1);
	dev = sqrt(dev);
	return dev;

}

void DSP_Filter::SlideArrLeft(double *dest, double *source, int lenght, int slide)
{
	for(int i=0;i<lenght-slide;i++)
	{
		*(dest+i) = *(source+i+slide);
	}
}

void DSP_Filter::SlideArrRight(double *dest, double *source, int lenght, int slide)
{
	for(int i=0;i<lenght-slide;i++)
	{
		*(dest+i+slide) = *(source+i);
	}

}

int DSP_Filter::CreateWF50Noht(int fsmpl,int f_kernel)
{
	int M = f_kernel;
	double k=500/fsmpl;
	double sum = 0;

	//calculate band noht 50Hz first <45Hz
	double FC = 0.09*k;
	for(int i=0;i<M;i++)
	{
		if((i-M/2)==0)
			B1[i] = 2*PI*FC;
		if(0>(i-M/2)|0<(i-M/2))
			B1[i] = sin(2*PI*FC*(i-M/2))/(i-M/2);
		B1[i] = B1[i]*(0.54-0.46*cos(2*PI*i/M));
	}
	//Calculate unity gain at dc
	sum = 0;
	for(i=0;i<M;i++)
	{
		sum = sum+B1[i];
	}
	//Normalize for unity gain
	for(i=0;i<M;i++)
	{
		B1[i] = B1[i]/sum;
	}
	//Calculate >55Hz
	FC = 0.11*k;
	for(i=0;i<M;i++)
	{
		if((i-M/2)==0)
			B2[i] = 2*PI*FC;
		if(0>(i-M/2)|0<(i-M/2))
			B2[i] = sin(2*PI*FC*(i-M/2))/(i-M/2);
		B2[i] = B2[i]*(0.54-0.46*cos(2*PI*i/M));
	}
	sum = 0;
	for(i=0;i<M;i++)
	{
		sum = sum+B2[i];
	}
	for(i=0;i<M;i++)
	{
		B2[i] = B2[i]/sum;
	}
	for(i=0;i<M;i++)
	{
		B2[i] = -B2[i];
	}
	B2[M/2]=B2[M/2]+1;

	//Calculate summ kernel
	for(i=0;i<M;i++)
	{
		B1[i] = B1[i]+B2[i];
	}

	return M;
}

int DSP_Filter::CreateWFHigh(int fsmpl, int f_kernel)
{
	int M = f_kernel;
	double k=500/fsmpl;
	double FC = 0.00134*k;
	//Filter > 0.67Hz
	for(int i=0;i<M;i++)
	{
		if((i-M/2)==0)
			H[i] = 2*PI*FC;
		if(0>(i-M/2)|0<(i-M/2))
			H[i] = sin(2*PI*FC*(i-M/2))/(i-M/2);
		H[i] = H[i]*(0.54-0.46*cos(2*PI*i/M));
	}
	double sum=0;
	for(i=0;i<M;i++)
	{
		sum = sum+H[i];
	}
	for(i=0;i<M;i++)
	{
		H[i] = H[i]/sum;
	}
	for(i=0;i<M;i++)
	{
		H[i] = -H[i];
	}
	H[M/2]=H[M/2]+1;
	return M;
}

int DSP_Filter::CreateWFLow(int fsmpl, int f_kernel)
{
	int M = f_kernel;
	double k=500/fsmpl;
	//Filter < 100Hz
	double FC = 0.2*k;
	for(int i=0;i<M;i++)
	{
		if((i-M/2)==0)
			L[i] = 2*PI*FC;
		if(0>(i-M/2)|0<(i-M/2))
			L[i] = sin(2*PI*FC*(i-M/2))/(i-M/2);
		L[i] = L[i]*(0.54-0.46*cos(2*PI*i/M));
	}
	double sum=0;
	for(i=0;i<M;i++)
	{
		sum = sum+L[i];
	}
	for(i=0;i<M;i++)
	{
		L[i] = L[i]/sum;
	}
	return M;
}

void DSP_Filter::LoadFilterKernels(int fsmpl)
{
	if(fsmpl>500)
		MessageBox(NULL,"Sampling rate<500Hz","Stop",MB_OK);
	CreateWFLow(fsmpl,100);
	CreateWFHigh(fsmpl,200);
	CreateWF50Noht(fsmpl,100);
	CreateWF40Low(fsmpl,100);
}

void DSP_Filter::WFilter_Low100Hz(double *dest, double *source, int lenght, int fsmpl, double cf)
{
	//Win=-sync filter hight < 100hz
	int lenght_1 = lenght+200;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,dest,lenght);
	int M = 100;

	SlideArrRight(Y,X,lenght_1,M/2);
	MoveArr(X,Y,lenght_1);
	for(int i=0;i<lenght_1;i++)
	{
		*(Y+i)=0;
	}
	for(int j=M;j<lenght_1-M;j++)
	{
		for(i=0;i<M;i++)
		{
			*(Y+j)=*(Y+j)+*(X+j-i)*L[i];
		//	*(Y+j) = *(Y+j)+*(X+j+i)*H[i];
		}
	}
	SlideArrLeft(Y,Y,lenght_1,M);//
//	FilterLowFromTo(Y,Y,0,M,lenght);
	MoveArr(dest,Y,lenght);
	//Free memory
	Delete_Arrs();

}

void DSP_Filter::WFilter_Noht(double *dest, double *source, int lenght, int fsmpl, double cf)
{
	//Win=-sync filter hight < 47hz > 53hz
	int lenght_1 = lenght+200;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,dest,lenght);
	int M = 100;

	SlideArrRight(Y,X,lenght_1,M/2);
	MoveArr(X,Y,lenght_1);
	for(int i=0;i<lenght_1;i++)
	{
		*(Y+i)=0;
	}
	for(int j=M;j<lenght_1-M;j++)
	{
		for(i=0;i<M;i++)
		{
			*(Y+j)=*(Y+j)+*(X+j-i)*B1[i];
		//	*(Y+j) = *(Y+j)+*(X+j+i)*H[i];
		}
	}
	SlideArrLeft(Y,Y,lenght_1,M);//
//	FilterLowFromTo(Y,Y,0,M,lenght);
	MoveArr(dest,Y,lenght);
	//Free memory
	Delete_Arrs();
}

int DSP_Filter::CreateWF40Low(int fsmpl, int f_kernel)
{
	int M = f_kernel;
	double k=500/fsmpl;
	double sum = 0;

	//calculate band <41Hz
	double FC = 0.082*k;
	for(int i=0;i<M;i++)
	{
		if((i-M/2)==0)
			L40HZ[i] = 2*PI*FC;
		if(0>(i-M/2)|0<(i-M/2))
			L40HZ[i] = sin(2*PI*FC*(i-M/2))/(i-M/2);
		L40HZ[i] = L40HZ[i]*(0.54-0.46*cos(2*PI*i/M));
	}
	//Calculate unity gain at dc
	sum = 0;
	for(i=0;i<M;i++)
	{
		sum = sum+L40HZ[i];
	}
	//Normalize for unity gain
	for(i=0;i<M;i++)
	{
		L40HZ[i] = L40HZ[i]/sum;
	}

	return M;
}

void DSP_Filter::WFilter_Low40Hz(double *dest, double *source, int lenght, int fsmpl, double cf)
{
	int lenght_1 = lenght+200;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,dest,lenght);
	int M = 100;

	SlideArrRight(Y,X,lenght_1,M/2);
	MoveArr(X,Y,lenght_1);
	for(int i=0;i<lenght_1;i++)
	{
		*(Y+i)=0;
	}
	for(int j=M;j<lenght_1-M;j++)
	{
		for(i=0;i<M;i++)
		{
			*(Y+j)=*(Y+j)+*(X+j-i)*L40HZ[i];
		}
	}
	SlideArrLeft(Y,Y,lenght_1,M);//
	MoveArr(dest,Y,lenght);
	//Free memory
	Delete_Arrs();
}

double DSP_Filter::FindMax(double *source, int lenght)
{
	//Finding the max value from source array
	double max=0;
	max = *(source);
	for(int i=0;i<lenght;i++)
	{
		if(max<*(source+i))
			max = *(source+i);
	}
	return max;
}

double DSP_Filter::FindMin(double *source, int lenght)
{
	//Finding min value from source array
	double min=0;
	min = *(source);
	for(int i=0;i<lenght;i++)
	{
		if(min>*(source+i))
			min = *(source+i);
	}
	return min;

}

void DSP_Filter::Amplifier(double *dest, double *source, int lenght,double ampl)
{
	int lenght_1 = lenght+200;
	Greate_Arrs(lenght_1);
	ZeroArrs(lenght_1);
	MoveArr(X,source,lenght);
	for(int i=0;i<lenght;i++)
	{
		*(Y+i) = *(X+i)*ampl;
	}
	
	MoveArr(dest,Y,lenght);
	//Free memory
	Delete_Arrs();
}