From 5a9cb089a1776d449f8ef04eec6f8d10589bbea5 Mon Sep 17 00:00:00 2001
From: Matt Scalzo <matthew.scalzo@uconn.edu>
Date: Fri, 8 May 2020 16:26:40 -0400
Subject: [PATCH] Hmmm

---
 .../io_procs/AHRSInputIOProcessor.cpp         | 22 ++++++++++++++-----
 1 file changed, 17 insertions(+), 5 deletions(-)

diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
index 91fae83..6201ab1 100644
--- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
+++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
@@ -131,17 +131,29 @@ void AHRSInputIOProcessor::loop()
         }
     }
 
+    unsigned char rollArray[8];
+    unsigned char pitchArray[8];
     unsigned char yawArray[8];
     if (index != -1) // This puts all those bytes 
     {
-        for (int i=0; i < 8; i++)
-            yawArray[i] = szBuff[index + (8-i)];
+        for (int i = 0; i < 8; i++)
+        {
+            rollArray[i] = szBuff[index - (i + 8)];
+            pitchArray[i] = szBuff[index - i];
+            yawArray[i] = szBuff[index + (8 - i)];
+        }
     }
     
+    double doubleRoll = *reinterpret_cast<double* const>(rollArray);
+    double doublePitch = *reinterpret_cast<double* const>(pitchArray);
     double doubleYaw = *reinterpret_cast<double * const>(yawArray);
-    Attribute attrib("yawAngle", 8, &doubleYaw);
-    getComms()->sendAttribute(attrib);
+    Attribute attRoll("rolAngle", 8, &doubleRoll);
+    getComms()->sendAttribute(attRoll);
+    Attribute attPitch("pitAngle", 8, &doublePitch);
+    getComms()->sendAttribute(attPitch);
+    Attribute attYaw("yawAngle", 8, &doubleYaw);
+    getComms()->sendAttribute(attYaw);
 
     // I am not sure if it will close autimatically when the process ends, at the moment I guess we just won't close it...
     //CloseHandle(hSerial);
-}
\ No newline at end of file
+}