fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 18:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged LSM303D lowpass

Browse Source
This commit is contained in:
Andrew Tridgell
2013-08-04 15:49:34 +02:00
committed by Lorenz Meier
parent 686edfefb8
commit 26204496b6
1 changed files with 32 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/lsm303d/lsm303d.cpp
+32 -21
View File
@@ -64,6 +64,7 @@
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <board_config.h> #include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
#ifdef ERROR #ifdef ERROR
@@ -232,6 +233,10 @@ private:
perf_counter_t _accel_sample_perf; perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf; perf_counter_t _mag_sample_perf;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
/** /**
* Start automatic measurement. * Start automatic measurement.
*/ */
@@ -402,7 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_range_scale(0.0f), _mag_range_scale(0.0f),
_mag_range_ga(0.0f), _mag_range_ga(0.0f),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_accel_filter_x(800, 30),
_accel_filter_y(800, 30),
_accel_filter_z(800, 30)
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@@ -446,7 +454,6 @@ LSM303D::init()
{ {
int ret = ERROR; int ret = ERROR;
int mag_ret; int mag_ret;
int fd_mag;
/* do SPI init (and probe) first */ /* do SPI init (and probe) first */
if (SPI::init() != OK) if (SPI::init() != OK)
@@ -622,7 +629,6 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 1600); return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
@@ -645,6 +651,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
/* adjust sample rate of sensor */ /* adjust sample rate of sensor */
set_samplerate(arg); set_samplerate(arg);
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f/ticks;
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
/* update interval for next measurement */ /* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */ /* XXX this is a bit shady, but no other way to adjust... */
_accel_call.period = _call_accel_interval = ticks; _accel_call.period = _call_accel_interval = ticks;
@@ -695,15 +708,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
/* XXX implement */ /* XXX implement */
return -EINVAL; return -EINVAL;
// case ACCELIOCSLOWPASS: case ACCELIOCSLOWPASS: {
float cutoff_freq_hz = arg;
float sample_rate = 1.0e6f / _call_accel_interval;
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
return OK;
}
case ACCELIOCGLOWPASS: case ACCELIOCGLOWPASS:
return _accel_filter_x.get_cutoff_freq();
unsigned bandwidth;
if (OK == get_antialias_filter_bandwidth(bandwidth))
return bandwidth;
else
return -EINVAL;
case ACCELIOCSSCALE: case ACCELIOCSSCALE:
{ {
@@ -1186,17 +1201,13 @@ LSM303D::measure()
accel_report->y_raw = raw_accel_report.y; accel_report->y_raw = raw_accel_report.y;
accel_report->z_raw = raw_accel_report.z; accel_report->z_raw = raw_accel_report.z;
// float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_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 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; 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->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; accel_report->x = _accel_filter_x.apply(x_in_new);
accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; accel_report->y = _accel_filter_y.apply(y_in_new);
accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; accel_report->z = _accel_filter_z.apply(z_in_new);
accel_report->scaling = _accel_range_scale; accel_report->scaling = _accel_range_scale;
accel_report->range_m_s2 = _accel_range_m_s2; accel_report->range_m_s2 = _accel_range_m_s2;