From ddbd4f8c807f8cff1352b83555e5c174ec915301 Mon Sep 17 00:00:00 2001
From: peabody124 <peabody124>
Date: Sun, 15 Aug 2010 06:03:44 +0000
Subject: [PATCH] Simple attitude calculation based on gravity and magnetic
 vector

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1298 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/AHRS/ahrs.c                            | 38 +++++++++----------
 .../OpenPilotOSX.xcodeproj/project.pbxproj    |  4 --
 2 files changed, 19 insertions(+), 23 deletions(-)

diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 5e35c4233..0f59d57a1 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -230,13 +230,9 @@ int main()
   
   // Main loop
   while (1) {
-    uint8_t loop_ctr;
 
     // Alive signal
-    if (loop_ctr++ > 100) {
-      PIOS_LED_Toggle(LED1);
-      loop_ctr = 0;
-    }
+    PIOS_LED_Toggle(LED1);
     
     // Get 3 ID bytes
     strcpy ((char *)mag_data.id, "ZZZ");
@@ -266,23 +262,27 @@ int main()
     
     ahrs_state = AHRS_IDLE;
     
-    /* Simulate a rotating airframe */
-    attitude_data.quaternion.q1 += .001;
-    attitude_data.quaternion.q2 += .002;
-    attitude_data.quaternion.q3 += .003;
-    attitude_data.quaternion.q4 += 1;
-
-    attitude_data.euler.roll  += .004;
-    if (attitude_data.euler.roll > 360.0) attitude_data.euler.roll -= 360.0;
-    attitude_data.euler.pitch += .005;
-    if (attitude_data.euler.pitch > 360.0) attitude_data.euler.pitch -= 360.0;
-    attitude_data.euler.yaw   += .006;
-    if (attitude_data.euler.yaw > 360.0) attitude_data.euler.yaw -= 360.0;
+    /* Very simple computation of the heading and attitude from accel. */
+    attitude_data.euler.yaw = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI;
+    attitude_data.euler.pitch = atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI;
+    attitude_data.euler.roll = -atan2(accel_data.filtered.x,accel_data.filtered.z) * 180 / M_PI;
+    if (attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360.0;
+    
+    float c1 = cos(attitude_data.euler.yaw/2);
+    float s1 = sin(attitude_data.euler.yaw/2);
+    float c2 = cos(attitude_data.euler.pitch/2);
+    float s2 = sin(attitude_data.euler.pitch/2);
+    float c3 = cos(attitude_data.euler.roll/2);
+    float s3 = sin(attitude_data.euler.roll/2);
+    float c1c2 = c1*c2;
+    float s1s2 = s1*s2;
+    attitude_data.quaternion.q1 = c1c2*c3 - s1s2*s3;
+    attitude_data.quaternion.q2 = c1c2*s3 + s1s2*c3;
+    attitude_data.quaternion.q3 = s1*c2*c3 + c1*s2*s3;
+    attitude_data.quaternion.q4 =c1*s2*c3 - s1*c2*s3;
     
     process_spi_request();
     
-    // Delay until next reading
-    //PIOS_DELAY_WaitmS(50);
   }
   
   return 0;
diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
index 39eaa61fa..c7c4b6c01 100644
--- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
+++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
@@ -61,10 +61,8 @@
 		65B7E6AE120DF1E2000C1123 /* ahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs.c; path = ../../AHRS/ahrs.c; sourceTree = SOURCE_ROOT; };
 		65B7E6B0120DF1E2000C1123 /* ahrs.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs.h; sourceTree = "<group>"; };
 		65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_fsm.h; sourceTree = "<group>"; };
-		65B7E6B2120DF1E2000C1123 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
 		65B7E6B3120DF1E2000C1123 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = "<group>"; };
 		65B7E6B4120DF1E2000C1123 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_config.h; sourceTree = "<group>"; };
-		65B7E6B5120DF1E2000C1123 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
 		65B7E6B6120DF1E2000C1123 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../AHRS/Makefile; sourceTree = SOURCE_ROOT; };
 		65B7E6B7120DF1E2000C1123 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board.c; path = ../../AHRS/pios_board.c; sourceTree = SOURCE_ROOT; };
 		65E8EF1F11EEA61E00BBF654 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../OpenPilot/Makefile; sourceTree = SOURCE_ROOT; };
@@ -523,7 +521,6 @@
 				65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */,
 				65B7E6AE120DF1E2000C1123 /* ahrs.c */,
 				65B7E6AF120DF1E2000C1123 /* inc */,
-				65B7E6B5120DF1E2000C1123 /* insgps.c */,
 				65B7E6B6120DF1E2000C1123 /* Makefile */,
 				65B7E6B7120DF1E2000C1123 /* pios_board.c */,
 			);
@@ -535,7 +532,6 @@
 			children = (
 				65B7E6B0120DF1E2000C1123 /* ahrs.h */,
 				65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,
-				65B7E6B2120DF1E2000C1123 /* insgps.h */,
 				65B7E6B3120DF1E2000C1123 /* pios_board.h */,
 				65B7E6B4120DF1E2000C1123 /* pios_config.h */,
 			);