fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

ADIS16477 support filtering params and enable onboard filter

Browse Source
This commit is contained in:
Daniel Agar
2018-10-09 10:37:12 -04:00
parent f74b96e918
commit 4731f8c735
1 changed files with 36 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/adis16477/ADIS16477.cpp
+36
View File
@@ -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;
}