From c207afbfef8984bea6cd2c81d849d84335725cc7 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Wed, 31 Oct 2012 19:05:59 +0100 Subject: [PATCH 1/9] Implement smoothing filter for accelerometer data. --- flight/Modules/Attitude/attitude.c | 19 +++++++++++++++++-- .../uavobjectdefinition/attitudesettings.xml | 1 + 2 files changed, 18 insertions(+), 2 deletions(-) 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 @@ + From b7bdf9861db1ae257f97a266883a796a8ece268b Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Thu, 1 Nov 2012 12:14:33 +0100 Subject: [PATCH 2/9] Run rotated gravity vector through smoothing filter to match phase with filtered accelerometer data. --- flight/Modules/Attitude/attitude.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index a691aab58..c5673a50a 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -444,7 +444,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) float * gyros = &gyrosData->x; float * accels = &accelsData->x; - float grot[3]; + static float grot[3]; float accel_err[3]; // Apply smoothing to accel values, to reduce vibration noise before main calculations. @@ -455,9 +455,9 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) 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]); + grot[0] = grot[0] * accel_alpha - (2 * (q[1] * q[3] - q[0] * q[2])) * (1 - accel_alpha); + grot[1] = grot[1] * accel_alpha - (2 * (q[2] * q[3] + q[0] * q[1])) * (1 - accel_alpha); + grot[2] = grot[2] * accel_alpha - (q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) * (1 - accel_alpha); CrossProduct((const float *) accels_filtered, (const float *) grot, accel_err); // Account for accel magnitude From f63db712d05b4521845e4ebe385f905cd16b2671 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Fri, 2 Nov 2012 09:12:24 +0100 Subject: [PATCH 3/9] Disable accel smoothing during init/arming so it does not interfere --- flight/Modules/Attitude/attitude.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index c5673a50a..0855f1767 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -87,6 +87,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv); static float accelKi = 0; static float accelKp = 0; static float accel_alpha = 0; +static float accel_alpha_setting = 0; static float yawBiasRate = 0; static float gyroGain = 0.42; static int16_t accelbias[3]; @@ -216,18 +217,21 @@ static void AttitudeTask(void *parameters) accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; + accel_alpha = 0; init = 0; } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; + accel_alpha = 0; init = 0; } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsYawBiasRateGet(&yawBiasRate); + accel_alpha = accel_alpha_setting; init = 1; } @@ -543,10 +547,12 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) { // 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 + accel_alpha_setting = 0; // not trusting this to resolve to 0 else - accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); + accel_alpha_setting = expf(-fakeDt / attitudeSettings.AccelTau); + accel_alpha = accel_alpha_setting; + zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; From 677f921b15df0ddb1fcbb9df8cb05d6c94ba45db Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Fri, 2 Nov 2012 09:22:18 +0100 Subject: [PATCH 4/9] Take magnitude of filtered gravity vector into account. --- flight/Modules/Attitude/attitude.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 0855f1767..c5299c8f5 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -469,9 +469,14 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) if(accel_mag < 1.0e-3f) return; - accel_err[0] /= accel_mag; - accel_err[1] /= accel_mag; - accel_err[2] /= accel_mag; + // Account for filtered gravity vector magnitude + float grot_mag = sqrtf(grot[0]*grot[0] + grot[1]*grot[1] + grot[2]*grot[2]); + if(grot_mag < 1.0e-3f) + return; + + accel_err[0] /= (accel_mag*grot_mag); + accel_err[1] /= (accel_mag*grot_mag); + accel_err[2] /= (accel_mag*grot_mag); // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s gyro_correct_int[0] += accel_err[0] * accelKi; From 104c61a17431c13d8904913274395fe9d50be379 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Tue, 6 Nov 2012 20:17:10 +0100 Subject: [PATCH 5/9] Move accel filter to separate function, for cleaner code and easier filter alteration. Optimize for filter disabled case (AccelTau = 0.0). --- flight/Modules/Attitude/attitude.c | 60 +++++++++++++++++++----------- 1 file changed, 39 insertions(+), 21 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index c5299c8f5..2d267883b 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -87,7 +87,9 @@ static void settingsUpdatedCb(UAVObjEvent * objEv); static float accelKi = 0; static float accelKp = 0; static float accel_alpha = 0; -static float accel_alpha_setting = 0; +static bool accel_filter_enabled = false; +static float accels_filtered[3]; +static float grot_filtered[3]; static float yawBiasRate = 0; static float gyroGain = 0.42; static int16_t accelbias[3]; @@ -217,21 +219,22 @@ static void AttitudeTask(void *parameters) accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; - accel_alpha = 0; + accel_filter_enabled = false; init = 0; } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; - accel_alpha = 0; + accel_filter_enabled = false; init = 0; } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsYawBiasRateGet(&yawBiasRate); - accel_alpha = accel_alpha_setting; + if(accel_alpha > 0.0f) + accel_filter_enabled = true; init = 1; } @@ -435,6 +438,19 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) return 0; } +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]; + } +} + static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) { float dT; @@ -448,21 +464,20 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) float * gyros = &gyrosData->x; float * accels = &accelsData->x; - static float grot[3]; + float grot[3]; float accel_err[3]; // Apply smoothing to accel values, to reduce vibration noise before main calculations. - static float accels_filtered[3]; + apply_accel_filter(accels,accels_filtered); - 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, filter 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]); + + apply_accel_filter(grot,grot_filtered); - // Rotate gravity to body frame and cross with accels - grot[0] = grot[0] * accel_alpha - (2 * (q[1] * q[3] - q[0] * q[2])) * (1 - accel_alpha); - grot[1] = grot[1] * accel_alpha - (2 * (q[2] * q[3] + q[0] * q[1])) * (1 - accel_alpha); - grot[2] = grot[2] * accel_alpha - (q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) * (1 - accel_alpha); - CrossProduct((const float *) accels_filtered, (const float *) grot, accel_err); + CrossProduct((const float *) accels_filtered, (const float *) grot_filtered, accel_err); // Account for accel magnitude float accel_mag = sqrtf(accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]); @@ -470,7 +485,9 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) return; // Account for filtered gravity vector magnitude - float grot_mag = sqrtf(grot[0]*grot[0] + grot[1]*grot[1] + grot[2]*grot[2]); + float grot_mag; + if(accel_filter_enabled) grot_mag = sqrtf(grot[0]*grot[0] + grot[1]*grot[1] + grot[2]*grot[2]); + else grot_mag = 1.0f; if(grot_mag < 1.0e-3f) return; @@ -551,13 +568,14 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) { // 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_setting = 0; // not trusting this to resolve to 0 - else - accel_alpha_setting = expf(-fakeDt / attitudeSettings.AccelTau); + if(attitudeSettings.AccelTau < 0.0001) { + 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; + } - accel_alpha = accel_alpha_setting; - zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; From 04c194fa48ff6fb0c8fbfbe4de893886fc375fa8 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Sun, 11 Nov 2012 21:10:52 +0100 Subject: [PATCH 6/9] Coding style fixes --- flight/Modules/Attitude/attitude.c | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 2d267883b..2449dbb63 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -233,7 +233,7 @@ static void AttitudeTask(void *parameters) AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsYawBiasRateGet(&yawBiasRate); - if(accel_alpha > 0.0f) + if (accel_alpha > 0.0f) accel_filter_enabled = true; init = 1; } @@ -438,9 +438,9 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) return 0; } -static inline void apply_accel_filter(const float * raw, float * filtered) +static inline void apply_accel_filter(const float *raw, float *filtered) { - if(accel_filter_enabled) { + 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); @@ -468,27 +468,31 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) float accel_err[3]; // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter(accels,accels_filtered); + apply_accel_filter(accels, accels_filtered); // Rotate gravity to body frame, filter 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]); - apply_accel_filter(grot,grot_filtered); + apply_accel_filter(grot, grot_filtered); - CrossProduct((const float *) accels_filtered, (const float *) grot_filtered, accel_err); + CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); // Account for accel magnitude 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) + if (accel_mag < 1.0e-3f) return; // Account for filtered gravity vector magnitude float grot_mag; - if(accel_filter_enabled) grot_mag = sqrtf(grot[0]*grot[0] + grot[1]*grot[1] + grot[2]*grot[2]); - else grot_mag = 1.0f; - if(grot_mag < 1.0e-3f) + + if (accel_filter_enabled) + grot_mag = sqrtf(grot[0]*grot[0] + grot[1]*grot[1] + grot[2]*grot[2]); + else + grot_mag = 1.0f; + + if (grot_mag < 1.0e-3f) return; accel_err[0] /= (accel_mag*grot_mag); @@ -568,7 +572,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) { // 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) { + if (attitudeSettings.AccelTau < 0.0001) { accel_alpha = 0; // not trusting this to resolve to 0 accel_filter_enabled = false; } else { From e38ba912cd4287771ea0b46a8e0ed87272e52126 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Sun, 11 Nov 2012 21:13:05 +0100 Subject: [PATCH 7/9] Clarify comment about gravity vector --- flight/Modules/Attitude/attitude.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 2449dbb63..e71c96dbe 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -470,7 +470,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) // Apply smoothing to accel values, to reduce vibration noise before main calculations. apply_accel_filter(accels, accels_filtered); - // Rotate gravity to body frame, filter and cross with accels + // Rotate gravity unit vector to body frame, filter 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]); From e186101fe767067871c03639c3ff3afe2f795895 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Sun, 11 Nov 2012 21:16:48 +0100 Subject: [PATCH 8/9] Change default AccelTau value to zero (filter disabled) --- shared/uavobjectdefinition/attitudesettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index ef50e0cee..274a2ef30 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -7,7 +7,7 @@ - + From f9f58f22d839e106e3fb85334f1195bcda2a66a4 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Mon, 12 Nov 2012 17:16:28 +0100 Subject: [PATCH 9/9] Take the magnitude of the correct gravity vector --- flight/Modules/Attitude/attitude.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index e71c96dbe..8fd0f94ed 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -488,7 +488,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) float grot_mag; if (accel_filter_enabled) - grot_mag = sqrtf(grot[0]*grot[0] + grot[1]*grot[1] + grot[2]*grot[2]); + 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;