Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 18:07:25 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[ahrs] int_cmpl_quat: feedback_frac corrected

Browse Source 
- feedback_frac for both accel and mag correction
- omega and zeta  nominal and min/max settings adjusted
This commit is contained in:
Loic Drumettaz
2013-04-13 10:35:07 +02:00
committed by Felix Ruess
parent 2b712a1b51
commit 347d69d63f
2 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/estimation/ahrs_int_cmpl_quat.xml
+3 -3
View File
@@ -6,9 +6,9 @@
<dl_settings NAME="AHRS"> <dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.use_gravity_heuristic" min="0" step="1" max="1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="gravity_heuristic" values="OFF|ON" param="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC"/> <dl_setting var="ahrs_impl.use_gravity_heuristic" min="0" step="1" max="1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="gravity_heuristic" values="OFF|ON" param="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC"/>
<dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/> <dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_impl.accel_zeta" min="0.5" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/> <dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/>
<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA"/> <dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_impl.mag_zeta" min="0.5" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA"/> <dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+33 -33
View File
@@ -75,14 +75,14 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
#define AHRS_ACCEL_OMEGA 0.063 #define AHRS_ACCEL_OMEGA 0.063
#endif #endif
#ifndef AHRS_ACCEL_ZETA #ifndef AHRS_ACCEL_ZETA
#define AHRS_ACCEL_ZETA 1. #define AHRS_ACCEL_ZETA 0.9
#endif #endif
#ifndef AHRS_MAG_OMEGA #ifndef AHRS_MAG_OMEGA
#define AHRS_MAG_OMEGA 0.063 #define AHRS_MAG_OMEGA 0.04
#endif #endif
#ifndef AHRS_MAG_ZETA #ifndef AHRS_MAG_ZETA
#define AHRS_MAG_ZETA 1. #define AHRS_MAG_ZETA 0.9
#endif #endif
/** by default use the gravity heursitic to reduce gain */ /** 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 * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
* rate_correction FRAC: RATE_FRAC = 12 * rate_correction FRAC: RATE_FRAC = 12
* FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 * 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 * 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= 4096 * 8 / (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= 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); Bound(inv_rate_scale, 8192, 4194304);
ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale;
ahrs_impl.rate_correction.q -= residual.y / 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 * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
* high_rez_bias = RATE_FRAC+28 = 40 * high_rez_bias = RATE_FRAC+28 = 40
* FRAC_conversion: 2^40 / 2^24 = 2^16 * 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 * 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 = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 8
* inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 512) * 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) Bound(inv_bias_gain, 8, 65536)
ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5;
ahrs_impl.high_rez_bias.q += (residual.y / 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 * residual FRAC: 2 * MAG_FRAC = 22
* rate_correction FRAC: RATE_FRAC = 12 * rate_correction FRAC: RATE_FRAC = 12
* FRAC conversion: 2^12 / 2^22 = 1/1024 * FRAC conversion: 2^12 / 2^22 = 1/1024
* Feedback_FRAC: 4 * Feedback_FRAC: 1/8
* *
* Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC * 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 * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
* / FRAC_conversion * Feedback_FRAC * / 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.p += residual.x / inv_rate_gain;
ahrs_impl.rate_correction.q += residual.y / 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 * residual FRAC: 2* MAG_FRAC = 22
* high_rez_bias FRAC: RATE_FRAC+28 = 40 * high_rez_bias FRAC: RATE_FRAC+28 = 40
* FRAC conversion: 2^40 / 2^22 = 2^18 * 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 * 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 * 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.p -= residual.x * bias_gain;
ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain;
ahrs_impl.high_rez_bias.r -= residual.z * 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, 0,
(measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)}; (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)};
struct Int32Vect3 residual_imu; struct Int32Vect3 residual_imu;
INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); 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 * residual_imu FRAC = residual_ltp FRAC = 17
* rate_correction FRAC: RATE_FRAC = 12 * rate_correction FRAC: RATE_FRAC = 12
* FRAC conversion: 2^12 / 2^17 = 1/32 * FRAC conversion: 2^12 / 2^17 = 1/32
* Feedback_FRAC: 4 * Feedback_FRAC: 1/8
* * Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5
* 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 * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY
* / FRAC_conversion * Feedback_FRAC * / FRAC_conversion * Feedback_FRAC * 2^5
* inv_rate_gain = 32 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY * 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; 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.p += (residual_imu.x / inv_rate_gain) << 5;
ahrs_impl.rate_correction.q += residual_imu.y / inv_rate_gain; ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain) << 5;
ahrs_impl.rate_correction.r += residual_imu.z / inv_rate_gain; ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain) << 5;
/* Complementary filter integral gain /* Complementary filter integral gain
* Correct the gyro bias. * Correct the gyro bias.
@@ -418,16 +417,17 @@ static inline void ahrs_update_mag_2d(void) {
* residual_imu FRAC = residual_ltp FRAC = 17 * residual_imu FRAC = residual_ltp FRAC = 17
* high_rez_bias FRAC: RATE_FRAC+28 = 40 * high_rez_bias FRAC: RATE_FRAC+28 = 40
* FRAC conversion: 2^40 / 2^17 = 2^23 * 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 * 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 * FRAC_conversion/Feedback_frac
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 * 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; 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.p -= (residual_imu.x * bias_gain) << 5;
ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_gain; ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain) << 5;
ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_gain; 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); INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);