Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

boolean to disable centrifugal correction. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@987 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-01 19:53:40 +00:00
parent d209f3d59e
commit 275624358d
2 changed files with 20 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_DCM/AP_DCM.cpp
View File

@ -93,15 +93,16 @@ AP_DCM::matrix_update(float _G_Dt)
//Record when you saturate any of the gyros.
if((fabs(_gyro_vector.x) >= radians(300)) ||
(fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300)))
(fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300)))
gyro_sat_count++;
_omega_integ_corr = _gyro_vector + _omega_I; // Used for centrep correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centrifugal){
_omega_integ_corr = _gyro_vector + _omega_I; // Used for centrep correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
accel_adjust(); // Remove centrifugal acceleration.
}
accel_adjust(); // Remove centrifugal acceleration.
#if OUTPUTMODE == 1
update_matrix.a.x = 0;
update_matrix.a.y = -_G_Dt * _omega.z; // -delta Theta z
@ -150,7 +151,16 @@ AP_DCM::accel_adjust(void)
}
/**************************************************/
/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard
Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
to approximations rather than identities. In effect, the axes in the two frames of reference no
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
*/
void
AP_DCM::normalize(void)
{

4
libraries/AP_DCM/AP_DCM.h
View File

@ -42,6 +42,8 @@ public:
Vector3f get_accel(void);
Matrix3f get_dcm_matrix(void);
void set_centrifugal(bool b);
// Methods
void update_DCM(float _G_Dt);
@ -86,7 +88,7 @@ private:
float _errorCourse;
float _course_over_ground_x; // Course overground X axis
float _course_over_ground_y; // Course overground Y axis
bool _centrifugal;
};
#endif