Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Linked update and bias gain (#2749)

Browse Source
This commit is contained in:
Matteo Barbera
2021-07-13 16:16:02 +02:00
committed by GitHub
parent b24389655d
commit 36183eea19
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+2 -2
View File
@@ -390,7 +390,7 @@ void ahrs_fc_update_mag_2d_dumb(struct FloatVect3 *mag)
}; };
const float mag_rate_update_gain = 2.5; const float mag_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm)); RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
const float mag_bias_update_gain = -2.5e-4; const float mag_bias_update_gain = -mag_rate_update_gain * 1e-4;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm)); RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm));
} }
@@ -462,7 +462,7 @@ void ahrs_fc_update_heading(float heading)
* Otherwise the bias will be falsely "corrected". * Otherwise the bias will be falsely "corrected".
*/ */
if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) { if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) {
heading_bias_update_gain = -2.5e-4; heading_bias_update_gain = -heading_rate_update_gain * 1e-4;
} else { } else {
heading_bias_update_gain = 0.0; heading_bias_update_gain = 0.0;
} }