Removed LSM303D filter

4 files changed, 3 insertions, 338 deletions

diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c
deleted file mode 100644
index 8311f14d6..000000000
--- a/src/drivers/lsm303d/iirFilter.c
+++ /dev/null
@@ -1,255 +0,0 @@
-#include "math.h"
-#include "string.h"
-#include "iirFilter.h"
-
-///////////////////////////////////////////////////////////////////////////////
-// Internal function prototypes
-
-int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd);
-
-int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd);
-
-int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt);
-
-int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly);
-
-///////////////////////////////////////////////////////////////////////////////
-// external functions
-
-int testFunction()
-{
-	printf("TEST\n");
-	return 1;
-}
-
-int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt)
-{
-	TF_POLY_t polyd;
-	TF_ZPG_t zpgd;
-
-	memset(pFilt, 0, sizeof(FIL_T));
-
-	// perform bilinear transform with frequency pre warping
-	btDifcToZpgd(pDifc, Ts, &zpgd);
-
-	// calculate polynom
-	tZpgxToPolyx(&zpgd, &polyd);
-
-	// set the filter parameters
-	tPolydToFil(&polyd, pFilt);
-
-	return 1;
-}
-
-// run filter using inp, return output of the filter
-float updateFilter(FIL_T *pFilt, float inp)
-{
-	float outp = 0;
-	int idx; // index used for different things
-	int i; // loop variable
-
-	// Store the input to the input array
-	idx = pFilt->inpCnt % MAX_LENGTH;
-	pFilt->inpData[idx] = inp;
-
-	// calculate numData * inpData
-	for (i = 0; i < pFilt->numLength; i++)
-	{
-		// index of inp array
-		idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH;
-		outp += pFilt->numData[i] * pFilt->inpData[idx];
-	}
-
-	// calculate denData * outData
-	for (i = 0; i < pFilt->denLength; i++)
-	{
-		// index of inp array
-		idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH;
-		outp -= pFilt->denData[i] * pFilt->outData[idx];
-	}
-
-	// store the ouput data to the output array
-	idx = pFilt->inpCnt % MAX_LENGTH;
-	pFilt->outData[idx] = outp;
-
-	pFilt->inpCnt += 1;
-
-	return outp;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Internal functions
-
-int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt)
-{
-	double gain;
-	int i;
-
-	if (pkPolyd->Ts < 0)
-	{
-		return 0;
-	}
-
-	// initialize filter to 0
-	memset(pFilt, 0, sizeof(FIL_T));
-
-	gain = pkPolyd->denData[pkPolyd->denLength - 1];
-	pFilt->Ts = pkPolyd->Ts;
-
-	pFilt->denLength = pkPolyd->denLength - 1;
-	pFilt->numLength = pkPolyd->numLength;
-
-	for (i = 0; i < pkPolyd->numLength; i++)
-	{
-		pFilt->numData[i] = pkPolyd->numData[i] / gain;
-	}
-
-	for (i = 0; i < (pkPolyd->denLength - 1); i++)
-	{
-		pFilt->denData[i] = pkPolyd->denData[i] / gain;
-	}
-}
-
-// bilinear transformation of poles and zeros
-int btDifcToZpgd(const TF_DIF_t *pkDifc,
-				 double Ts,
-				 TF_ZPG_t *pZpgd)
-{
-	TF_ZPG_t zpgc;
-	int totDiff;
-	int i;
-
-	zpgc.zerosLength = 0;
-	zpgc.polesLength = 0;
-	zpgc.gain = pkDifc->gain;
-	zpgc.Ts = pkDifc->Ts;
-
-	// Total number of differentiators / integerators
-	// positive diff, negative integ, 0 for no int/diff
-	totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength;
-
-	while (zpgc.zerosLength < totDiff)
-	{
-		zpgc.zerosData[zpgc.zerosLength] = 0;
-		zpgc.zerosLength++;
-	}
-	while (zpgc.polesLength < -totDiff)
-	{
-		zpgc.polesData[zpgc.polesLength] = 0;
-		zpgc.polesLength++;
-	}
-
-	// The next step is to calculate the poles
-	// This has to be done for the highpass and lowpass filters
-	// As we are using bilinear transformation there will be frequency
-	// warping which has to be accounted for
-	for (i = 0; i < pkDifc->lowpassLength; i++)
-	{
-		// pre-warping:
-		double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0);
-		// calculate pole:
-		zpgc.polesData[zpgc.polesLength] = -freq;
-		// adjust gain such that lp has gain = 1 for low frequencies
-		zpgc.gain *= freq;
-		zpgc.polesLength++;
-	}
-	for (i = 0; i < pkDifc->highpassLength; i++)
-	{
-		// pre-warping
-		double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0);
-		// calculate pole:
-		zpgc.polesData[zpgc.polesLength] = -freq;
-		// gain does not have to be adjusted
-		zpgc.polesLength++;
-	}
-
-	return btZpgcToZpgd(&zpgc, Ts, pZpgd);
-}
-
-// bilinear transformation of poles and zeros
-int btZpgcToZpgd(const TF_ZPG_t *pkZpgc,
-				 double Ts,
-				 TF_ZPG_t *pZpgd)
-{
-	int i;
-
-	// init digital gain
-	pZpgd->gain = pkZpgc->gain;
-
-	// transform the poles
-	pZpgd->polesLength = pkZpgc->polesLength;
-	for (i = 0; i < pkZpgc->polesLength; i++)
-	{
-		pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]);
-		pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]);
-	}
-
-	// transform the zeros
-	pZpgd->zerosLength = pkZpgc->zerosLength;
-	for (i = 0; i < pkZpgc->zerosLength; i++)
-	{
-		pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]);
-		pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]);
-	}
-
-	// if we don't have the same number of poles as zeros we have to add
-	// poles or zeros due to the bilinear transformation
-	while (pZpgd->zerosLength < pZpgd->polesLength)
-	{
-		pZpgd->zerosData[pZpgd->zerosLength] = -1.0;
-		pZpgd->zerosLength++;
-	}
-	while (pZpgd->zerosLength > pZpgd->polesLength)
-	{
-		pZpgd->polesData[pZpgd->polesLength] = -1.0;
-		pZpgd->polesLength++;
-	}
-
-	pZpgd->Ts = Ts;
-
-	return 1;
-}
-
-// calculate polynom from zero, pole, gain form
-int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly)
-{
-	int i, j; // counter variable
-	double tmp0, tmp1, gain;
-
-	// copy Ts
-	pPoly->Ts = pkZpg->Ts;
-
-	// calculate denominator polynom
-	pPoly->denLength = 1;
-	pPoly->denData[0] = 1.0;
-	for (i = 0; i < pkZpg->polesLength; i++)
-	{
-		// init temporary variable
-		tmp0 = 0.0;
-		// increase the polynom by 1
-		pPoly->denData[pPoly->denLength] = 0;
-		pPoly->denLength++;
-		for (j = 0; j < pPoly->denLength; j++)
-		{
-			tmp1 = pPoly->denData[j];
-			pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0;
-			tmp0 = tmp1;
-		}
-	}
-
-	// calculate numerator polynom
-	pPoly->numLength = 1;
-	pPoly->numData[0] = pkZpg->gain;
-	for (i = 0; i < pkZpg->zerosLength; i++)
-	{
-		tmp0 = 0.0;
-		pPoly->numData[pPoly->numLength] = 0;
-		pPoly->numLength++;
-		for (j = 0; j < pPoly->numLength; j++)
-		{
-			tmp1 = pPoly->numData[j];
-			pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0;
-			tmp0 = tmp1;
-		}
-	}
-}
diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h
deleted file mode 100644
index ab4223a8e..000000000
--- a/src/drivers/lsm303d/iirFilter.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#ifndef IIRFILTER__H__
-#define IIRFILTER__H__
-
-__BEGIN_DECLS
-
-#define MAX_LENGTH 4
-
-typedef struct FILTER_s
-{
-	float denData[MAX_LENGTH];
-	float numData[MAX_LENGTH];
-	int denLength;
-	int numLength;
-	float Ts;
-	float inpData[MAX_LENGTH];
-	float outData[MAX_LENGTH];
-	unsigned int inpCnt;
-} FIL_T;
-
-typedef struct TF_ZPG_s
-{
-	int    zerosLength;
-	double zerosData[MAX_LENGTH + 1];
-	int    polesLength;
-	double polesData[MAX_LENGTH + 1];
-	double gain;
-	double Ts;
-} TF_ZPG_t;
-
-typedef struct TF_POLY_s
-{
-	int    numLength;
-	double numData[MAX_LENGTH];
-	int    denLength;
-	double denData[MAX_LENGTH];
-	double Ts;
-} TF_POLY_t;
-
-typedef struct TF_DIF_s
-{
-	int    numInt;
-	int    numDiff;
-	int    lowpassLength;
-	int    highpassLength;
-	double lowpassData[MAX_LENGTH];
-	int    highpassData[MAX_LENGTH];
-	double gain;
-	double Ts;
-} TF_DIF_t;
-
-__EXPORT int testFunction(void);
-
-__EXPORT float updateFilter(FIL_T *pFilt, float inp);
-
-__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt);
-
-__END_DECLS
-
-#endif
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 7a65f644a..9ebffcac9 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -65,8 +65,6 @@
 
 #include <board_config.h>
 
-#include "iirFilter.h"
-
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -221,10 +219,6 @@ private:
 
 	unsigned			_current_samplerate;
 
-//	FIL_T				_filter_x;
-//	FIL_T				_filter_y;
-//	FIL_T				_filter_z;
-
 	unsigned			_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
@@ -494,22 +488,6 @@ LSM303D::init()
 	set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
 	set_samplerate(400); /* max sample rate */
 
-	/* initiate filter */
-//	TF_DIF_t tf_filter;
-//	tf_filter.numInt = 0;
-//	tf_filter.numDiff = 0;
-//	tf_filter.lowpassLength = 2;
-//	tf_filter.highpassLength = 0;
-//	tf_filter.lowpassData[0] = 10;
-//	tf_filter.lowpassData[1] = 10;
-//	//tf_filter.highpassData[0] = ;
-//	tf_filter.gain = 1;
-//	tf_filter.Ts = 1/_current_samplerate;
-//
-//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x);
-//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y);
-//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z);
-
 	mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
 	mag_set_samplerate(100);
 
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index 8fd5679c9..e40f718c5 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -3,5 +3,6 @@
 #
 
 MODULE_COMMAND	 = lsm303d
-SRCS		 = lsm303d.cpp \
-		iirFilter.c
+SRCS		 = lsm303d.cpp
+
+