diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp index 97d268d..dde9ca8 100644 --- a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -14,8 +14,6 @@ #ifndef AHRS_INPUT_IO_PROCESSOR_HPP #define AHRS_INPUT_IO_PROCESSOR_HPP - - #include "IOProcessor.hpp" #include "DataSyncThread.hpp" #include "Attribute.hpp" diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 6201ab1..7aeccff 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -1,6 +1,5 @@ #include "AHRSInputIOProcessor.hpp" - /* * Return Values: * 0 : No Errors. @@ -121,8 +120,26 @@ 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. + * + * MESSAGE STRUCTURE: + * Roll Angle Bytes = RX, Pitch Angle Bytes = PX, Yaw Angle Bytes = YX + * +-------------------------+-------------------------+-------------------------+ + * | Roll | Pitch | Yaw | + * +-------------------------+-------------------------+-------------------------+ + * | 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| | + * +-------------------------+-------------------------+-------------------------+ + */ int index = -1; - for (int i=0; i < 32; i++) // This gets the bytes for the Yaw angle + for (int i=0; i < 32; i++) { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) { @@ -134,7 +151,8 @@ void AHRSInputIOProcessor::loop() unsigned char rollArray[8]; unsigned char pitchArray[8]; unsigned char yawArray[8]; - if (index != -1) // This puts all those bytes + // Puts the bytes for each angle in their own array backwards, since they are sent in big endian + if (index != -1) { for (int i = 0; i < 8; i++) { @@ -153,7 +171,4 @@ void AHRSInputIOProcessor::loop() 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); }