Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fixes for IMU filter settings #9367

Merged
merged 1 commit into from
Apr 25, 2018
Merged

Fixes for IMU filter settings #9367

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions src/drivers/imu/bmi160/bmi160.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -655,14 +655,12 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg)
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / ticks;
_set_dlpf_filter(cutoff_freq_hz);
_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);


float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
_set_dlpf_filter(cutoff_freq_hz_gyro);
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
Expand Down
10 changes: 0 additions & 10 deletions src/drivers/imu/mpu6000/mpu6000.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -815,9 +815,6 @@ int MPU6000::reset()
_set_sample_rate(_sample_rate);
usleep(1000);

// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);

if (is_icm_device()) {
Expand Down Expand Up @@ -1409,19 +1406,12 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / ticks;
_set_dlpf_filter(cutoff_freq_hz);

if (is_icm_device()) {
_set_icm_acc_dlpf_filter(cutoff_freq_hz);
}

_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);


float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
_set_dlpf_filter(cutoff_freq_hz_gyro);
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
Expand Down
16 changes: 8 additions & 8 deletions src/drivers/imu/mpu6000/mpu6000.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,13 +114,13 @@
#define MPUREG_TRIM2 0x0E
#define MPUREG_TRIM3 0x0F
#define MPUREG_TRIM4 0x10
#define MPU_GYRO_DLPF_CFG_256HZ_NOLPF2 0x00
#define MPU_GYRO_DLPF_CFG_188HZ 0x01
#define MPU_GYRO_DLPF_CFG_98HZ 0x02
#define MPU_GYRO_DLPF_CFG_42HZ 0x03
#define MPU_GYRO_DLPF_CFG_20HZ 0x04
#define MPU_GYRO_DLPF_CFG_10HZ 0x05
#define MPU_GYRO_DLPF_CFG_5HZ 0x06
#define MPU_GYRO_DLPF_CFG_256HZ_NOLPF2 0x00 // delay: 0.98ms
#define MPU_GYRO_DLPF_CFG_188HZ 0x01 // delay: 1.9ms
#define MPU_GYRO_DLPF_CFG_98HZ 0x02 // delay: 2.8ms
#define MPU_GYRO_DLPF_CFG_42HZ 0x03 // delay: 4.8ms
#define MPU_GYRO_DLPF_CFG_20HZ 0x04 // delay: 8.3ms
#define MPU_GYRO_DLPF_CFG_10HZ 0x05 // delay: 13.4ms
#define MPU_GYRO_DLPF_CFG_5HZ 0x06 // delay: 18.6ms
Copy link
Member

@mhkabir mhkabir Apr 24, 2018 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should be able to use this delay data to improve IMU timestamping. We should subtract this value (based on selected DLPF) from the hrt_absolute_time() we use to timestamp the accel/gyro data.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm fine with that. You'll have to make sure that it works with the estimators.

#define MPU_GYRO_DLPF_CFG_2100HZ_NOLPF 0x07
#define MPU_DLPF_CFG_MASK 0x07

Expand Down Expand Up @@ -210,7 +210,7 @@
#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE
#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30

#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42
#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 98

#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
Expand Down
5 changes: 0 additions & 5 deletions src/drivers/imu/mpu9250/mpu9250.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -462,9 +462,6 @@ int MPU9250::reset_mpu()
// SAMPLE RATE
_set_sample_rate(_sample_rate);

// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);

// Gyro scale 2000 deg/s ()
Expand Down Expand Up @@ -830,14 +827,12 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / ticks;
_set_dlpf_filter(cutoff_freq_hz);
_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);


float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
_set_dlpf_filter(cutoff_freq_hz_gyro);
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
Expand Down
2 changes: 1 addition & 1 deletion src/drivers/imu/mpu9250/mpu9250.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,7 +188,7 @@
#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30

#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92

#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100)

Expand Down