From 4731f8c7358eda63b3341b99a3556a1a3197b1c1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 4 Oct 2018 00:48:01 -0400 Subject: [PATCH] ADIS16477 support filtering params and enable onboard filter --- src/drivers/imu/adis16477/ADIS16477.cpp | 36 +++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index dc8e8cf062..4ebcd5ea98 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -60,6 +60,9 @@ static constexpr uint8_t Z_ACCL_OUT = 0x1A; // Output, z-axis accelerometer, hig static constexpr uint8_t TEMP_OUT = 0x1A; // Output, temperature static constexpr uint8_t TIME_STAMP = 0x1A; // Output, time stamp +static constexpr uint8_t FILT_CTRL = 0x5C; +static constexpr uint8_t DEC_RATE = 0x64; + static constexpr uint8_t GLOB_CMD = 0x68; static constexpr uint8_t PROD_ID = 0x72; @@ -106,6 +109,33 @@ ADIS16477::ADIS16477(int bus, const char *path_accel, const char *path_gyro, uin _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; + + const unsigned sample_rate = 1000; + + // set software low pass filter for controllers + param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); + float accel_cut = ADIS16477_ACCEL_DEFAULT_RATE; + + if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) { + _accel_filter_x.set_cutoff_frequency(sample_rate, accel_cut); + _accel_filter_y.set_cutoff_frequency(sample_rate, accel_cut); + _accel_filter_z.set_cutoff_frequency(sample_rate, accel_cut); + + } else { + PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); + } + + param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); + float gyro_cut = ADIS16477_GYRO_DEFAULT_RATE; + + if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) { + _gyro_filter_x.set_cutoff_frequency(sample_rate, gyro_cut); + _gyro_filter_y.set_cutoff_frequency(sample_rate, gyro_cut); + _gyro_filter_z.set_cutoff_frequency(sample_rate, gyro_cut); + + } else { + PX4_ERR("IMU_GYRO_CUTOFF param invalid"); + } } ADIS16477::~ADIS16477() @@ -211,6 +241,12 @@ int ADIS16477::reset() // Reset Recovery Time up_mdelay(193); + // configure digital filter, 16 taps + write_reg16(FILT_CTRL, 0x04); + + // configure decimation rate + //write_reg16(DEC_RATE, 0x00); + return OK; }