From ba6ef1931405fb65d6facc7fa4ac5609a4d7bf24 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
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),
--
GitLab