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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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),