fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 11:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

bmi055: fixes for on-chip filter

Browse Source 
- 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.
This commit is contained in:
Beat Küng
2019-02-11 15:06:03 +01:00
committed by Daniel Agar
parent 6762f09490
commit ba6ef19314
6 changed files with 9 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/bmi055/BMI055.hpp
-2
View File
@@ -62,8 +62,6 @@ protected:
struct hrt_call _call;
unsigned _call_interval;
unsigned _dlpf_freq;
uint8_t _register_wait;
uint64_t _reset_wait;
src/drivers/imu/bmi055/BMI055_accel.cpp
+1 -51
View File
@@ -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)
{
src/drivers/imu/bmi055/BMI055_accel.hpp
+1 -8
View File
@@ -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
*/
src/drivers/imu/bmi055/BMI055_gyro.cpp
+6 -6
View File
@@ -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 {
src/drivers/imu/bmi055/BMI055_gyro.hpp
+1 -1
View File
@@ -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)
src/drivers/imu/bmi055/bmi055_main.cpp
-1
View File
@@ -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),