Bug with how I reimplemented Arithmatic

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 7th index of the message (dataStart). This is where the first
+     * it finds the 14th index of the message (dataStart). This is where the last
      * 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 + 7, which is where the data we want starts (for Roll).
+     * dataStart = i + 14, which is where the data we want starts (for Roll).
      * To find Pitch and Yaw, we add 8 and 16 respectivly.
      *
      * MESSAGE STRUCTURE:
@@ -144,16 +144,16 @@ 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;
     for (int i=0; i < 32; i++)
     {
         if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36)
         {
-            dataStart = i + 7;
+            dataStart = i + 14;
             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 = 7; i > -1; i--)
+        for (int i = 0; i < 8; 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];
         }
     }