diff --git a/.gitignore b/.gitignore index 346c329..70b4c79 100644 --- a/.gitignore +++ b/.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/* diff --git a/bfs/implementations/breadcrumbs/include/AHRSOutputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSOutputIOProcessor.hpp new file mode 100644 index 0000000..5e9eb3d --- /dev/null +++ b/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 +#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/AHRSOutputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSOutputIOProcessor.cpp new file mode 100644 index 0000000..24c4026 --- /dev/null +++ b/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(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 diff --git a/bfs/src/io/SensorInterface.hpp b/bfs/src/io/SensorInterface.hpp new file mode 100644 index 0000000..a5478bf --- /dev/null +++ b/bfs/src/io/SensorInterface.hpp @@ -0,0 +1,51 @@ +/* + * + * Stores the interface for building a sensor thread + * implementation. + * + */ + +#include +#include + + +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; +}; diff --git a/bfs/src/io/out_procs/VirtualOutputProcessor.cpp b/bfs/src/io/out_procs/VirtualOutputProcessor.cpp new file mode 100644 index 0000000..1c78bdb --- /dev/null +++ b/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)); +} diff --git a/bfs/src/io/out_procs/VirtualOutputProcessor.hpp b/bfs/src/io/out_procs/VirtualOutputProcessor.hpp new file mode 100644 index 0000000..4e283e6 --- /dev/null +++ b/bfs/src/io/out_procs/VirtualOutputProcessor.hpp @@ -0,0 +1,11 @@ + +#include + +#include "../SensorInterface.hpp" + + +class VirtualOutputProcessor : public SensorThread +{ +public: + virtual void threadRuntime(); +}; diff --git a/breadcrumbs/src/Breadcrumbs.cpp b/breadcrumbs/src/Breadcrumbs.cpp new file mode 100644 index 0000000..61522c4 --- /dev/null +++ b/breadcrumbs/src/Breadcrumbs.cpp @@ -0,0 +1,26 @@ +#include + +#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; +} diff --git a/serialPortComs/Docs/Serial.pdf b/serialPortComs/Docs/Serial.pdf new file mode 100644 index 0000000..670b52f Binary files /dev/null and b/serialPortComs/Docs/Serial.pdf differ diff --git a/serialPortComs/Docs/serial-win.pdf b/serialPortComs/Docs/serial-win.pdf new file mode 100644 index 0000000..07c96ae Binary files /dev/null and b/serialPortComs/Docs/serial-win.pdf differ diff --git a/serialPortComs/checksum.cpp b/serialPortComs/checksum.cpp new file mode 100644 index 0000000..27f7233 --- /dev/null +++ b/serialPortComs/checksum.cpp @@ -0,0 +1,66 @@ +#include +#include +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 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 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; +// } \ No newline at end of file diff --git a/serialPortComs/doubleConv.cpp b/serialPortComs/doubleConv.cpp new file mode 100644 index 0000000..b551201 --- /dev/null +++ b/serialPortComs/doubleConv.cpp @@ -0,0 +1,12 @@ +#include + +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(array); + + cout << doubleVal << endl; +} diff --git a/serialPortComs/serial.cpp b/serialPortComs/serial.cpp new file mode 100644 index 0000000..a10744c --- /dev/null +++ b/serialPortComs/serial.cpp @@ -0,0 +1,148 @@ +#include +#include +#include "checksum.cpp" + +int main() +{ + // This needs to be run once to configure the MT + 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; + } + + std::cout << "Attempting to read data..." << std::endl; + + int msgsToRead = 1000; + int msgsRead = 0; + + // This is the stuff that happens everytime + while (msgsRead < msgsToRead) + { + 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; + + msgsRead++; + } + + std::cout << "Closing Handle..." << std::endl; + CloseHandle(hSerial); + std::cout << "Handle Closed." << std::endl; +}