Pull Request for AHRS IO Processor

bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp

@@ -0,0 +1,23 @@
+
+#include <iostream>
+
+#include "DataSyncThread.hpp"
+#include "AHRSInputIOProcessor.hpp"
+
+
+int main()
+{
+	IOProcessor* client = new AHRSInputIOProcessor;
+
+	if (!client->init())
+	{
+		while (client->loopCondition())
+			client->loop();
+
+		int result = client->close();
+		delete client;
+		return result;
+	}
+
+	return 0;
+}

bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp

@@ -0,0 +1,40 @@
+/*
+*
+* 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 "IOProcessor.hpp"
+#include "DataSyncThread.hpp"
+#include "Attribute.hpp"
+
+#include <windows.h>
+#include <iostream>
+
+class AHRSInputIOProcessor : 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,159 @@
+#include "AHRSInputIOProcessor.hpp"
+
+
+/*
+ * Return Values:
+ * 0 : No Errors.
+ * 1 : Serial port does not exist.
+ * 2 : Some other error occurred.
+ * 3 : Error getting state.
+ * 4 : Error setting serial port state
+ * 5 : Error occured while setting timoemouts.
+ * 6 : Error occured while trying to send GoToConfig message.
+ * 7 : Error occured while trying to send SetOutputConfiguration message.
+ * 8 : Error occured while trying to send GoToMeasurment message.
+*/
+
+int AHRSInputIOProcessor::configAHRS()
+{
+    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::cerr << "Serial port does not exist." << std::endl;
+            return 1;
+        }
+        std::cerr << "Some other error occurred." << std::endl;
+        return 2;
+    }
+
+    DCB dcb = { 0 };
+    dcb.DCBlength = sizeof(dcb);
+
+    if (!GetCommState(hSerial, &dcb))
+    {
+        std::cerr << "Error getting state." << std::endl;
+        return 3;
+    }
+
+    dcb.BaudRate = CBR_115200; // defualt baud rate of MT
+    dcb.ByteSize = 8;
+    dcb.StopBits = ONESTOPBIT;
+    dcb.Parity = NOPARITY;
+
+    if (!SetCommState(hSerial, &dcb))
+    {
+        std::cerr << "Error setting serial port state." << std::endl;
+        return 4;
+    }
+
+    COMMTIMEOUTS timeouts = {0};
+
+    timeouts.ReadIntervalTimeout = 50;
+    timeouts.ReadTotalTimeoutConstant = 50;
+    timeouts.ReadTotalTimeoutMultiplier = 10;
+    timeouts.WriteTotalTimeoutConstant = 50;
+    timeouts.WriteTotalTimeoutMultiplier = 10;
+
+    if (!SetCommTimeouts(hSerial, &timeouts))
+    {
+        std::cerr << "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::cerr << "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::cerr << "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::cerr << "Error occured while trying to send GoToMeasurment message." << std::endl;
+        return 8;
+    }
+
+    configured = true;
+    return 0;
+}
+
+bool AHRSInputIOProcessor::loopCondition()
+{
+    return true;
+}
+
+void AHRSInputIOProcessor::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::cerr << "Error occured while trying to read bytes." << std::endl;
+    }
+
+    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 rollArray[8];
+    unsigned char pitchArray[8];
+    unsigned char yawArray[8];
+    if (index != -1) // This puts all those bytes 
+    {
+        for (int i = 0; i < 8; i++)
+        {
+            rollArray[i] = szBuff[index - (i + 8)];
+            pitchArray[i] = szBuff[index - i];
+            yawArray[i] = szBuff[index + (8 - i)];
+        }
+    }
+
+    double doubleRoll = *reinterpret_cast<double* const>(rollArray);
+    double doublePitch = *reinterpret_cast<double* const>(pitchArray);
+    double doubleYaw = *reinterpret_cast<double * const>(yawArray);
+    Attribute attRoll("rolAngle", 8, &doubleRoll);
+    getComms()->sendAttribute(attRoll);
+    Attribute attPitch("pitAngle", 8, &doublePitch);
+    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);
+}