rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Implement smoothing filter for accelerometer data.

Browse Source
This commit is contained in:
Erik Gustavsson 2012-10-31 19:05:59 +01:00
parent 245e1e36cb
commit c207afbfef
2 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
flight/Modules/Attitude/attitude.c
View File

@ -86,6 +86,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0;
static float accelKp = 0;
static float accel_alpha = 0;
static float yawBiasRate = 0;
static float gyroGain = 0.42;
static int16_t accelbias[3];
@ -445,15 +446,22 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
float grot[3];
float accel_err[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
static float accels_filtered[3];
accels_filtered[0] = accels_filtered[0] * accel_alpha + accels[0] * (1 - accel_alpha);
accels_filtered[1] = accels_filtered[1] * accel_alpha + accels[1] * (1 - accel_alpha);
accels_filtered[2] = accels_filtered[2] * accel_alpha + accels[2] * (1 - accel_alpha);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) accels, (const float *) grot, accel_err);
CrossProduct((const float *) accels_filtered, (const float *) grot, accel_err);
// Account for accel magnitude
float accel_mag = sqrtf(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
float accel_mag = sqrtf(accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]);
if(accel_mag < 1.0e-3f)
return;
@ -531,6 +539,13 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025;
if(attitudeSettings.AccelTau < 0.0001)
accel_alpha = 0; // not trusting this to resolve to 0
else
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;

1
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -7,6 +7,7 @@
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.02"/>
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
<field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="BiasCorrectGyro" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>