From 4be4b9810e83f48218c59d5b3cc9dfea575610e9 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Thu, 19 Apr 2018 21:10:00 +0200
Subject: [PATCH] mpu9250 wrapper: support updating gyro & accel cutoff via
parameter
- useful for tuning snapdragon flight
Signed-off-by: Roman <bapstroman@gmail.com>
---
.../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 25 +++++++++++++++++++
1 file changed, 25 insertions(+)
diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index 1438fb8cac..170b7fe182 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -242,6 +242,31 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) :
_mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f;
}
+
+ // set software low pass filter for controllers
+ param_t param_handle = param_find("IMU_ACCEL_CUTOFF");
+ float param_val = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
+
+ if (param_handle != PARAM_INVALID && (param_get(param_handle, ¶m_val) == PX4_OK)) {
+ _accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
+ _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
+ _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
+
+ } else {
+ PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
+ }
+
+ param_handle = param_find("IMU_GYRO_CUTOFF");
+ param_val = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
+
+ if (param_handle != PARAM_INVALID && (param_get(param_handle, ¶m_val) == PX4_OK)) {
+ _gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
+ _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
+ _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
+
+ } else {
+ PX4_ERR("IMU_GYRO_CUTOFF param invalid");
+ }
}
DfMpu9250Wrapper::~DfMpu9250Wrapper()
--
GitLab