From 75bb3e9bac7c7125e4b81bcaf537b845764747e0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 12 Feb 2019 10:17:59 +0100
Subject: [PATCH] bmi055: add support for IMU_GYRO_CUTOFF and IMU_ACCEL_CUTOFF
---
src/drivers/imu/bmi055/BMI055_accel.cpp | 19 +++++++++++++++----
src/drivers/imu/bmi055/BMI055_gyro.cpp | 20 ++++++++++++--------
2 files changed, 27 insertions(+), 12 deletions(-)
diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp
index 5bf7ba27cd..04a89b7b79 100644
--- a/src/drivers/imu/bmi055/BMI055_accel.cpp
+++ b/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) {
- goto out;
+ ret = reset();
+
+ 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;
}
diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp
index eb737fc62d..275dba833d 100644
--- a/src/drivers/imu/bmi055/BMI055_gyro.cpp
+++ b/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) {
- goto out;
+ ret = reset();
+
+ 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 (ret != OK) {
- DEVICE_DEBUG("gyro init failed");
- return ret;
+ if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
+
+ _gyro_filter_x.set_cutoff_frequency(BMI055_GYRO_DEFAULT_RATE, gyro_cut);
+ _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;
}
--
GitLab