Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Accelerometer Filtering #151

Merged
merged 3 commits into from
Sep 28, 2016
Merged

Accelerometer Filtering #151

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
aee7404
sensors_cf2: Increase accel range to 16g, increase update rate to 1kH…
stephanbro Sep 22, 2016
248b670
filter: Add a 2 pole low pass filter.
stephanbro Sep 26, 2016
6b24168
sensors_cf2: Apply a 30hz 2pole low pass to the accel data.
stephanbro Sep 26, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 21 additions & 8 deletions src/hal/src/sensors_cf2.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -66,8 +67,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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is the accel DLPF setting. Probably what you want to change?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good catch. I think that's likely the case.

#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);
Expand All @@ -348,9 +351,13 @@ 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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This will change setting to 1kHz sampling output right? Please update comment above if that is the case. Also does 1kHz sample output improve anything, latency maybe?

// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
mpu6500SetDLPFMode(MPU6500_DLPF_BW_42);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This will set the gyro DLPF. Is that what you intended to do or you wanted to change the accel DLPF? I think this setting might slow down the system.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You are right, I don't think we want to change the DLPF on the gyro.


for (uint8_t i = 0; i < 3; i++) {
lpf2pInit(&accLpf[i], 1000, 30);
}
#endif


Expand Down Expand Up @@ -398,7 +405,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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please update comment as well. Faster sampling rate might improve latency? Reason why you set it to 100Hz?

#endif

mpu6500SetI2CBypassEnabled(false);
Expand Down Expand Up @@ -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)
Expand Down
13 changes: 13 additions & 0 deletions src/utils/interface/filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_
41 changes: 41 additions & 0 deletions src/utils/src/filter.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,10 @@
*
* filter.h - Filtering functions
*/

#include <math.h>
#include <stdlib.h>

#include "filter.h"

/**
Expand Down Expand Up @@ -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;
}