Fix confusing math

I fixed the math for getting the bytes for the Angles to make a bit easier to
understand. I also clarfied my comments a bit to try and make them
clearer with the explanation.

bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp

@@ -120,11 +120,15 @@ void AHRSInputIOProcessor::loop()
         std::cerr << "Error occured while trying to read bytes." << std::endl;
     }
 
-    /* This gets the index of the bytes for the Euler angles in captured buffer.
-     * The 22 index of the message is where the last byte of the pitch angle is.
-     * It finds the first byte of the message and then finds where the data 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.
+    /* This gets the index of the bytes for the Euler angles in the captured buffer
+     * szBuff. The 14 index of the message is where the last byte of the roll angle is.
+     * It finds the first byte of the message and then finds where the data 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.
+     * index = 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:
      * Roll Angle Bytes = RX, Pitch Angle Bytes = PX, Yaw Angle Bytes = YX
@@ -134,16 +138,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 | ...
-     *     |                         |                      ^  |                         |
-     *     |                         |                    Index|                         |
+     *     |                      ^  |                      ^  |                      ^  |
+     *     |                    index|                index + 8|               index + 16|
      *     +-------------------------+-------------------------+-------------------------+
     */ 
     int index = -1;
     for (int i=0; i < 32; i++)
     {
         if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36)
         {
-            index = i + 22;
+            index = i + 14;
             break;
         }
     }
@@ -156,9 +160,9 @@ void AHRSInputIOProcessor::loop()
     {
         for (int i = 0; i < 8; i++)
         {
-            rollArray[i] = szBuff[index - (i + 8)];
-            pitchArray[i] = szBuff[index - i];
-            yawArray[i] = szBuff[index + (8 - i)];
+            rollArray[i] = szBuff[index - i];
+            pitchArray[i] = szBuff[index - i + 8];
+            yawArray[i] = szBuff[index - i + 16];
         }
     }