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 1438fb8cac31..170b7fe182fd 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()