bmi055: add support for IMU_GYRO_CUTOFF and IMU_ACCEL_CUTOFF

src/drivers/imu/bmi055/BMI055_accel.cpp

@@ -118,11 +118,13 @@ BMI055_accel::init()
 	_accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
 
 	if (_accel_reports == nullptr) {
-		goto out;
+		return -ENOMEM;
 	}
 
-	if (reset() != OK) {
+	ret = reset();
-		goto out;
+
+	if (ret != OK) {
+		return ret;
 	}
 
 	/* Initialize offsets and scales */
@@ -135,6 +137,16 @@ BMI055_accel::init()
 
 	_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
 
+	param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
+	float accel_cut = BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
+
+	if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
+
+		_accel_filter_x.set_cutoff_frequency(BMI055_ACCEL_DEFAULT_RATE, accel_cut);
+		_accel_filter_y.set_cutoff_frequency(BMI055_ACCEL_DEFAULT_RATE, accel_cut);
+		_accel_filter_z.set_cutoff_frequency(BMI055_ACCEL_DEFAULT_RATE, accel_cut);
+	}
+
 	measure();
 
 	/* advertise sensor topic, measure manually to initialize valid report */
@@ -149,7 +161,6 @@ BMI055_accel::init()
 		warnx("ADVERT FAIL");
 	}
 
-out:
 	return ret;
 }
 

src/drivers/imu/bmi055/BMI055_gyro.cpp

@@ -117,11 +117,13 @@ BMI055_gyro::init()
 	_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
 
 	if (_gyro_reports == nullptr) {
-		goto out;
+		return -ENOMEM;
 	}
 
-	if (reset() != OK) {
+	ret = reset();
-		goto out;
+
+	if (ret != OK) {
+		return ret;
 	}
 
 	/* Initialize offsets and scales */
@@ -132,11 +134,14 @@ BMI055_gyro::init()
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
+	param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
+	float gyro_cut = BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
 
-	/* if probe/setup failed, bail now */
+	if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
-	if (ret != OK) {
+
-		DEVICE_DEBUG("gyro init failed");
+		_gyro_filter_x.set_cutoff_frequency(BMI055_GYRO_DEFAULT_RATE, gyro_cut);
-		return ret;
+		_gyro_filter_y.set_cutoff_frequency(BMI055_GYRO_DEFAULT_RATE, gyro_cut);
+		_gyro_filter_z.set_cutoff_frequency(BMI055_GYRO_DEFAULT_RATE, gyro_cut);
 	}
 
 	_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
@@ -154,7 +159,6 @@ BMI055_gyro::init()
 		warnx("ADVERT FAIL");
 	}
 
-out:
 	return ret;
 }