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

Merged LSM303D lowpass

This commit is contained in:
Andrew Tridgell 2013-08-04 15:10:42 +10:00 committed by Lorenz Meier
parent 686edfefb8
commit 26204496b6
1 changed files with 32 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -64,6 +64,7 @@
#include <drivers/drv_mag.h>
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
/* 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 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:
unsigned bandwidth;
if (OK == get_antialias_filter_bandwidth(bandwidth))
return bandwidth;
else
return -EINVAL;
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);
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_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;
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;