From aee7404aab4ad28eeac54702d7cf2e35e92415bc Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Thu, 22 Sep 2016 16:34:36 -0700 Subject: [PATCH 1/3] sensors_cf2: Increase accel range to 16g, increase update rate to 1kHz, and set DLPF to 42Hz. --- src/hal/src/sensors_cf2.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/hal/src/sensors_cf2.c b/src/hal/src/sensors_cf2.c index f27942ab9f..7c8d9d874e 100644 --- a/src/hal/src/sensors_cf2.c +++ b/src/hal/src/sensors_cf2.c @@ -66,8 +66,8 @@ #define SENSORS_GYRO_FS_CFG MPU6500_GYRO_FS_2000 #define SENSORS_DEG_PER_LSB_CFG MPU6500_DEG_PER_LSB_2000 -#define SENSORS_ACCEL_FS_CFG MPU6500_ACCEL_FS_8 -#define SENSORS_G_PER_LSB_CFG MPU6500_G_PER_LSB_8 +#define SENSORS_ACCEL_FS_CFG MPU6500_ACCEL_FS_16 +#define SENSORS_G_PER_LSB_CFG MPU6500_G_PER_LSB_16 #define SENSORS_VARIANCE_MAN_TEST_TIMEOUT M2T(2000) // Timeout in ms #define SENSORS_MAN_TEST_LEVEL_MAX 5.0f // Max degrees off @@ -348,9 +348,9 @@ static void sensorsDeviceInit(void) // To low DLPF bandwidth might cause instability and decrease agility // but it works well for handling vibrations and unbalanced propellers // Set output rate (1): 1000 / (1 + 1) = 500Hz - mpu6500SetRate(1); + mpu6500SetRate(0); // Set digital low-pass bandwidth - mpu6500SetDLPFMode(MPU6500_DLPF_BW_98); + mpu6500SetDLPFMode(MPU6500_DLPF_BW_42); #endif @@ -398,7 +398,7 @@ static void sensorsSetupSlaveRead(void) // SMPLRT_DIV is only used for 1kHz internal sampling." Slowest update rate is then 500Hz. mpu6500SetSlave4MasterDelay(15); // read slaves at 500Hz = (8000Hz / (1 + 15)) #else - mpu6500SetSlave4MasterDelay(4); // read slaves at 100Hz = (500Hz / (1 + 4)) + mpu6500SetSlave4MasterDelay(9); // read slaves at 100Hz = (500Hz / (1 + 4)) #endif mpu6500SetI2CBypassEnabled(false); From 248b67033085f8668d8c5c756b61fe4878f624a8 Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Mon, 26 Sep 2016 10:42:21 -0700 Subject: [PATCH 2/3] filter: Add a 2 pole low pass filter. --- src/utils/interface/filter.h | 13 ++++++++++++ src/utils/src/filter.c | 41 ++++++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+) diff --git a/src/utils/interface/filter.h b/src/utils/interface/filter.h index 384c23d096..15adf1cdb7 100644 --- a/src/utils/interface/filter.h +++ b/src/utils/interface/filter.h @@ -31,4 +31,17 @@ int16_t iirLPFilterSingle(int32_t in, int32_t attenuation, int32_t* filt); +typedef struct { + float a1; + float a2; + float b0; + float b1; + float b2; + float delay_element_1; + float delay_element_2; +} lpf2pData; + +void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq); +float lpf2pApply(lpf2pData* lpfData, float sample); + #endif //FILTER_H_ diff --git a/src/utils/src/filter.c b/src/utils/src/filter.c index 442e1db7b7..e8e8f0e688 100644 --- a/src/utils/src/filter.c +++ b/src/utils/src/filter.c @@ -23,6 +23,10 @@ * * filter.h - Filtering functions */ + +#include +#include + #include "filter.h" /** @@ -53,3 +57,40 @@ int16_t iirLPFilterSingle(int32_t in, int32_t attenuation, int32_t* filt) return out; } + +/** + * 2-Pole low pass filter + */ +void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq) +{ + if (lpfData == NULL || cutoff_freq <= 0.0f) { + return; + } + + float fr = sample_freq/cutoff_freq; + float ohm = tanf(M_PI/fr); + float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm; + lpfData->b0 = ohm*ohm/c; + lpfData->b1 = 2.0f*lpfData->b0; + lpfData->b2 = lpfData->b0; + lpfData->a1 = 2.0f*(ohm*ohm-1.0f)/c; + lpfData->a2 = (1.0f-2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm)/c; + lpfData->delay_element_1 = 0.0f; + lpfData->delay_element_2 = 0.0f; +} + +float lpf2pApply(lpf2pData* lpfData, float sample) +{ + float delay_element_0 = sample - lpfData->delay_element_1 * lpfData->a1 - lpfData->delay_element_2 * lpfData->a2; + if (!isfinite(delay_element_0)) { + // don't allow bad values to propigate via the filter + delay_element_0 = sample; + } + + float output = delay_element_0 * lpfData->b0 + lpfData->delay_element_1 * lpfData->b1 + lpfData->delay_element_2 * lpfData->b2; + + lpfData->delay_element_2 = lpfData->delay_element_1; + lpfData->delay_element_1 = delay_element_0; + return output; +} + From 6b24168a08b7959c9290b970d53b6d1f5fb1ac3a Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Mon, 26 Sep 2016 10:42:53 -0700 Subject: [PATCH 3/3] sensors_cf2: Apply a 30hz 2pole low pass to the accel data. --- src/hal/src/sensors_cf2.c | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/hal/src/sensors_cf2.c b/src/hal/src/sensors_cf2.c index 7c8d9d874e..ced792c99b 100644 --- a/src/hal/src/sensors_cf2.c +++ b/src/hal/src/sensors_cf2.c @@ -51,6 +51,7 @@ #include "nvicconf.h" #include "ledseq.h" #include "sound.h" +#include "filter.h" /** * Enable 250Hz digital LPF mode. However does not work with @@ -119,6 +120,7 @@ static Axis3f gyroBiasStdDev; static bool gyroBiasFound = false; static float accScaleSum = 0; static float accScale = 1; +static lpf2pData accLpf[3]; static bool isBarometerPresent = false; static bool isMagnetometerPresent = false; @@ -153,6 +155,7 @@ static void sensorsCalculateBiasMean(BiasObj* bias, Axis3i32* meanOut); static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z); static bool sensorsFindBiasValue(BiasObj* bias); static void sensorsAccAlignToGravity(Axis3f* in, Axis3f* out); +static void sensorsAccApplyLpf(Axis3f* in); bool sensorsReadGyro(Axis3f *gyro) { @@ -293,6 +296,7 @@ void processAccGyroMeasurements(const uint8_t *buffer) accScaled.z = (az) * SENSORS_G_PER_LSB_CFG / accScale; sensorsAccAlignToGravity(&accScaled, &sensors.acc); + sensorsAccApplyLpf(&sensors.acc); } static void sensorsDeviceInit(void) @@ -335,10 +339,9 @@ static void sensorsDeviceInit(void) // Set accelerometer full scale range mpu6500SetFullScaleAccelRange(SENSORS_ACCEL_FS_CFG); #ifdef ESTIMATOR_TYPE_complementary + mpu6500SetRate(0); mpu6500SetAccelDLPF(MPU6500_ACCEL_DLPF_BW_20); -#endif - -#ifdef SENSORS_MPU6500_DLPF_256HZ +#elif SENSORS_MPU6500_DLPF_256HZ // 256Hz digital low-pass filter only works with little vibrations // Set output rate (15): 8000 / (1 + 15) = 500Hz mpu6500SetRate(15); @@ -351,6 +354,10 @@ static void sensorsDeviceInit(void) mpu6500SetRate(0); // Set digital low-pass bandwidth mpu6500SetDLPFMode(MPU6500_DLPF_BW_42); + + for (uint8_t i = 0; i < 3; i++) { + lpf2pInit(&accLpf[i], 1000, 30); + } #endif @@ -847,6 +854,12 @@ static void sensorsAccAlignToGravity(Axis3f* in, Axis3f* out) out->z = ry.z; } +static void sensorsAccApplyLpf(Axis3f* in) +{ + for (uint8_t i = 0; i < 3; i++) { + in->axis[i] = lpf2pApply(&accLpf[i], in->axis[i]); + } +} PARAM_GROUP_START(imu_sensors) PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, HMC5883L, &isMagnetometerPresent)