changeset 148:6cd38e261027 ClockSync

Simple IirFilter class

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/core/IirFilter.cpp	Mon Sep 21 03:11:32 2015 +0100
@@ -0,0 +1,110 @@
+/*
+ * IirFilter.cpp
+ *
+ *  Created on: 17 Sep 2015
+ *      Author: giulio
+ */
+#include "IirFilter.h"
+
+IirFilterStage::IirFilterStage(){
+	for(int n = 0; n < IIR_FILTER_STAGE_COEFFICIENTS; n++){
+		coefficients[n] = 0;
+	}
+	for(int n = 0; n < IIR_FILTER_STAGE_STATES; n++){
+		states[n]=0;
+	}
+}
+void IirFilterStage::setCoefficients(double* newCoefficients){
+	for(int n = 0; n < IIR_FILTER_STAGE_COEFFICIENTS; n++){
+		coefficients[n] = newCoefficients[n];
+	}
+}
+void IirFilterStage::setStates(double* newStates){
+	for(int n = 0; n < IIR_FILTER_STAGE_STATES; n++){
+		states[n] = newStates[n];
+	}
+}
+// this is not meant to be efficient, just of practical use!
+double IirFilterStage::process(double in){
+	process(&in, 1);
+	return in;
+}
+
+void IirFilterStage::process(double* inout, int length){
+	// make variables explicit. A smart compiler will optimize these out, right?
+	double b0=coefficients[0];
+	double b1=coefficients[1];
+	double b2=coefficients[2];
+	double a1=coefficients[3];
+	double a2=coefficients[4];
+	double x1 = states[0];
+	double x2=  states[1];
+	double y1 = states[2];
+	double y2 = states[3];
+	for(int n = 0; n < length; n++){
+		double x0 = inout[n];
+		double y = x0 * b0 + x1 * b1 + x2 * b2 +
+				- y1 * a1 - y2 * a2;
+		inout[n] = y;
+		x2 = x1;
+		x1 = x0;
+		y2 = y1;
+		y1 = y;
+	}
+	states[0] = x1;
+	states[1] = x2;
+	states[2] = y1;
+	states[3] = y2;
+}
+/*	struct IirFilterStage* stages;
+	int numberOfStages;
+*/
+IirFilter::IirFilter(){
+	stages=(IirFilterStage**)0;
+	setNumberOfStages(0);
+}
+IirFilter::IirFilter(int newNumberOfStages){
+	setNumberOfStages(newNumberOfStages);
+}
+IirFilter::IirFilter(int newNumberOfStages, double* newCoefficients){
+	setNumberOfStages(newNumberOfStages);
+	setCoefficients(newCoefficients);
+}
+void IirFilter::dealloc(){
+	if( stages == 0 )
+		return;
+	for(int n = 0; n < numberOfStages; n++){
+		delete stages[n];
+	}
+	delete stages;
+	stages = 0;
+	numberOfStages = 0;
+}
+void IirFilter::setCoefficients(double* newCoefficients){
+	for(int n = 0; n < numberOfStages; n++){
+		stages[n]->setCoefficients(newCoefficients);
+	}
+};
+void IirFilter::setStates(double* newStates){
+	for(int n = 0; n < numberOfStages; n++){
+			stages[n]->setStates(newStates);
+	}
+};
+void IirFilter::setNumberOfStages(int newNumberOfStages){
+	dealloc();
+	numberOfStages=newNumberOfStages;
+	stages = new IirFilterStage*[numberOfStages];
+	for( int n = 0; n < numberOfStages; n++){
+		stages[n] = new IirFilterStage;
+	}
+}
+double IirFilter::process(double in){
+	process(&in, 1);
+	return in;
+};
+void IirFilter::process(double* inout, int length){
+	for(int n = 0; n < numberOfStages && n < 8/* TODO: this "8" compensates for a memory corruption somewhere on the BBB*/; n++){
+		stages[n]->process(inout, length);
+	}
+}
+//void IirFilter::process(double* in, double* out, int length);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/IirFilter.h	Mon Sep 21 03:11:32 2015 +0100
@@ -0,0 +1,45 @@
+/*
+ * IirFilter.h
+ *
+ *  Created on: 17 Sep 2015
+ *      Author: giulio
+ */
+
+#ifndef IIRFILTER_H_
+#define IIRFILTER_H_
+
+#define IIR_FILTER_STAGE_COEFFICIENTS (5)
+#define IIR_FILTER_STAGE_STATES (IIR_FILTER_STAGE_COEFFICIENTS - 1)
+
+class IirFilterStage{ //TODO : to save (some) memory we should only store the coefficients pointers here,
+						//so that IirFilter can share them among multiple stages if needbe)
+private:
+	double coefficients[IIR_FILTER_STAGE_COEFFICIENTS]; // these are b0,b1,b2,a1,a2
+	double states[IIR_FILTER_STAGE_STATES]; // these are xprev, xprevprev, yprev, yprevprev
+public:
+	IirFilterStage();
+	void setCoefficients(double* newCoefficients);
+	void setStates(double* newStates);
+	double process(double in);
+	void process(double* inout, int length);
+	void process(double* in, double* out, int length);
+};
+
+class IirFilter{
+private:
+	struct IirFilterStage** stages;
+	int numberOfStages;
+	void dealloc();
+public:
+	IirFilter();
+	IirFilter(int newNumberOfStages);
+	IirFilter(int newNumberOfStages, double *newCoefficients);
+	void setCoefficients(double* newCoefficients);
+	void setStates(double* newStates);
+	void setNumberOfStages(int newNumberOfStages);
+	double process(double in);
+	void process(double* inout, int length);
+	void process(double* in, double* out, int length);
+};
+
+#endif /* IIRFILTER_H_ */