Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 30 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_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_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_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_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.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.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.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>
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
#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);