Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Imu filter (#7606)

Browse Source 
* Add adjustable driver level cutoff freq for accel/gyro.

* Fix copy and paste error.

* Updated print_info.

* imu filter minor cleanup
This commit is contained in:
James Goppert
2017-09-25 13:55:56 -04:00
committed by GitHub
parent a945cd9fc2
commit e72769c924
3 changed files with 110 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+37 -7
View File
@@ -658,19 +658,19 @@ MPU6000::init()
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
return ret;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
return ret;
}
ret = -EIO;
if (reset() != OK) {
goto out;
return ret;
}
/* Initialize offsets and scales */
@@ -688,6 +688,34 @@ MPU6000::init()
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) {
PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
_accel_filter_x.set_cutoff_frequency(MPU6000_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_y.set_cutoff_frequency(MPU6000_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_z.set_cutoff_frequency(MPU6000_ACCEL_DEFAULT_RATE, accel_cut);
} else {
PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
}
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) {
PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
_gyro_filter_x.set_cutoff_frequency(MPU6000_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_y.set_cutoff_frequency(MPU6000_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_z.set_cutoff_frequency(MPU6000_GYRO_DEFAULT_RATE, gyro_cut);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
@@ -711,10 +739,9 @@ MPU6000::init()
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
PX4_WARN("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
@@ -723,10 +750,9 @@ MPU6000::init()
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_gyro->_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
PX4_WARN("ADVERT FAIL");
}
out:
return ret;
}
@@ -2137,6 +2163,10 @@ MPU6000::print_info()
}
::printf("temperature: %.1f\n", (double)_last_temperature);
float accel_cut = _accel_filter_x.get_cutoff_freq();
::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut));
float gyro_cut = _gyro_filter_x.get_cutoff_freq();
::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut));
}
void
src/drivers/mpu9250/mpu9250.cpp
+43 -11
View File
@@ -282,18 +282,18 @@ MPU9250::init()
ret = -ENOMEM;
if (_accel_reports == nullptr) {
goto out;
return ret;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
return ret;
}
if (reset_mpu() != OK) {
PX4_ERR("Exiting! Device failed to take initialization");
goto out;
return ret;
}
/* Initialize offsets and scales */
@@ -311,22 +311,51 @@ MPU9250::init()
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
} else {
PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
}
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
goto out;
return ret;
}
#ifdef USE_I2C
if (!_mag->is_passthrough() && _mag->_interface->init() != OK) {
warnx("failed to setup ak8963 interface");
if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) {
PX4_ERR("failed to setup ak8963 interface");
}
#endif
#endif /* USE_I2C */
/* do CDev init for the mag device node, keep it optional */
if (_whoami == MPU_WHOAMI_9250) {
@@ -336,7 +365,7 @@ MPU9250::init()
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("mag init failed");
goto out;
return ret;
}
@@ -364,7 +393,7 @@ MPU9250::init()
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
goto out;
return ret;
}
/* advertise sensor topic, measure manually to initialize valid report */
@@ -376,10 +405,9 @@ MPU9250::init()
if (_gyro->_gyro_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
goto out;
return ret;
}
out:
return ret;
}
@@ -1565,6 +1593,10 @@ MPU9250::print_info()
}
::printf("temperature: %.1f\n", (double)_last_temperature);
float accel_cut = _accel_filter_x.get_cutoff_freq();
::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut));
float gyro_cut = _gyro_filter_x.get_cutoff_freq();
::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut));
}
void
src/modules/sensors/sensor_params.c
+30
View File
@@ -1144,3 +1144,33 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
* @group Sensor Enable
*/
PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
/**
* Driver level cut frequency for gyro
*
* The cut frequency for the 2nd order butterworth filter on the gyro driver. This features
* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
* controllers, not the estimators. 0 disables the filter.
*
* @min 5
* @max 1000
* @unit Hz
* @reboot_required true
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
/**
* Driver level cut frequency for accel
*
* The cut frequency for the 2nd order butterworth filter on the accel driver. This features
* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
* controllers, not the estimators. 0 disables the filter.
*
* @min 5
* @max 1000
* @unit Hz
* @reboot_required true
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f);