yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

bmi055: add support for IMU_GYRO_CUTOFF and IMU_ACCEL_CUTOFF

Browse Source
This commit is contained in:
Beat Küng
2019-02-12 10:17:59 +01:00
committed by Daniel Agar
parent ba6ef19314
commit 75bb3e9bac
2 changed files with 27 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/drivers/imu/bmi055/BMI055_accel.cpp
View File

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

20
src/drivers/imu/bmi055/BMI055_gyro.cpp
View File

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