Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[ahrs] int_cmpl_quat: physical units

Browse Source 
- accel and mag cutt-off frequency and damping in physical units
- accel weight sent to telemetry
This commit is contained in:
Loic Drumettaz
2013-03-28 16:17:57 +01:00
committed by Felix Ruess
parent f7dc61a764
commit f39cc5c25b
5 changed files with 105 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+1
View File
@@ -1295,6 +1295,7 @@
</message> </message>
<message name="AHRS_QUAT_INT" id="157"> <message name="AHRS_QUAT_INT" id="157">
<field name="weight" type="float"/>
<field name="imu_qi" type="int32" alt_unit="" alt_unit_coef="0.0000305"/> <field name="imu_qi" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
<field name="imu_qx" type="int32" alt_unit="" alt_unit_coef="0.0000305"/> <field name="imu_qx" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
<field name="imu_qy" type="int32" alt_unit="" alt_unit_coef="0.0000305"/> <field name="imu_qy" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
conf/settings/estimation/ahrs_int_cmpl_quat.xml
+4 -4
View File
@@ -5,10 +5,10 @@
<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_inv_omega" min="4" step="4" max="128" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_inv_omega" param="AHRS_ACCEL_INV_OMEGA"/> <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_inv_zeta" min="16" step="4" max="128" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_inv_zeta" param="AHRS_ACCEL_INV_ZETA"/> <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.mag_attitude_gain" min="1" step="1" max="100" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_att_gain" param="AHRS_MAG_GYROBIAS_GAIN"/> <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_gyrobias_gain" min="1" step="1" max="100" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_bias_gain" param="AHRS_MAG_GYROBIAS_GAIN"/> <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_settings> </dl_settings>
</dl_settings> </dl_settings>
sw/airborne/firmwares/rotorcraft/telemetry.h
+1
View File
@@ -454,6 +454,7 @@
#define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \ #define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \
DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \ DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \
&ahrs_impl.weight, \
&ahrs_impl.ltp_to_imu_quat.qi, \ &ahrs_impl.ltp_to_imu_quat.qi, \
&ahrs_impl.ltp_to_imu_quat.qx, \ &ahrs_impl.ltp_to_imu_quat.qx, \
&ahrs_impl.ltp_to_imu_quat.qy, \ &ahrs_impl.ltp_to_imu_quat.qy, \
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+94 -63
View File
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) 2008-2012 The Paparazzi Team * Copyright (C) 2008-2013 The Paparazzi Team
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -71,19 +71,19 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
/* /*
* default gains for correcting attitude and bias from accel/mag * default gains for correcting attitude and bias from accel/mag
*/ */
#ifndef AHRS_ACCEL_INV_OMEGA #ifndef AHRS_ACCEL_OMEGA
#define AHRS_ACCEL_INV_OMEGA 32 #define AHRS_ACCEL_OMEGA 0.063
#endif #endif
#ifndef AHRS_ACCEL_INV_ZETA #ifndef AHRS_ACCEL_ZETA
#define AHRS_ACCEL_INV_ZETA 64 #define AHRS_ACCEL_ZETA 1.
#endif
#ifndef AHRS_MAG_ATTITUDE_GAIN
#define AHRS_MAG_ATTITUDE_GAIN 32
#endif
#ifndef AHRS_MAG_GYROBIAS_GAIN
#define AHRS_MAG_GYROBIAS_GAIN 32
#endif #endif
#ifndef AHRS_MAG_OMEGA
#define AHRS_MAG_OMEGA 0.063
#endif
#ifndef AHRS_MAG_ZETA
#define AHRS_MAG_ZETA 1.
#endif
/** by default use the gravity heursitic to reduce gain */ /** by default use the gravity heursitic to reduce gain */
#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC #ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
@@ -124,11 +124,14 @@ void ahrs_init(void) {
INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias); INT_RATES_ZERO(ahrs_impl.high_rez_bias);
/* set default correction gains */ /* set default filter cut-off frequency and damping */
ahrs_impl.accel_inv_omega = AHRS_ACCEL_INV_OMEGA; ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_inv_zeta = AHRS_ACCEL_INV_ZETA; ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_impl.mag_attitude_gain = AHRS_MAG_ATTITUDE_GAIN; ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_gyrobias_gain = AHRS_MAG_GYROBIAS_GAIN; ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
/* set default gravity heuristic */
ahrs_impl.weight = 1.0;
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
ahrs_impl.correct_gravity = TRUE; ahrs_impl.correct_gravity = TRUE;
@@ -233,7 +236,7 @@ void ahrs_update_accel(void) {
struct Int32Vect3 acc_c_imu; struct Int32Vect3 acc_c_imu;
INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
} else { } else {
VECT3_COPY(pseudo_gravity_measurement, imu.accel); VECT3_COPY(pseudo_gravity_measurement, imu.accel);
@@ -251,11 +254,10 @@ void ahrs_update_accel(void) {
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
float weight = 1.0;
if (ahrs_impl.use_gravity_heuristic) { if (ahrs_impl.use_gravity_heuristic) {
/* heuristic on acceleration (gravity estimate) norm */ /* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
/* Factor how strongly to change the weight.
* e.g. for WEIGHT_FACTOR 3: * e.g. for WEIGHT_FACTOR 3:
* <0.66G = 0, 1G = 1.0, >1.33G = 0 * <0.66G = 0, 1G = 1.0, >1.33G = 0
*/ */
@@ -264,20 +266,23 @@ void ahrs_update_accel(void) {
struct FloatVect3 g_meas_f; struct FloatVect3 g_meas_f;
ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement);
const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81;
weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm);
Bound(weight, 0.15, 1.0); Bound(ahrs_impl.weight, 0.15, 1.0);
} }
/* Complementary filter proportionnal gain. /* Complementary filter proportional gain.
* Kp = 1/(2 * zeta * omega) * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
* 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
* *
* Kp = 1 / inv_rate_scale * 4096 = 1/(2*45*32*500/32) * 4096 = 0.091 * Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC
* zeta = Kp/2/SQRT(Ki analog) = 0.73 * 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 )
*/ */
int32_t inv_rate_scale = 2 * ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_zeta * AHRS_CORRECT_FREQUENCY / (32 * weight);
int32_t inv_rate_scale = 8192 * 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;
@@ -286,22 +291,22 @@ void ahrs_update_accel(void) {
/* Complementary filter integral gain /* Complementary filter integral gain
* Correct the gyro bias. * Correct the gyro bias.
* Ki = 1/(omega*omega) * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
* 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
* Ki = 1/2^16 / inv_bias_gain * 32 = 1/2^16 / (500*32*32/8192) * 32 = 7.8e-6 * Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC
* Ki analog = 7.8e-6 * 500 = 3.9e-3 * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 4
* omega = SQRT(Ki analog)/(2*pi) = 0.01 Hz - T = 100 s. * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 512)
*/ */
int32_t inv_bias_gain = ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_omega * AHRS_CORRECT_FREQUENCY / (weight * weight * 8192);
int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 512);
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;
ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.r += (residual.z / inv_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);
} }
@@ -327,28 +332,41 @@ static inline void ahrs_update_mag_full(void) {
struct Int32Vect3 residual; struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
/* residual FRAC: 2 * MAG_FRAC = 22 /* Complementary filter proportionnal gain.
* Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
* 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
* *
* Scale with mag_attitude_gain * 2 for convenient range, * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC
* and with mag 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
* inv_rate_gain = 1024 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
*/ */
ahrs_impl.rate_correction.p += residual.x * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY);
ahrs_impl.rate_correction.q += residual.y * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY);
ahrs_impl.rate_correction.r += residual.z * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY);
/* residual FRAC: 2* MAG_FRAC = 22 int32_t inv_rate_gain = 2048 / (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;
ahrs_impl.rate_correction.r += residual.z / inv_rate_gain;
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
* 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
* *
* bias correction gain scale with 1/2^13 compared to attitude correction: * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY
* 2^18 / 2^11 = 32 * 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
*/ */
int32_t bias_scale = 32 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 65536;
ahrs_impl.high_rez_bias.p -= residual.x * bias_scale; ahrs_impl.high_rez_bias.p -= residual.x * bias_gain;
ahrs_impl.high_rez_bias.q -= residual.y * bias_scale; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain;
ahrs_impl.high_rez_bias.r -= residual.z * bias_scale; ahrs_impl.high_rez_bias.r -= residual.z * bias_gain;
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);
@@ -373,30 +391,43 @@ static inline void ahrs_update_mag_2d(void) {
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);
/* residual_imu FRAC = residual_ltp FRAC = 17 /* Complementary filter proportionnal gain.
* Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY/ AHRS_MAG_CORRECT_FREQUENCY
* 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
*
* 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 = 32 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY
* *
* Scale with mag_attitude_gain * 2 for convenient range,
* and with mag frequency.
*/ */
ahrs_impl.rate_correction.p += residual_imu.x * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY);
ahrs_impl.rate_correction.q += residual_imu.y * (int32_t)ahrs_impl.mag_attitude_gain / (16 * 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.r += residual_imu.z * (int32_t)ahrs_impl.mag_attitude_gain / (16 * 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;
/* residual_imu FRAC = residual_ltp FRAC = 17 /* Complementary filter integral gain
* Correct the gyro bias.
* Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
* 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
* *
* bias correction with 1/2^11 compared to attitude correction: * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY
* 2^23 / 2^11 = 4096 * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC
* also divide mag_gyrobias_gain by 4 for convenience range * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21
*/ */
int32_t bias_scale = 1024 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; 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_scale; ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_gain;
ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_scale; ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_gain;
ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_scale; ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_gain;
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);
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
+5 -4
View File
@@ -47,10 +47,11 @@ struct AhrsIntCmplQuat {
struct Int32Quat ltp_to_imu_quat; struct Int32Quat ltp_to_imu_quat;
struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry
struct Int32Vect3 mag_h; struct Int32Vect3 mag_h;
uint32_t accel_inv_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)
uint32_t accel_inv_zeta; ///< filter damping correcting the gyro-bias from accels (pseudo-gravity measurement) float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
uint32_t mag_attitude_gain; ///< gain for correcting the attitude (heading) from magnetometer float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer
uint32_t mag_gyrobias_gain; ///< gain for correcting the gyro bias from magnetometer float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer
float weight;
int32_t ltp_vel_norm; int32_t ltp_vel_norm;
bool_t ltp_vel_norm_valid; bool_t ltp_vel_norm_valid;
bool_t correct_gravity; bool_t correct_gravity;