From ba6ef1931405fb65d6facc7fa4ac5609a4d7bf24 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 11 Feb 2019 15:06:03 +0100 Subject: [PATCH] bmi055: fixes for on-chip filter - Accel: use cutoff of 62.5 Hz instead of 500 Hz - Gyro: the cutoff frequency is coupled with the ODR and is fixed to 116 Hz at 1 kHz readout rate. So this patch does not change anything for the gyro. --- src/drivers/imu/bmi055/BMI055.hpp | 2 - src/drivers/imu/bmi055/BMI055_accel.cpp | 52 +------------------------ src/drivers/imu/bmi055/BMI055_accel.hpp | 9 +---- src/drivers/imu/bmi055/BMI055_gyro.cpp | 12 +++--- src/drivers/imu/bmi055/BMI055_gyro.hpp | 2 +- src/drivers/imu/bmi055/bmi055_main.cpp | 1 - 6 files changed, 9 insertions(+), 69 deletions(-) diff --git a/src/drivers/imu/bmi055/BMI055.hpp b/src/drivers/imu/bmi055/BMI055.hpp index 54fb3403c1..a341ce288c 100644 --- a/src/drivers/imu/bmi055/BMI055.hpp +++ b/src/drivers/imu/bmi055/BMI055.hpp @@ -62,8 +62,6 @@ protected: struct hrt_call _call; unsigned _call_interval; - unsigned _dlpf_freq; - uint8_t _register_wait; uint64_t _reset_wait; diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp index f97d0e8735..5bf7ba27cd 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.cpp +++ b/src/drivers/imu/bmi055/BMI055_accel.cpp @@ -60,7 +60,6 @@ BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enu _accel_topic(nullptr), _accel_orb_class_instance(-1), _accel_class_instance(-1), - _accel_sample_rate(BMI055_ACCEL_DEFAULT_RATE), _accel_filter_x(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -159,13 +158,12 @@ int BMI055_accel::reset() write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset up_udelay(5000); - write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_1000); //Write accel bandwidth + write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_62_5); //Write accel bandwidth (DLPF) write_checked_reg(BMI055_ACC_RANGE, BMI055_ACCEL_RANGE_2_G);//Write range write_checked_reg(BMI055_ACC_INT_EN_1, BMI055_ACC_DRDY_INT_EN); //Enable DRDY interrupt write_checked_reg(BMI055_ACC_INT_MAP_1, BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1 set_accel_range(BMI055_ACCEL_DEFAULT_RANGE_G);//set accel range - accel_set_sample_rate(BMI055_ACCEL_DEFAULT_RATE);//set accel ODR //Enable Accelerometer in normal mode write_reg(BMI055_ACC_PMU_LPW, BMI055_ACCEL_NORMAL); @@ -211,54 +209,6 @@ BMI055_accel::probe() return -EIO; } -int -BMI055_accel::accel_set_sample_rate(float frequency) -{ - uint8_t setbits = 0; - uint8_t clearbits = BMI055_ACCEL_BW_1000; - - if (frequency < (3125 / 100)) { - setbits |= BMI055_ACCEL_BW_7_81; - _accel_sample_rate = 1563 / 100; - - } else if (frequency < (625 / 10)) { - setbits |= BMI055_ACCEL_BW_15_63; - _accel_sample_rate = 625 / 10; - - } else if (frequency < (125)) { - setbits |= BMI055_ACCEL_BW_31_25; - _accel_sample_rate = 625 / 10; - - } else if (frequency < 250) { - setbits |= BMI055_ACCEL_BW_62_5; - _accel_sample_rate = 125; - - } else if (frequency < 500) { - setbits |= BMI055_ACCEL_BW_125; - _accel_sample_rate = 250; - - } else if (frequency < 1000) { - setbits |= BMI055_ACCEL_BW_250; - _accel_sample_rate = 500; - - } else if (frequency < 2000) { - setbits |= BMI055_ACCEL_BW_500; - _accel_sample_rate = 1000; - - } else if (frequency >= 2000) { - setbits |= BMI055_ACCEL_BW_1000; - _accel_sample_rate = 2000; - - } else { - return -EINVAL; - } - - /* Write accel ODR */ - modify_reg(BMI055_ACC_BW, clearbits, setbits); - - return OK; -} - ssize_t BMI055_accel::read(struct file *filp, char *buffer, size_t buflen) { diff --git a/src/drivers/imu/bmi055/BMI055_accel.hpp b/src/drivers/imu/bmi055/BMI055_accel.hpp index a4ccb6af57..773f3fad51 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.hpp +++ b/src/drivers/imu/bmi055/BMI055_accel.hpp @@ -109,7 +109,7 @@ // BMI055 Accelerometer Chip-Id #define BMI055_ACC_WHO_AM_I 0xFA -//BMI055_ACC_BW 0x10 +// DLPF filter bandwidth settings #define BMI055_ACCEL_BW_7_81 (1<<3) | (0<<2) | (0<<1) | (0<<0) #define BMI055_ACCEL_BW_15_63 (1<<3) | (0<<2) | (0<<1) | (1<<0) #define BMI055_ACCEL_BW_31_25 (1<<3) | (0<<2) | (1<<1) | (0<<0) @@ -191,8 +191,6 @@ private: int _accel_orb_class_instance; int _accel_class_instance; - float _accel_sample_rate; - math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; @@ -279,11 +277,6 @@ private: */ int self_test(); - /* - * set accel sample rate - */ - int accel_set_sample_rate(float desired_sample_rate_hz); - /* * check that key registers still have the right value */ diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp index 793726479e..eb737fc62d 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.cpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp @@ -162,13 +162,13 @@ int BMI055_gyro::reset() { write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset usleep(5000); - write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth + write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate()) write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range write_checked_reg(BMI055_GYR_INT_EN_0, BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1 set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range - gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR + gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth //Enable Gyroscope in normal mode write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL); @@ -221,19 +221,19 @@ BMI055_gyro::gyro_set_sample_rate(float frequency) uint8_t clearbits = BMI055_GYRO_BW_MASK; if (frequency <= 100) { - setbits |= BMI055_GYRO_RATE_100; + setbits |= BMI055_GYRO_RATE_100; /* 32 Hz cutoff */ _gyro_sample_rate = 100; } else if (frequency <= 250) { - setbits |= BMI055_GYRO_RATE_400; + setbits |= BMI055_GYRO_RATE_400; /* 47 Hz cutoff */ _gyro_sample_rate = 400; } else if (frequency <= 1000) { - setbits |= BMI055_GYRO_RATE_1000; + setbits |= BMI055_GYRO_RATE_1000; /* 116 Hz cutoff */ _gyro_sample_rate = 1000; } else if (frequency > 1000) { - setbits |= BMI055_GYRO_RATE_2000; + setbits |= BMI055_GYRO_RATE_2000; /* 230 Hz cutoff */ _gyro_sample_rate = 2000; } else { diff --git a/src/drivers/imu/bmi055/BMI055_gyro.hpp b/src/drivers/imu/bmi055/BMI055_gyro.hpp index b8dbc20ea6..cc1eb42ce6 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.hpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.hpp @@ -102,7 +102,7 @@ // BMI055 Gyroscope Chip-Id #define BMI055_GYR_WHO_AM_I 0x0F -//BMI055_GYR_BW 0x10 +// ODR & DLPF filter bandwidth settings (they are coupled) #define BMI055_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0) #define BMI055_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0) #define BMI055_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0) diff --git a/src/drivers/imu/bmi055/bmi055_main.cpp b/src/drivers/imu/bmi055/bmi055_main.cpp index e0bc5ef82d..44db028cc9 100644 --- a/src/drivers/imu/bmi055/bmi055_main.cpp +++ b/src/drivers/imu/bmi055/bmi055_main.cpp @@ -448,7 +448,6 @@ BMI055::BMI055(const char *name, const char *devname, int bus, uint32_t device, _whoami(0), _call{}, _call_interval(0), - _dlpf_freq(0), _register_wait(0), _reset_wait(0), _rotation(rotation),