Add AHRS IO Proc files into newer pull from dev

bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp

@@ -0,0 +1,26 @@
+#ifndef AHRS_OUTPUT_IO_PROCESSOR_HPP
+#define AHRS_OUTPUT_IO_PROCESSOR_HPP
+
+#include "IOProcessor.hpp"
+#include "DataSyncThread.hpp"
+#include "Attribute.hpp"
+#include <windows.h>
+#include <iostream>
+
+class AHRSOutputIOProcessor : public IOProcessor
+{
+public:
+    using IOProcessor::IOProcessor;
+
+    void loop();
+    bool loopCondition();
+    int configAHRS();   // Initialize the AHRS for the first time
+
+private:
+    int iterations = 10;
+    bool configured = false;
+    HANDLE hSerial;
+
+};
+
+#endif

bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp

@@ -0,0 +1,150 @@
+#include "AHRSOutputIOProcessor.hpp"
+
+
+int AHRSOutputIOProcessor::configAHRS()
+{
+    DCB dcb = {0};
+	//HANDLE hSerial;
+
+	hSerial = CreateFile("COM8",						// The COM port to connect to, may have to be changed
+						  GENERIC_READ | GENERIC_WRITE,	// depending on what COM port gets selected.
+						  0,
+						  0,
+						  OPEN_EXISTING,
+						  FILE_ATTRIBUTE_NORMAL,
+						  0);
+
+	if (hSerial == INVALID_HANDLE_VALUE)
+	{
+		if(GetLastError() == ERROR_FILE_NOT_FOUND)
+		{
+			std::cout << "Serial port does not exist." << std::endl;
+			return 1; 	// serial port does not exist.
+		}
+		std::cout << "Some other error occurred." << std::endl;
+		return 2; 		// some other error occurred.
+	}
+
+	dcb.DCBlength = sizeof(dcb);
+
+	if (!GetCommState(hSerial, &dcb))
+	{
+		std::cout << "Error getting state." << std::endl;
+		return 3;		// error getting state.
+	}
+
+	dcb.BaudRate = CBR_115200; // defualt baud rate of MT
+	dcb.ByteSize = 8;
+	dcb.StopBits = ONESTOPBIT;
+	dcb.Parity = NOPARITY;
+
+	if (!SetCommState(hSerial, &dcb))
+	{
+		std::cout << "Error setting serial port state." << std::endl;
+		return 4; 		// error setting serial port state
+	}
+
+	COMMTIMEOUTS timeouts = {0};
+
+	timeouts.ReadIntervalTimeout = 50;
+	timeouts.ReadTotalTimeoutConstant = 50;
+	timeouts.ReadTotalTimeoutMultiplier = 10;
+	timeouts.WriteTotalTimeoutConstant = 50;
+	timeouts.WriteTotalTimeoutMultiplier = 10;
+
+	if (!SetCommTimeouts(hSerial, &timeouts))
+	{
+		std::cout << "Error occured while setting timoemouts." << std::endl;
+		return 5;
+	}
+
+	std::cout << "Connection established!" << std::endl;
+	std::cout << "Setting up Config..." << std::endl;
+
+	unsigned char wBuff1[] = {0xFA,0xFF,0x30,0x00,0xD1}; // This is the GoToConfig message
+	DWORD dwBytesWritten = 0; // Number of bytes actually written
+
+	if (!WriteFile(hSerial, wBuff1, 6, &dwBytesWritten, NULL))
+	{
+		std::cout << "Error occured while trying to send GoToConfig message." << std::endl;
+		return 6;
+	}
+
+	unsigned char wBuff2[] = {0xFA,0xFF,0xC0,0x04,0x20,0x33,0x00,0x00,0xEA};	// This is the SetOutputConfiguration message
+	dwBytesWritten = 0;
+
+	if (!WriteFile(hSerial, wBuff2, 10, &dwBytesWritten, NULL))
+	{
+		std::cout << "Error occured while trying to send SetOutputConfiguration message." << std::endl;
+		return 7;
+	}
+
+	unsigned char wBuff3[] = {0xFA,0xFF,0x10,0x00,0xF1};	// This is the GoToMeasurment message
+	dwBytesWritten = 0;
+
+	if (!WriteFile(hSerial, wBuff3, 6, &dwBytesWritten, NULL))
+	{
+		std::cout << "Error occured while trying to send GoToMeasurment message." << std::endl;
+		return 8;
+	}
+
+    configured = true;
+    return 0;
+}
+
+bool AHRSOutputIOProcessor::loopCondition()
+{
+    return iterations > 0;
+}
+
+void AHRSOutputIOProcessor::loop()
+{
+    if (!configured)
+        configAHRS();
+
+    unsigned char szBuff[64 + 1] = {0}; // Buffer size
+	DWORD dwBytesRead = 0; // Number of bytes actually read
+
+	if (!ReadFile(hSerial, szBuff, 64, &dwBytesRead, NULL))
+	{
+		std::cout << "Error occured while trying to read bytes." << std::endl;
+		return 9;
+	}
+
+	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)
+		{
+			index = i + 22;
+			break;
+		}
+	}
+
+	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<double * const>(yawArray);
+
+	//std::cout << "Yaw: " << doubleYaw << std::endl;
+
+    Attribute attrib("yawAngle", 8, &doubleYaw);
+    getComs()->sendAttribute(attriv);
+    iterations--;
+
+    if (iterations == 0)
+        CloseHandle(hSerial);
+}