Pull Request AHRS IO proc into dev

.gitignore

@@ -1,4 +1,7 @@
 .vs/*
+# These can be removed when xsensCom branch is merged/removed
+*.exe
+.vscode/*
 
 # Excluding the build and executable folders
 bfs/build/*

bfs/implementations/breadcrumbs/include/AHRSOutputIOProcessor.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/AHRSOutputIOProcessor.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);
+}

bfs/src/io/SensorInterface.hpp

@@ -0,0 +1,51 @@
+/*
+ * 
+ * Stores the interface for building a sensor thread
+ * implementation.
+ *
+ */
+
+#include <Windows.h>
+#include <stdio.h>
+
+
+class SensorThread
+{
+public:
+
+	// Thread start 'caller' function
+	static DWORD __stdcall threadStart(LPVOID pUserData) {
+		((SensorThread*) pUserData)->threadRuntime();
+		return 0;
+	}
+	// Thread routine for implementation to override
+	virtual void threadRuntime() = 0;
+
+	// Async control
+	UINT8 startThread(LPVOID targs)s
+	{
+		threadArgs = targs;
+		hThread = CreateThread(
+			NULL,                   // default security attributes
+			0,                      // use default stack size  
+			threadStart,            // thread function name
+			this,                   // argument to thread function 
+			0,                      // use default creation flags 
+			&dwThreadId);			// returns the thread identifier 
+		return TRUE;
+	};
+	BOOL waitForThread()
+	{
+		WaitForSingleObject(hThread, INFINITE);
+		return TRUE;
+	};
+
+	// Sync control
+	BOOLEAN bufferDataAvailable() { return TRUE; };
+	SIZE_T getBufferData(LPCSTR* bufferKeyArray, LPCSTR* bufferValueArray) { return 0; };
+	VOID setDataStoreValue(LPCSTR key, LPCVOID value, SIZE_T valueSize) {};
+protected:
+	LPCVOID threadArgs;
+	DWORD dwThreadId;
+	HANDLE hThread;
+};

bfs/src/io/out_procs/VirtualOutputProcessor.cpp

@@ -0,0 +1,8 @@
+
+#include "VirtualOutputProcessor.hpp"
+
+
+void VirtualOutputProcessor::threadRuntime()
+{
+	printf("Hello from thread %d!\n", *((int*) threadArgs));
+}

bfs/src/io/out_procs/VirtualOutputProcessor.hpp

@@ -0,0 +1,11 @@
+
+#include <stdio.h>
+
+#include "../SensorInterface.hpp"
+
+
+class VirtualOutputProcessor : public SensorThread
+{
+public:
+	virtual void threadRuntime();
+};

breadcrumbs/src/Breadcrumbs.cpp

@@ -0,0 +1,26 @@
+#include <stdio.h>
+
+#include "io/out_procs/VirtualOutputProcessor.hpp"
+
+
+#define THREADS 5
+
+
+int main() {
+	printf("Hello World!\n");
+
+	SensorThread* procs[THREADS];
+	int targs[THREADS];
+
+	for (int i = 0; i < THREADS; i++) {
+		procs[i] = new VirtualOutputProcessor;
+		targs[i] = i + 1;
+		procs[i]->startThread(targs + i);
+	}
+
+	for (int i = 0; i < THREADS; i++) {
+		procs[i]->waitForThread();
+		delete procs[i];
+	}
+	return 0;
+}

serialPortComs/checksum.cpp

@@ -0,0 +1,66 @@
+#include <iostream>
+#include <vector>
+using namespace std;
+
+struct HexCharStruct        // Fixes char to hex stuff?
+{
+  unsigned char c;
+  HexCharStruct(unsigned char _c) : c(_c) { }
+};
+
+inline ostream& operator<<(ostream& o, const HexCharStruct& hs)
+{
+  return (o << hex << (int)hs.c);
+}
+
+inline HexCharStruct hex(unsigned char _c)
+{
+  return HexCharStruct(_c);
+}
+
+unsigned char calChecksum(const vector<unsigned char> msg)
+{
+  unsigned char sum = 0;
+
+  for (int i=1; i < msg.size(); i++)
+  {
+    sum += msg[i];
+  }
+
+  unsigned char check = 0x100 - (sum & 0xFF);
+
+  return check;
+}
+
+
+// int main() // Use vector so that it can be trivialy long.
+// {
+//     unsigned char preamb = 0xFA;
+//     unsigned char BID = 0xFF;
+//     unsigned char MID = 0x30;
+//     unsigned char LEN = 0x00;
+
+//     //unsigned char check1 = 0x100 - ((BID + MID + LEN) & 0xFF); // This outputs the correct checksum.
+
+//     vector<unsigned char> mess;
+
+//     mess.push_back(preamb);
+//     mess.push_back(BID);
+//     mess.push_back(MID);
+//     mess.push_back(LEN);
+//     // Data chars would go here
+
+//     unsigned char check = calChecksum(mess);
+
+//     //cout << "Checksum is: " << hex(check) << endl;
+//     mess.push_back(check);
+
+//     cout << "GoToConfig message: ";
+
+//     for (unsigned char c: mess) // Could instead be adding these to the buffer
+//         cout << hex(c) << " "; 
+
+//     cout << endl;
+
+//     return 0;
+// }

serialPortComs/doubleConv.cpp

@@ -0,0 +1,12 @@
+#include <iostream>
+
+using namespace std;
+
+int main()
+{
+    unsigned char array[] = {0x00, 0x00, 0x00, 0x20, 0xDB, 0xE2, 0xC4, 0x3F}; //0x3F, 0xC4, 0xE2, 0xDB, 0x20, 0x00, 0x00, 0x00
+
+    double doubleVal = *reinterpret_cast<double * const>(array);
+
+    cout << doubleVal << endl;
+}