From 1bd9cdafab63be88d17230ab3bacb4ac1eec0f70 Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Mon, 20 May 2013 10:42:23 +0200
Subject: [PATCH] added missing attitude state variable ;)

---
 .../modules/StateEstimation/inc/stateestimation.h  | 10 ++++++----
 flight/modules/StateEstimation/stateestimation.c   | 14 +++++++++++++-
 2 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h
index dd6a3a157..84672d939 100644
--- a/flight/modules/StateEstimation/inc/stateestimation.h
+++ b/flight/modules/StateEstimation/inc/stateestimation.h
@@ -36,16 +36,18 @@ typedef enum {
     gyr_UPDATED     = 1 << 0,
         acc_UPDATED = 1 << 1,
         mag_UPDATED = 1 << 2,
-        pos_UPDATED = 1 << 3,
-        vel_UPDATED = 1 << 4,
-        air_UPDATED = 1 << 5,
-        bar_UPDATED = 1 << 6,
+        att_UPDATED = 1 << 3,
+        pos_UPDATED = 1 << 4,
+        vel_UPDATED = 1 << 5,
+        air_UPDATED = 1 << 6,
+        bar_UPDATED = 1 << 7,
 } sensorUpdates;
 
 typedef struct {
     float gyr[3];
     float acc[3];
     float mag[3];
+    float att[4];
     float pos[3];
     float vel[3];
     float air[2];
diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c
index 1ecdf7e1b..4968f2aba 100644
--- a/flight/modules/StateEstimation/stateestimation.c
+++ b/flight/modules/StateEstimation/stateestimation.c
@@ -44,6 +44,7 @@
 #include <magstate.h>
 #include <barostate.h>
 #include <airspeedstate.h>
+#include <attitudestate.h>
 #include <positionstate.h>
 #include <velocitystate.h>
 
@@ -51,7 +52,7 @@
 #include "homelocation.h"
 #include "flightstatus.h"
 
-// #include "CoordinateConversions.h"
+#include "CoordinateConversions.h"
 
 // Private constants
 #define STACK_SIZE_BYTES  2048
@@ -397,6 +398,17 @@ static void StateEstimationCb(void)
         statename##Set(&s); \
     }
     STORE1(BaroState, bar, Altitude);
+    // attitude nees manual conversion from quaternion to euler
+    if (ISSET(states.updated, att_UPDATED)) { \
+        AttitudeStateData s;
+        AttitudeStateGet(&s);
+        s.q1 = states.att[0];
+        s.q2 = states.att[1];
+        s.q3 = states.att[2];
+        s.q4 = states.att[3];
+        Quaternion2RPY(&s.q1, &s.Roll);
+        AttitudeStateSet(&s);
+    }
 
 
     // clear alarms if everything is alright, then schedule callback execution after timeout