diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c
index 6b0c16cec..a691aab58 100644
--- a/flight/Modules/Attitude/attitude.c
+++ b/flight/Modules/Attitude/attitude.c
@@ -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;
diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml
index ca0ba9dc5..ef50e0cee 100644
--- a/shared/uavobjectdefinition/attitudesettings.xml
+++ b/shared/uavobjectdefinition/attitudesettings.xml
@@ -7,6 +7,7 @@
+