forked from Archive/PX4-Autopilot
Added iirFilter to LSM303D
This commit is contained in:
parent
f4df4a4e08
commit
da152e148d
|
@ -18,7 +18,6 @@ MODULES += drivers/led
|
|||
MODULES += drivers/px4io
|
||||
MODULES += drivers/px4fmu
|
||||
MODULES += drivers/boards/px4fmu
|
||||
MODULES += drivers/lsm303d
|
||||
MODULES += drivers/ardrone_interface
|
||||
MODULES += drivers/l3gd20
|
||||
MODULES += drivers/bma180
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
|
@ -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
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -3,4 +3,5 @@
|
|||
#
|
||||
|
||||
MODULE_COMMAND = lsm303d
|
||||
SRCS = lsm303d.cpp
|
||||
SRCS = lsm303d.cpp \
|
||||
iirFilter.c
|
||||
|
|
Loading…
Reference in New Issue