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

Port CF patch from Cyr that filters accel to reduce attitude drift due to vibrations

Browse Source
This commit is contained in:
Alessio Morale 2013-05-22 00:55:10 +02:00
parent 0b5a28f19e
commit 294a295510
1 changed files with 61 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
flight/modules/Attitude/revolution/attitude.c
View File

@ -114,6 +114,13 @@ static bool volatile variance_error = true;
static bool volatile initialization_required = true;
static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
static float rollPitchBiasRate = 0;
// Accel filtering
static float accel_alpha = 0;
static bool accel_filter_enabled = false;
static float accels_filtered[3];
static float grot_filtered[3];
// Private functions
static void AttitudeTask(void *parameters);
@ -285,6 +292,19 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
}
}
static inline void apply_accel_filter(const float *raw, float *filtered)
{
if (accel_filter_enabled) {
filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha);
filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha);
filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha);
} else {
filtered[0] = raw[0];
filtered[1] = raw[1];
filtered[2] = raw[2];
}
}
float accel_mag;
float qmag;
float attitudeDt;
@ -367,12 +387,14 @@ static int32_t updateAttitudeComplementary(bool first_run)
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f;
init = 0;
@ -380,6 +402,8 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
rollPitchBiasRate = 0.0f;
if (accel_alpha > 0.0f)
accel_filter_enabled = true;
init = 1;
}
@ -400,19 +424,43 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Get the current attitude estimate
quat_copy(&attitudeActual.q1, q);
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter((const float *)&accelsData.x, accels_filtered);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2.0f * (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 *)&accelsData.x, (const float *)grot, accel_err);
apply_accel_filter(grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
accel_mag = accelsData.x * accelsData.x + accelsData.y * accelsData.y + accelsData.z * accelsData.z;
accel_mag = accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2];
accel_mag = sqrtf(accel_mag);
//TODO! check accel vector magnitude value for correctness
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
float grot_mag;
if (accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
}
// TODO! check grot_mag value for correctness.
accel_err[0] /= (accel_mag * grot_mag);
accel_err[1] /= (accel_mag * grot_mag);
accel_err[2] /= (accel_mag * grot_mag);
if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) {
// Rotate gravity to body frame and cross with accels
float brot[3];
@ -451,7 +499,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
if(revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + (gyrosData.x + gyrosBias.x) * rollPitchBiasRate;;
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + (gyrosData.x + gyrosBias.x) * rollPitchBiasRate;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi + (gyrosData.y + gyrosBias.y) * rollPitchBiasRate;
} else {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + gyrosData.x * rollPitchBiasRate;;
@ -1133,6 +1181,16 @@ static void settingsUpdatedCb(UAVObjEvent *ev)
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle()) {
AttitudeSettingsGet(&attitudeSettings);
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025f;
if (attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false;
} else {
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
accel_filter_enabled = true;
}
}
}
/**