From 275624358ddf1dd91b72636d75d60e17213a05d7 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Wed, 1 Dec 2010 19:53:40 +0000 Subject: [PATCH] boolean to disable centrifugal correction. git-svn-id: https://arducopter.googlecode.com/svn/trunk@987 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_DCM/AP_DCM.cpp | 24 +++++++++++++++++------- libraries/AP_DCM/AP_DCM.h | 4 +++- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index db52269410..eb248f954b 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -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) { diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 2253b8434f..158f78904e 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -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