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

Disable IIR filter for now

This commit is contained in:
Julian Oes 2013-07-18 19:45:44 +02:00
parent da152e148d
commit e309b9ab4a
1 changed files with 28 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -221,9 +221,9 @@ private:
unsigned _current_samplerate; unsigned _current_samplerate;
FIL_T _filter_x; // FIL_T _filter_x;
FIL_T _filter_y; // FIL_T _filter_y;
FIL_T _filter_z; // FIL_T _filter_z;
unsigned _num_mag_reports; unsigned _num_mag_reports;
volatile unsigned _next_mag_report; volatile unsigned _next_mag_report;
@ -495,20 +495,20 @@ LSM303D::init()
set_samplerate(400); /* max sample rate */ set_samplerate(400); /* max sample rate */
/* initiate filter */ /* initiate filter */
TF_DIF_t tf_filter; // TF_DIF_t tf_filter;
tf_filter.numInt = 0; // tf_filter.numInt = 0;
tf_filter.numDiff = 0; // tf_filter.numDiff = 0;
tf_filter.lowpassLength = 2; // tf_filter.lowpassLength = 2;
tf_filter.highpassLength = 0; // tf_filter.highpassLength = 0;
tf_filter.lowpassData[0] = 10; // tf_filter.lowpassData[0] = 10;
tf_filter.lowpassData[1] = 10; // tf_filter.lowpassData[1] = 10;
//tf_filter.highpassData[0] = ; // //tf_filter.highpassData[0] = ;
tf_filter.gain = 1; // tf_filter.gain = 1;
tf_filter.Ts = 1/_current_samplerate; // 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_x);
initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); // initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y);
initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); // initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z);
mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
mag_set_samplerate(100); mag_set_samplerate(100);
@ -1182,13 +1182,17 @@ 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 = updateFilter(&_filter_x, x_in_new); accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
accel_report->y = updateFilter(&_filter_y, y_in_new); accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
accel_report->z = updateFilter(&_filter_z, z_in_new); accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
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;