Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: don't add the P terms in _omega

Browse Source 
this can lead feedback via the _P_gain()
This commit is contained in:
Andrew Tridgell 2012-06-29 14:53:06 +10:00
parent dadc7d5a7d
commit 16c55c64d4
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -76,10 +76,14 @@ AP_AHRS_DCM::update(void)
void
AP_AHRS_DCM::matrix_update(float _G_Dt)
{
// Equation 16, adding proportional and integral correction terms
_omega = _gyro_vector + _omega_I + _omega_P + _omega_yaw_P;
// note that we do not include the P terms in _omega. This is
// because the spin_rate is calculated from _omega.length(),
// and including the P terms would give positive feedback into
// the _P_gain() calculation, which can lead to a very large P
// value
_omega = _gyro_vector + _omega_I;
_dcm_matrix.rotate(_omega * _G_Dt);
_dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
}