Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-27 02:04:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use LowPassFilter2pVector3f

Browse Source
This commit is contained in:
Jonathan Challinger 2015-02-17 14:48:26 -08:00 committed by Andrew Tridgell
parent 0133f0bb57
commit 502446d821
10 changed files with 47 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -67,12 +67,8 @@ AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu), AP_InertialSensor_Backend(imu),
_have_gyro_sample(false), _have_gyro_sample(false),
_have_accel_sample(false), _have_accel_sample(false),
_accel_filter_x(raw_sample_rate_hz, 10), _accel_filter(raw_sample_rate_hz, 10),
_accel_filter_y(raw_sample_rate_hz, 10), _gyro_filter(raw_sample_rate_hz, 10),
_accel_filter_z(raw_sample_rate_hz, 10),
_gyro_filter_x(raw_sample_rate_hz, 10),
_gyro_filter_y(raw_sample_rate_hz, 10),
_gyro_filter_z(raw_sample_rate_hz, 10),
_last_gyro_timestamp(0), _last_gyro_timestamp(0),
_last_accel_timestamp(0) _last_accel_timestamp(0)
{} {}
@ -167,12 +163,8 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
if (filter_hz == 0) if (filter_hz == 0)
filter_hz = _default_filter_hz; filter_hz = _default_filter_hz;
_accel_filter_x.set_cutoff_frequency(raw_sample_rate_hz, filter_hz); _accel_filter.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
_accel_filter_y.set_cutoff_frequency(raw_sample_rate_hz, filter_hz); _gyro_filter.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
_accel_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
_gyro_filter_x.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
_gyro_filter_y.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
_gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
} }
@ -239,9 +231,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
int16_t y = -((((int16_t)buffer[1]) << 8) | buffer[0]); // chip X axis int16_t y = -((((int16_t)buffer[1]) << 8) | buffer[0]); // chip X axis
int16_t x = -((((int16_t)buffer[3]) << 8) | buffer[2]); // chip Y axis int16_t x = -((((int16_t)buffer[3]) << 8) | buffer[2]); // chip Y axis
int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // chip Z axis int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // chip Z axis
_accel_filtered = Vector3f(_accel_filter_x.apply(x), _accel_filtered = _accel_filter.apply(Vector3f(x,y,z));
_accel_filter_y.apply(y),
_accel_filter_z.apply(z));
_have_accel_sample = true; _have_accel_sample = true;
_last_accel_timestamp = now; _last_accel_timestamp = now;
} }
@ -256,9 +246,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
int16_t y = -((((int16_t)buffer[0]) << 8) | buffer[1]); // chip X axis int16_t y = -((((int16_t)buffer[0]) << 8) | buffer[1]); // chip X axis
int16_t x = -((((int16_t)buffer[2]) << 8) | buffer[3]); // chip Y axis int16_t x = -((((int16_t)buffer[2]) << 8) | buffer[3]); // chip Y axis
int16_t z = -((((int16_t)buffer[4]) << 8) | buffer[5]); // chip Z axis int16_t z = -((((int16_t)buffer[4]) << 8) | buffer[5]); // chip Z axis
_gyro_filtered = Vector3f(_gyro_filter_x.apply(x), _gyro_filtered = _gyro_filter.apply(Vector3f(x,y,z));
_gyro_filter_y.apply(y),
_gyro_filter_z.apply(z));
_have_gyro_sample = true; _have_gyro_sample = true;
_last_gyro_timestamp = now; _last_gyro_timestamp = now;
} }

8
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -38,12 +38,8 @@ private:
void _set_filter_frequency(uint8_t filter_hz); void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel // Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x; LowPassFilter2pVector3f _accel_filter;
LowPassFilter2p _accel_filter_y; LowPassFilter2pVector3f _gyro_filter;
LowPassFilter2p _accel_filter_z;
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
uint8_t _gyro_instance; uint8_t _gyro_instance;
uint8_t _accel_instance; uint8_t _accel_instance;

24
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -115,12 +115,8 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu), AP_InertialSensor_Backend(imu),
_have_gyro_sample(false), _have_gyro_sample(false),
_have_accel_sample(false), _have_accel_sample(false),
_accel_filter_x(800, 10), _accel_filter(800, 10),
_accel_filter_y(800, 10), _gyro_filter(800, 10),
_accel_filter_z(800, 10),
_gyro_filter_x(800, 10),
_gyro_filter_y(800, 10),
_gyro_filter_z(800, 10)
{} {}
bool AP_InertialSensor_L3G4200D::_init_sensor(void) bool AP_InertialSensor_L3G4200D::_init_sensor(void)
@ -235,12 +231,8 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
if (filter_hz == 0) if (filter_hz == 0)
filter_hz = _default_filter_hz; filter_hz = _default_filter_hz;
_accel_filter_x.set_cutoff_frequency(800, filter_hz); _accel_filter.set_cutoff_frequency(800, filter_hz);
_accel_filter_y.set_cutoff_frequency(800, filter_hz); _gyro_filter.set_cutoff_frequency(800, filter_hz);
_accel_filter_z.set_cutoff_frequency(800, filter_hz);
_gyro_filter_x.set_cutoff_frequency(800, filter_hz);
_gyro_filter_y.set_cutoff_frequency(800, filter_hz);
_gyro_filter_z.set_cutoff_frequency(800, filter_hz);
} }
/* /*
@ -308,9 +300,7 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
if (hal.i2c->readRegisters(L3G4200D_I2C_ADDRESS, L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT, if (hal.i2c->readRegisters(L3G4200D_I2C_ADDRESS, L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT,
sizeof(buffer), (uint8_t *)&buffer[0][0]) == 0) { sizeof(buffer), (uint8_t *)&buffer[0][0]) == 0) {
for (uint8_t i=0; i<num_samples_available; i++) { for (uint8_t i=0; i<num_samples_available; i++) {
_gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]), _gyro_filtered = _gyro_filter.apply(Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]));
_gyro_filter_y.apply(-buffer[i][1]),
_gyro_filter_z.apply(-buffer[i][2]));
_have_gyro_sample = true; _have_gyro_sample = true;
} }
} }
@ -330,9 +320,7 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
sizeof(buffer[0]), num_samples_available, sizeof(buffer[0]), num_samples_available,
(uint8_t *)&buffer[0][0]) == 0) { (uint8_t *)&buffer[0][0]) == 0) {
for (uint8_t i=0; i<num_samples_available; i++) { for (uint8_t i=0; i<num_samples_available; i++) {
_accel_filtered = Vector3f(_accel_filter_x.apply(buffer[i][0]), _accel_filtered = _accel_filter.apply(Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]));
_accel_filter_y.apply(-buffer[i][1]),
_accel_filter_z.apply(-buffer[i][2]));
_have_accel_sample = true; _have_accel_sample = true;
} }
} }

8
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -43,12 +43,8 @@ private:
void _set_filter_frequency(uint8_t filter_hz); void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel // Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x; LowPassFilter2pVector3f _accel_filter;
LowPassFilter2p _accel_filter_y; LowPassFilter2pVector3f _gyro_filter;
LowPassFilter2p _accel_filter_z;
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
// gyro and accel instances // gyro and accel instances
uint8_t _gyro_instance; uint8_t _gyro_instance;

20
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -181,12 +181,8 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
_last_filter_hz(0), _last_filter_hz(0),
_error_count(0), _error_count(0),
#if MPU6000_FAST_SAMPLING #if MPU6000_FAST_SAMPLING
_accel_filter_x(1000, 15), _accel_filter(1000, 15),
_accel_filter_y(1000, 15), _gyro_filter(1000, 15),
_accel_filter_z(1000, 15),
_gyro_filter_x(1000, 15),
_gyro_filter_y(1000, 15),
_gyro_filter_z(1000, 15),
#else #else
_sample_count(0), _sample_count(0),
_accel_sum(), _accel_sum(),
@ -376,13 +372,13 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() {
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
#if MPU6000_FAST_SAMPLING #if MPU6000_FAST_SAMPLING
_accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), _accel_filtered = _accel_filter.apply(Vector3f(int16_val(rx.v, 1),
_accel_filter_y.apply(int16_val(rx.v, 0)), int16_val(rx.v, 0),
_accel_filter_z.apply(-int16_val(rx.v, 2))); -int16_val(rx.v, 2)));
_gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), _gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(rx.v, 5),
_gyro_filter_y.apply(int16_val(rx.v, 4)), int16_val(rx.v, 4),
_gyro_filter_z.apply(-int16_val(rx.v, 6))); -int16_val(rx.v, 6)));
#else #else
_accel_sum.x += int16_val(rx.v, 1); _accel_sum.x += int16_val(rx.v, 1);
_accel_sum.y += int16_val(rx.v, 0); _accel_sum.y += int16_val(rx.v, 0);

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -80,12 +80,8 @@ private:
Vector3f _gyro_filtered; Vector3f _gyro_filtered;
// Low Pass filters for gyro and accel // Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x; LowPassFilter2pVector3f _accel_filter;
LowPassFilter2p _accel_filter_y; LowPassFilter2pVector3f _gyro_filter;
LowPassFilter2p _accel_filter_z;
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
#else #else
// accumulation in timer - must be read with timer disabled // accumulation in timer - must be read with timer disabled
// the sum of the values since last read // the sum of the values since last read

26
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -327,12 +327,8 @@ static struct gyro_state_s st = {
AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) : AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu), AP_InertialSensor_Backend(imu),
_have_sample_available(false), _have_sample_available(false),
_accel_filter_x(800, 10), _accel_filter(800, 10),
_accel_filter_y(800, 10), _gyro_filter(800, 10),
_accel_filter_z(800, 10),
_gyro_filter_x(800, 10),
_gyro_filter_y(800, 10),
_gyro_filter_z(800, 10)
{ {
} }
@ -361,12 +357,8 @@ void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
if (filter_hz == 0) if (filter_hz == 0)
filter_hz = _default_filter_hz; filter_hz = _default_filter_hz;
_accel_filter_x.set_cutoff_frequency(800, filter_hz); _accel_filter.set_cutoff_frequency(800, filter_hz);
_accel_filter_y.set_cutoff_frequency(800, filter_hz); _gyro_filter.set_cutoff_frequency(800, filter_hz);
_accel_filter_z.set_cutoff_frequency(800, filter_hz);
_gyro_filter_x.set_cutoff_frequency(800, filter_hz);
_gyro_filter_y.set_cutoff_frequency(800, filter_hz);
_gyro_filter_z.set_cutoff_frequency(800, filter_hz);
} }
/** /**
@ -1087,15 +1079,9 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
// TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this // TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this
// is because the sensor is placed in the bottom side of the board? // is because the sensor is placed in the bottom side of the board?
_accel_filtered = Vector3f( _accel_filtered = _accel_filter.apply(Vector3f(accel_x, accel_y, accel_z));
_accel_filter_x.apply(accel_x),
_accel_filter_y.apply(accel_y),
_accel_filter_z.apply(accel_z));
_gyro_filtered = Vector3f( _gyro_filtered = _gyro_filter.apply(Vector3f(gyro_x, gyro_y, gyro_z));
_gyro_filter_x.apply(gyro_x),
_gyro_filter_y.apply(gyro_y),
_gyro_filter_z.apply(gyro_z));
_have_sample_available = true; _have_sample_available = true;
} }

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
View File

@ -52,12 +52,8 @@ private:
void _set_filter_frequency(uint8_t filter_hz); void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel // Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x; LowPassFilter2pVector3f _accel_filter;
LowPassFilter2p _accel_filter_y; LowPassFilter2pVector3f _gyro_filter;
LowPassFilter2p _accel_filter_z;
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
uint8_t _gyro_instance; uint8_t _gyro_instance;
uint8_t _accel_instance; uint8_t _accel_instance;

28
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -175,12 +175,8 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu), AP_InertialSensor_Backend(imu),
_last_filter_hz(-1), _last_filter_hz(-1),
_shared_data_idx(0), _shared_data_idx(0),
_accel_filter_x(1000, 15), _accel_filter(1000, 15),
_accel_filter_y(1000, 15), _gyro_filter(1000, 15),
_accel_filter_z(1000, 15),
_gyro_filter_x(1000, 15),
_gyro_filter_y(1000, 15),
_gyro_filter_z(1000, 15),
_have_sample_available(false) _have_sample_available(false)
{ {
} }
@ -341,13 +337,13 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
Vector3f _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), Vector3f _accel_filtered = _accel_filter.apply(Vector3f(int16_val(rx.v, 1),
_accel_filter_y.apply(int16_val(rx.v, 0)), int16_val(rx.v, 0),
_accel_filter_z.apply(-int16_val(rx.v, 2))); -int16_val(rx.v, 2)));
Vector3f _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), Vector3f _gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(rx.v, 5),
_gyro_filter_y.apply(int16_val(rx.v, 4)), int16_val(rx.v, 4),
_gyro_filter_z.apply(-int16_val(rx.v, 6))); -int16_val(rx.v, 6)));
// update the shared buffer // update the shared buffer
uint8_t idx = _shared_data_idx ^ 1; uint8_t idx = _shared_data_idx ^ 1;
_shared_data[idx]._accel_filtered = _accel_filtered; _shared_data[idx]._accel_filtered = _accel_filtered;
@ -395,13 +391,9 @@ void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz)
filter_hz = _default_filter_hz; filter_hz = _default_filter_hz;
} }
_accel_filter_x.set_cutoff_frequency(1000, filter_hz); _accel_filter.set_cutoff_frequency(1000, filter_hz);
_accel_filter_y.set_cutoff_frequency(1000, filter_hz);
_accel_filter_z.set_cutoff_frequency(1000, filter_hz);
_gyro_filter_x.set_cutoff_frequency(1000, filter_hz); _gyro_filter.set_cutoff_frequency(1000, filter_hz);
_gyro_filter_y.set_cutoff_frequency(1000, filter_hz);
_gyro_filter_z.set_cutoff_frequency(1000, filter_hz);
} }

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -60,12 +60,8 @@ private:
volatile uint8_t _shared_data_idx; volatile uint8_t _shared_data_idx;
// Low Pass filters for gyro and accel // Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x; LowPassFilter2pVector3f _accel_filter;
LowPassFilter2p _accel_filter_y; LowPassFilter2pVector3f _gyro_filter;
LowPassFilter2p _accel_filter_z;
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
// do we currently have a sample pending? // do we currently have a sample pending?
bool _have_sample_available; bool _have_sample_available;