diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp index f3227cf..a7a298f 100644 --- a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -1,11 +1,25 @@ +/* +* +* This is the IO Processor for the Xsens MTi-630 AHRS: +* https://www.xsens.com/products/mti-600-series +* The AHRS is able to calculate a lot of different orientational data, +* we are specfically using it to find the Yaw Euler Angle. +* Currently you must manually specify which COM port the device is connected +* to, this is right at the begining of the CreateFile() method call. +* By defualt it is set to COM8, you may have to change it to the correct port +* on your system. You can find the corrct port manually by opening Device Manager, +* under "Ports(COM & LPT)" you can see the ports detected by your system. +*/ + #ifndef AHRS_INPUT_IO_PROCESSOR_HPP #define AHRS_INPUT_IO_PROCESSOR_HPP +#include +#include + #include "IOProcessor.hpp" #include "DataSyncThread.hpp" #include "Attribute.hpp" -#include -#include class AHRSInputIOProcessor : public IOProcessor { @@ -15,12 +29,10 @@ class AHRSInputIOProcessor : public IOProcessor void loop(); bool loopCondition(); int configAHRS(); // Initialize the AHRS for the first time - private: int iterations = 10; bool configured = false; HANDLE hSerial; - }; #endif \ No newline at end of file diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 0f3ddd2..3363a98 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -112,7 +112,6 @@ void AHRSInputIOProcessor::loop() } int index = -1; - for (int i=0; i < 32; i++) // This gets the bytes for the Yaw angle { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) @@ -123,24 +122,13 @@ void AHRSInputIOProcessor::loop() } unsigned char yawArray[8]; - if (index != -1) // This puts all those bytes { for (int i=0; i < 8; i++) yawArray[i] = szBuff[index + (8-i)]; - // yawArray[0] = szBuff[index+8]; - // yawArray[1] = szBuff[index+7]; - // yawArray[2] = szBuff[index+6]; - // yawArray[3] = szBuff[index+5]; - // yawArray[4] = szBuff[index+4]; - // yawArray[5] = szBuff[index+3]; - // yawArray[6] = szBuff[index+2]; - // yawArray[7] = szBuff[index+1]; } + double doubleYaw = *reinterpret_cast(yawArray); - - //std::cout << "Yaw: " << doubleYaw << std::endl; - Attribute attrib("yawAngle", 8, &doubleYaw); getComms()->sendAttribute(attrib); iterations--;