From 31e634b082033b5c16cfaebd0118c93d964205a9 Mon Sep 17 00:00:00 2001 From: JochiPochi Date: Wed, 17 Aug 2016 13:44:38 -0400 Subject: [PATCH] mpu6000 driver: set icm registers only on icm devices --- src/drivers/mpu6000/mpu6000.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 36999b9030..642cabfc83 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -744,7 +744,11 @@ int MPU6000::reset() // was 90 Hz, but this ruins quality and does not improve the // system response _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); - _set_icm_acc_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); + + if (is_icm_device()) { + _set_icm_acc_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); + } + usleep(1000); // Gyro scale 2000 deg/s () write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -1329,7 +1333,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); - _set_icm_acc_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);