From 361589dd38487f3d5d4c38f5f417ea351935dbf4 Mon Sep 17 00:00:00 2001
From: Matt Scalzo <matthew.scalzo@uconn.edu>
Date: Mon, 11 May 2020 19:47:14 -0400
Subject: [PATCH] Revert "Revert "Change arround data byte capture math""

This reverts commit 596b9af0a6a24065ad0d5919ca6be2964194ca71.
---
 .../io_procs/AHRSInputIOProcessor.cpp          | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
index 69bb90d..4d06a36 100644
--- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
+++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
@@ -127,13 +127,13 @@ void AHRSInputIOProcessor::loop()
 
     /* This gets the index of the bytes for the Euler angles in the captured
      * buffer (szBuff). First, it finds the first byte of the message (i). Then,
-     * it finds the 14th index of the message (dataStart). This is where the last
+     * it finds the 7th index of the message (dataStart). This is where the first
      * byte of the roll angle is. This allows me loop backward through each group
      * of bytes, since the order needs to be reversed because the AHRS is in Big
      * Endian.
      
      * i is where the message starts in the buffer.
-     * dataStart = i + 14, which is where the data we want starts (for Roll).
+     * dataStart = i + 7, which is where the data we want starts (for Roll).
      * To find Pitch and Yaw, we add 8 and 16 respectivly.
      *
      * MESSAGE STRUCTURE:
@@ -144,8 +144,8 @@ void AHRSInputIOProcessor::loop()
      *     | R1 R2 R3 R4 R5 R6 R7 R8 | P1 P2 P3 P4 P5 P6 P7 P8 | Y1 Y2 Y3 Y4 Y5 Y6 Y7 Y8 |
      *     +-------------------------+-------------------------+-------------------------+
      * ... | 7  8  9  10 11 12 13 14 | 15 16 17 18 19 20 21 22 | 23 24 25 26 27 28 29 30 | ...
-     *     |                      ^  |                      ^  |                      ^  |
-     *     |                dataStart|            dataStart + 8|           dataStart + 16|
+     *     | ^                       | ^                       | ^                       |
+     *     |dataStart                |dataStart + 8            |dataStart + 16           |
      *     +-------------------------+-------------------------+-------------------------+
     */ 
     int dataStart = -1;
@@ -153,7 +153,7 @@ void AHRSInputIOProcessor::loop()
     {
         if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36)
         {
-            dataStart = i + 14;
+            dataStart = i + 7;
             break;
         }
     }
@@ -164,11 +164,11 @@ void AHRSInputIOProcessor::loop()
     // Puts the bytes for each angle in their own array backwards, since they are sent in big endian
     if (dataStart != -1)
     {
-        for (int i = 0; i < 8; i++)
+        for (int i = 7; i > -1; i--)
         {
-            rollArray[i] = szBuff[dataStart - i];
-            pitchArray[i] = szBuff[dataStart - i + 8];
-            yawArray[i] = szBuff[dataStart - i + 16];
+            rollArray[i] = szBuff[dataStart + i];
+            pitchArray[i] = szBuff[dataStart + i + 8];
+            yawArray[i] = szBuff[dataStart + i + 16];
         }
     }