dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Removed LSM303D filter

This commit is contained in:
Andrew Tridgell 2013-08-04 13:46:30 +10:00 committed by Lorenz Meier
parent a87690d0e2
commit 686edfefb8
4 changed files with 3 additions and 338 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

255
src/drivers/lsm303d/iirFilter.c
View File

@ -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;
}
}
}

59
src/drivers/lsm303d/iirFilter.h
View File

@ -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

22
src/drivers/lsm303d/lsm303d.cpp
View File

@ -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);

5
src/drivers/lsm303d/module.mk
View File

@ -3,5 +3,6 @@
#
MODULE_COMMAND = lsm303d
SRCS = lsm303d.cpp \
iirFilter.c
SRCS = lsm303d.cpp