diff options
author | Julian Oes <julian@oes.ch> | 2013-07-18 16:15:43 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-07-18 16:15:43 +0200 |
commit | da152e148d471ded29857e29040f33c7356c9050 (patch) | |
tree | 543c579c679853134de09667241a78d93ed3d054 /src/drivers/lsm303d/lsm303d.cpp | |
parent | f4df4a4e081d9eaaa5dbeef013fa6320b0cea3f7 (diff) | |
download | px4-firmware-da152e148d471ded29857e29040f33c7356c9050.tar.gz px4-firmware-da152e148d471ded29857e29040f33c7356c9050.tar.bz2 px4-firmware-da152e148d471ded29857e29040f33c7356c9050.zip |
Added iirFilter to LSM303D
Diffstat (limited to 'src/drivers/lsm303d/lsm303d.cpp')
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 32 |
1 files changed, 29 insertions, 3 deletions
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; |