From e8fcf7296677ac86ed7617a305eec79eed1b6980 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@gmail.com>
Date: Sun, 11 Sep 2011 11:03:55 -0700
Subject: [PATCH] Added a state machine intended to run at 250hz to the DCM.
 Spread load of DCM out to lower CPU.

---
 libraries/AP_DCM/AP_DCM.cpp | 71 ++++++++++++++++++++++++++++++++-----
 libraries/AP_DCM/AP_DCM.h   |  7 +++-
 2 files changed, 69 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp
index 6e297ca49e..b6dcaaf4b1 100644
--- a/libraries/AP_DCM/AP_DCM.cpp
+++ b/libraries/AP_DCM/AP_DCM.cpp
@@ -41,7 +41,7 @@ AP_DCM::set_compass(Compass *compass)
 
 /**************************************************/
 void
-AP_DCM::update_DCM(float _G_Dt)
+AP_DCM::update_DCM_fast(float _G_Dt)
 {
 	_imu->update();
 	_gyro_vector 	= _imu->get_gyro();			// Get current values for IMU sensors
@@ -49,13 +49,48 @@ AP_DCM::update_DCM(float _G_Dt)
 
 	matrix_update(_G_Dt); 	// Integrate the DCM matrix
 
-	//if(_toggle){
-		normalize();			// Normalize the DCM matrix
-	//}else{
-		drift_correction();		// Perform drift correction
-	//}
-	//_toggle = !_toggle;
+	switch(_toggle++){
+		case 0:
+			normalize();				// Normalize the DCM matrix
+		break;
 
+		case 1:
+			//drift_correction();			// Normalize the DCM matrix
+			euler_rp();			// Calculate pitch, roll, yaw for stabilization and navigation
+		break;
+
+		case 2:
+			drift_correction();			// Normalize the DCM matrix
+		break;
+
+		case 3:
+			//drift_correction();			// Normalize the DCM matrix
+			euler_rp();			// Calculate pitch, roll, yaw for stabilization and navigation
+		break;
+
+		case 4:
+			euler_yaw();
+		break;
+
+		default:
+			euler_rp();			// Calculate pitch, roll, yaw for stabilization and navigation
+			_toggle = 0;
+			//drift_correction();			// Normalize the DCM matrix
+		break;
+	}
+}
+
+/**************************************************/
+void
+AP_DCM::update_DCM(float _G_Dt)
+{
+	_imu->update();
+	_gyro_vector 	= _imu->get_gyro();			// Get current values for IMU sensors
+	_accel_vector 	= _imu->get_accel();			// Get current values for IMU sensors
+
+	matrix_update(_G_Dt); 	// Integrate the DCM matrix
+	normalize();			// Normalize the DCM matrix
+	drift_correction();		// Perform drift correction
 	euler_angles();			// Calculate pitch, roll, yaw for stabilization and navigation
 }
 
@@ -83,11 +118,12 @@ AP_DCM::matrix_update(float _G_Dt)
 	Matrix3f	temp_matrix;
 
 	//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))){
 		gyro_sat_count++;
-	}
+	}*/
 
 	_omega_integ_corr 	= _gyro_vector 		+ _omega_I;		// Used for _centripetal correction (theoretically better than _omega)
 	_omega 				= _omega_integ_corr + _omega_P;		// Equation 16, adding proportional and integral correction terms
@@ -335,6 +371,25 @@ AP_DCM::euler_angles(void)
 		yaw_sensor += 36000;
 }
 
+void
+AP_DCM::euler_rp(void)
+{
+	pitch 			= -asin(_dcm_matrix.c.x);
+	roll 			= atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
+	roll_sensor 	= degrees(roll)  * 100;
+	pitch_sensor 	= degrees(pitch) * 100;
+}
+
+void
+AP_DCM::euler_yaw(void)
+{
+	yaw 			= atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
+	yaw_sensor 		= degrees(yaw)   * 100;
+
+	if (yaw_sensor < 0)
+		yaw_sensor += 36000;
+}
+
 /**************************************************/
 
 float
diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h
index bd705da8f8..9a10eb03bc 100644
--- a/libraries/AP_DCM/AP_DCM.h
+++ b/libraries/AP_DCM/AP_DCM.h
@@ -50,6 +50,7 @@ public:
 
 	// Methods
 	void 		update_DCM(float _G_Dt);
+	void 		update_DCM_fast(float _G_Dt);
 
 	float		get_health(void);
 
@@ -98,6 +99,10 @@ private:
 	void 		drift_correction(void);
 	void 		euler_angles(void);
 
+	void 		euler_rp(void);
+	void 		euler_yaw(void);
+
+
 	// members
 	Compass 	* _compass;
 
@@ -122,7 +127,7 @@ private:
 	float 		_course_over_ground_y; 		// Course overground Y axis
 	float		_health;
 	bool		_centripetal;
-	bool		_toggle;
+	uint8_t		_toggle;
 };
 
 #endif