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

View File

@ -97,10 +97,11 @@ AP_DCM::matrix_update(float _G_Dt)
(fabs(_gyro_vector.z) >= radians(300))) (fabs(_gyro_vector.z) >= radians(300)))
gyro_sat_count++; gyro_sat_count++;
if(_centrifugal){
_omega_integ_corr = _gyro_vector + _omega_I; // Used for centrep correction (theoretically better than _omega) _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 _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 #if OUTPUTMODE == 1
update_matrix.a.x = 0; update_matrix.a.x = 0;
@ -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 void
AP_DCM::normalize(void) AP_DCM::normalize(void)
{ {

View File

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