Hmmm

grf14003 · May 8, 2020 · 5a9cb08 · 5a9cb08
1 parent 3cbcf6e
commit 5a9cb08
Showing 1 changed file with 17 additions and 5 deletions.
diff --git 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);
-}
+}