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

@@ -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.y) >= radians(300)) || 
-	   (fabs(_gyro_vector.z) >= 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)
+	if(_centrifugal){
-	_omega 				= _omega_integ_corr + _omega_P;		// Equation 16, adding proportional and integral correction terms
+		_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)
 {

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