From 26204496b69740bddfd6f8ddbdd71ee4e755008c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:10:42 +1000 Subject: Merged LSM303D lowpass --- src/drivers/lsm303d/lsm303d.cpp | 55 ++++++++++++++++++++++++----------------- 1 file changed, 33 insertions(+), 22 deletions(-) (limited to 'src/drivers/lsm303d/lsm303d.cpp') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 9ebffcac9..56400c843 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -64,6 +64,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -232,6 +233,10 @@ private: perf_counter_t _accel_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. */ @@ -402,7 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_range_ga(0.0f), _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 _debug_enabled = true; @@ -446,7 +454,6 @@ LSM303D::init() { int ret = ERROR; int mag_ret; - int fd_mag; /* do SPI init (and probe) first */ if (SPI::init() != OK) @@ -622,7 +629,6 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: @@ -645,6 +651,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust sample rate of sensor */ 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 */ /* XXX this is a bit shady, but no other way to adjust... */ _accel_call.period = _call_accel_interval = ticks; @@ -695,15 +708,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX implement */ return -EINVAL; -// case ACCELIOCSLOWPASS: - case ACCELIOCGLOWPASS: + 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; + } - unsigned bandwidth; - - if (OK == get_antialias_filter_bandwidth(bandwidth)) - return bandwidth; - else - return -EINVAL; + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSSCALE: { @@ -1186,17 +1201,13 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; 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 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->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 = _accel_filter_x.apply(x_in_new); + accel_report->y = _accel_filter_y.apply(y_in_new); + accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; -- cgit v1.2.3