From 98655aacc85b2bca6701a8f017e9c0d492b5a8f3 Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Thu, 10 May 2012 00:29:23 -0500
Subject: [PATCH] Fix initialization of the complimentary filter where it
 didn't get the right initial dT

---
 flight/Modules/Attitude/revolution/attitude.c | 24 ++++++++++---------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c
index 00bfe9a9d..019511702 100644
--- a/flight/Modules/Attitude/revolution/attitude.c
+++ b/flight/Modules/Attitude/revolution/attitude.c
@@ -269,24 +269,26 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 	FlightStatusData flightStatus;
 	FlightStatusGet(&flightStatus);
 	if(first_run) {
+		// To initialize we need a valid mag reading
+		if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
+			return -1;
+
 		AttitudeActualData attitudeActual;
 		AttitudeActualGet(&attitudeActual);
 		MagnetometerData magData;
 		MagnetometerGet(&magData);
 		init = 0;
-		float rpy[3];
-		float q[4];
-		rpy[0] = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
-		rpy[1] = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
-		rpy[2] = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
+		attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
+		attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
+		attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
 
-		RPY2Quaternion(rpy,q);
-		quat_copy(q, &attitudeActual.q1);
-		
-		// Convert into eueler degrees (makes assumptions about RPY order)
-		Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
+		RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
 		AttitudeActualSet(&attitudeActual);
 
+		timeval = PIOS_DELAY_GetRaw();
+
+		return 0;
+
 	}
 
 	if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
@@ -306,7 +308,7 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 		AttitudeSettingsGet(&attitudeSettings);
 		magKp = 0.01f;
 		init = 1;
-	}	
+	}
 
 	GyrosGet(&gyrosData);