From 347d69d63f7cd31ea79a4a54434722ffbfe2a760 Mon Sep 17 00:00:00 2001 From: Loic Drumettaz Date: Sat, 13 Apr 2013 10:35:07 +0200 Subject: [PATCH] [ahrs] int_cmpl_quat: feedback_frac corrected - feedback_frac for both accel and mag correction - omega and zeta nominal and min/max settings adjusted --- .../estimation/ahrs_int_cmpl_quat.xml | 6 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 66 +++++++++---------- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 9856758ec1..b53f9e04e6 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -6,9 +6,9 @@ - - - + + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 1947911e42..1d261502a8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -75,14 +75,14 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #define AHRS_ACCEL_OMEGA 0.063 #endif #ifndef AHRS_ACCEL_ZETA -#define AHRS_ACCEL_ZETA 1. +#define AHRS_ACCEL_ZETA 0.9 #endif #ifndef AHRS_MAG_OMEGA -#define AHRS_MAG_OMEGA 0.063 +#define AHRS_MAG_OMEGA 0.04 #endif #ifndef AHRS_MAG_ZETA -#define AHRS_MAG_ZETA 1. +#define AHRS_MAG_ZETA 0.9 #endif /** by default use the gravity heursitic to reduce gain */ @@ -275,14 +275,14 @@ void ahrs_update_accel(void) { * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 * FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 - * Feedback_FRAC: 4 + * Feedback_FRAC: 8 * * Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC - * inv_rate_scale= 4096 * 4 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) - * inv_rate_scale= 8192 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) + * inv_rate_scale= 4096 * 8 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) + * inv_rate_scale= 16384 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) */ - int32_t inv_rate_scale = 8192 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); + int32_t inv_rate_scale = 16384 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; @@ -295,13 +295,13 @@ void ahrs_update_accel(void) { * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 * FRAC_conversion: 2^40 / 2^24 = 2^16 - * Feedback_FRAC: 4 + * Feedback_FRAC: 8 * Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC - * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 4 - * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 512) + * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 8 + * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 256) */ - int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 512); + int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 256); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; @@ -337,15 +337,15 @@ static inline void ahrs_update_mag_full(void) { * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY * / FRAC_conversion * Feedback_FRAC - * inv_rate_gain = 1024 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 1024 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ - int32_t inv_rate_gain = 2048 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; @@ -357,13 +357,13 @@ static inline void ahrs_update_mag_full(void) { * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^16 + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 */ - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 65536; + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; @@ -388,6 +388,7 @@ static inline void ahrs_update_mag_2d(void) { 0, (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)}; + struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); @@ -396,21 +397,19 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 - * Feedback_FRAC: 4 - * - * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * Feedback_FRAC: 1/8 + * Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5 * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY - * / FRAC_conversion * Feedback_FRAC - * inv_rate_gain = 32 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY - * + * / FRAC_conversion * Feedback_FRAC * 2^5 + * inv_rate_gain = 32 * 32 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.rate_correction.p += residual_imu.x / inv_rate_gain; - ahrs_impl.rate_correction.q += residual_imu.y / inv_rate_gain; - ahrs_impl.rate_correction.r += residual_imu.z / inv_rate_gain; - + ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain) << 5; + ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain) << 5; + ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain) << 5; /* Complementary filter integral gain * Correct the gyro bias. @@ -418,16 +417,17 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * - * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC + * Ki = bias_gain / FRAC_conversion * Feedback_frac * 2^5= ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion/Feedback_frac * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 */ + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; - ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_gain; - ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_gain; - ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_gain; + ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain) << 5; + ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain) << 5; + ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain) << 5; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);