Bug with how I reimplemented Arithmatic (#20)

* Add AHRS IO Proc files into newer pull from dev

* Fix names and built with python script

* Fix pull request comments

* Make edits for the pull request

There is a problem that may be cause by these changes, that being not
closing the handle, since the loop will run forever, I would have to
close the loop on some sort of exit condition. I am not sure if the
handle closes if the process is killed, if so, then I guess it should be
fine.

* Untabify AHRSInputIOProcessor.cpp

* Hmmm

* Revert "Hmmm"

This reverts commit 5a9cb08.

* Add functionality to get roll and pitch angles

This reverts commit e2ea108.

* Change order of #includes fixed build errors...

I have no idea why exactly but the build errors only got fixed when I
changed the order of the includes in AHRSInputIOProcessor.hpp.

* Add Comments Explaining Byte Capture

I added some comments explaining how I am capturing the bytes of the
Euler Angles from the message in the buffer. Quick version here, I first
capute a 2 times the size of the message into a buffer, I then search
for the begining of the message, when I find it, I then jump to the
index where the Angles are stored. I then add each group of 8 bytes for
each angle backwords into arrays to then be converted. I add them in
backwords since the AHRS is big endian and the computer we are
converting them to doubles on is little endian. After each angle's 8
bytes are storred in an array, I then cast them into doubles, where they
can then be sent. Hopefully that makes sense, feel free to ask me any
questions about it.

* 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.

* Fix Comments More

I further clarified my comments and also renamed index to dataStart to
make it more clear what it was.

* Rename Attribute Keys

* Fix type in comments

Changed "index" to "dataStart" in diagram to make it more clear.

* Add more specifics to Config message comments

* Change arround data byte capture math

Changed the for loop to loop backwords itself so that the equations for
actually getting the bytes only use +'s. They are still basically the
same equations, but hopefully they are a little more intuitive...

* Revert "Add more specifics to Config message comments"

This reverts commit 13c66ec.

* Revert "Change arround data byte capture math"

This reverts commit 9988615.

* Revert "Revert "Add more specifics to Config message comments""

This reverts commit f50c2f1.

* Revert "Revert "Change arround data byte capture math""

This reverts commit 596b9af.

* Revert "Revert "Revert "Change arround data byte capture math"""

This reverts commit 361589d.

* Revert changes with arithmetic

When I changed the arithmetic around it messed up the indexing of the
byte arrays. The reason i went up(forward) was so that I could put it
in the array, not to do some funky math. If we don't revert these
changes the byte order will simply be wrong.

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];
         }
     }