diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp new file mode 100644 index 0000000..5e9eb3d --- /dev/null +++ b/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 +#include + +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 \ No newline at end of file diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp new file mode 100644 index 0000000..24c4026 --- /dev/null +++ b/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(yawArray); + + //std::cout << "Yaw: " << doubleYaw << std::endl; + + Attribute attrib("yawAngle", 8, &doubleYaw); + getComs()->sendAttribute(attriv); + iterations--; + + if (iterations == 0) + CloseHandle(hSerial); +} \ No newline at end of file