giuliomoro@233: /*
giuliomoro@233:  * IirFilter.h
giuliomoro@233:  *
giuliomoro@233:  *  Created on: 17 Sep 2015
giuliomoro@233:  *      Author: giulio
giuliomoro@233:  */
giuliomoro@233: 
giuliomoro@233: #ifndef IIRFILTER_H_
giuliomoro@233: #define IIRFILTER_H_
giuliomoro@233: 
giuliomoro@233: #define IIR_FILTER_STAGE_COEFFICIENTS (5)
giuliomoro@233: #define IIR_FILTER_STAGE_STATES (IIR_FILTER_STAGE_COEFFICIENTS - 1)
giuliomoro@233: 
giuliomoro@233: class IirFilterStage{ //TODO : to save (some) memory we should only store the coefficients pointers here,
giuliomoro@233: 						//so that IirFilter can share them among multiple stages if needbe)
giuliomoro@233: private:
giuliomoro@233: 	double coefficients[IIR_FILTER_STAGE_COEFFICIENTS]; // these are b0,b1,b2,a1,a2
giuliomoro@233: 	double states[IIR_FILTER_STAGE_STATES]; // these are xprev, xprevprev, yprev, yprevprev
giuliomoro@233: public:
giuliomoro@233: 	IirFilterStage();
giuliomoro@233: 	void setCoefficients(double* newCoefficients);
giuliomoro@233: 	void setStates(double* newStates);
giuliomoro@233: 	// this is not meant to be efficient, just of practical use!
giuliomoro@233: 	double process(double in){
giuliomoro@233: 		process(&in, 1);
giuliomoro@233: 		return in;
giuliomoro@233: 	}
giuliomoro@233: 
giuliomoro@233: 	void process(double* inout, int length){
giuliomoro@233: 		// make variables explicit. A smart compiler will optimize these out, right?
giuliomoro@233: 		double b0=coefficients[0];
giuliomoro@233: 		double b1=coefficients[1];
giuliomoro@233: 		double b2=coefficients[2];
giuliomoro@233: 		double a1=coefficients[3];
giuliomoro@233: 		double a2=coefficients[4];
giuliomoro@233: 		double x1 = states[0];
giuliomoro@233: 		double x2=  states[1];
giuliomoro@233: 		double y1 = states[2];
giuliomoro@233: 		double y2 = states[3];
giuliomoro@233: 		for(int n = 0; n < length; n++){
giuliomoro@233: 			double x0 = inout[n];
giuliomoro@233: 			double y = x0 * b0 + x1 * b1 + x2 * b2 +
giuliomoro@233: 					- y1 * a1 - y2 * a2;
giuliomoro@233: 			inout[n] = y;
giuliomoro@233: 			x2 = x1;
giuliomoro@233: 			x1 = x0;
giuliomoro@233: 			y2 = y1;
giuliomoro@233: 			y1 = y;
giuliomoro@233: 		}
giuliomoro@233: 		states[0] = x1;
giuliomoro@233: 		states[1] = x2;
giuliomoro@233: 		states[2] = y1;
giuliomoro@233: 		states[3] = y2;
giuliomoro@233: 	}
giuliomoro@233: };
giuliomoro@233: 
giuliomoro@233: class IirFilter{
giuliomoro@233: private:
giuliomoro@233: 	struct IirFilterStage** stages;
giuliomoro@233: 	int numberOfStages;
giuliomoro@233: 	void dealloc();
giuliomoro@233: public:
giuliomoro@233: 	IirFilter();
giuliomoro@233: 	IirFilter(int newNumberOfStages);
giuliomoro@233: 	IirFilter(int newNumberOfStages, double *newCoefficients);
giuliomoro@233: 	void setCoefficients(double* newCoefficients);
giuliomoro@233: 	void setStates(double* newStates);
giuliomoro@233: 	void setNumberOfStages(int newNumberOfStages);
giuliomoro@233: //	double process(double in);
giuliomoro@233: //	inline void process(double* inout, int length);
giuliomoro@233: //	inline void process(double* in, double* out, int length);
giuliomoro@233: 	double process(double in){
giuliomoro@233: 		process(&in, 1);
giuliomoro@233: 		return in;
giuliomoro@233: 	};
giuliomoro@233: 	void process(double* inout, int length){
giuliomoro@233: 		for(int n = 0; n < numberOfStages && n < 8/* TODO: this "8" compensates for a memory corruption somewhere on the BBB*/; n++){
giuliomoro@233: 			stages[n]->process(inout, length);
giuliomoro@233: 		}
giuliomoro@233: 	}
giuliomoro@233: };
giuliomoro@233: 
giuliomoro@233: 
giuliomoro@233: //void IirFilter::process(double* in, double* out, int length);
giuliomoro@233: 
giuliomoro@233: #endif /* IIRFILTER_H_ */