removed redundant FP calcs for speed up.

added ability to alternate normalization and drift correction. Not fully implemented, needs further testing.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@2691 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_DCM/AP_DCM.cpp

@@ -48,8 +48,13 @@ AP_DCM::update_DCM(float _G_Dt)
 	_accel_vector 	= _imu->get_accel();			// Get current values for IMU sensors
 
 	matrix_update(_G_Dt); 	// Integrate the DCM matrix
+
+	//if(_toggle){
 		normalize();			// Normalize the DCM matrix
+	//}else{
 		drift_correction();		// Perform drift correction
+	//}
+	//_toggle = !_toggle;
 
 	euler_angles();			// Calculate pitch, roll, yaw for stabilization and navigation
 }
@@ -92,14 +97,20 @@ AP_DCM::matrix_update(float _G_Dt)
 	}
 
  #if OUTPUTMODE == 1
+ 	float tmp = _G_Dt * _omega.x;
+	update_matrix.b.z = -tmp; 		// -delta Theta x
+	update_matrix.c.y =  tmp; 		// delta Theta x
+
+ 	tmp = _G_Dt * _omega.y;
+	update_matrix.c.x = -tmp; 		// -delta Theta y
+	update_matrix.a.z =  tmp; 		// delta Theta y
+
+ 	tmp = _G_Dt * _omega.z;
+	update_matrix.b.x =  tmp; 		// delta Theta z
+	update_matrix.a.y = -tmp; 		// -delta Theta z
+
 	update_matrix.a.x = 0;
-	update_matrix.a.y = -_G_Dt * _omega.z; 		// -delta Theta z
-	update_matrix.a.z =  _G_Dt * _omega.y; 		// delta Theta y
-	update_matrix.b.x =  _G_Dt * _omega.z; 		// delta Theta z
 	update_matrix.b.y = 0;
-	update_matrix.b.z = -_G_Dt * _omega.x; 		// -delta Theta x
-	update_matrix.c.x = -_G_Dt * _omega.y; 		// -delta Theta y
-	update_matrix.c.y =  _G_Dt * _omega.x; 		// delta Theta x
 	update_matrix.c.z = 0;
  #else										// Uncorrected data (no drift correction)
 	update_matrix.a.x = 0;
@@ -115,7 +126,6 @@ AP_DCM::matrix_update(float _G_Dt)
 
 	temp_matrix = _dcm_matrix * update_matrix;
 	_dcm_matrix = _dcm_matrix + temp_matrix;		// Equation 17
-
 }
 
 

libraries/AP_DCM/AP_DCM.h

@@ -33,7 +33,8 @@ public:
 		_health(1.),
 		_kp_roll_pitch(0.05967),
 		_ki_roll_pitch(0.00001278),
-		_kp_yaw(0.8)
+		_kp_yaw(0.8),
+		_toggle(0)
 	{}
 
 	// Accessors
@@ -115,6 +116,7 @@ private:
 	float 		_course_over_ground_y; 		// Course overground Y axis
 	float		_health;
 	bool		_centripetal;
+	bool		_toggle;
 };
 
 #endif