Added iirFilter to LSM303D

4 files changed, 345 insertions, 4 deletions

diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c
new file mode 100644
index 000000000..8311f14d6
--- /dev/null
+++ b/src/drivers/lsm303d/iirFilter.c
@@ -0,0 +1,255 @@
+#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
new file mode 100644
index 000000000..ab4223a8e
--- /dev/null
+++ b/src/drivers/lsm303d/iirFilter.h
@@ -0,0 +1,59 @@
+#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 b157d74c6..80d777826 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -65,6 +65,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 
+#include "iirFilter.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -220,6 +221,10 @@ 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;
@@ -489,6 +494,22 @@ 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);
 
@@ -1161,9 +1182,14 @@ LSM303D::measure()
 	accel_report->y_raw = raw_accel_report.y;
 	accel_report->z_raw = raw_accel_report.z;
 
-	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	
+	accel_report->x = updateFilter(&_filter_x, x_in_new);
+	accel_report->y = updateFilter(&_filter_y, y_in_new);
+	accel_report->z = updateFilter(&_filter_z, z_in_new);
+
 	accel_report->scaling = _accel_range_scale;
 	accel_report->range_m_s2 = _accel_range_m_s2;
 
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index e93b1e331..8fd5679c9 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -3,4 +3,5 @@
 #
 
 MODULE_COMMAND	 = lsm303d
-SRCS		 = lsm303d.cpp
+SRCS		 = lsm303d.cpp \
+		iirFilter.c