diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/lsm303d/iirFilter.c | 255 | ||||
-rw-r--r-- | src/drivers/lsm303d/iirFilter.h | 59 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 22 | ||||
-rw-r--r-- | src/drivers/lsm303d/module.mk | 5 |
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 + + |