From 3cbcf6e0650a07a2db0acc483d78dff8d9736fb2 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 15:54:59 -0400 Subject: [PATCH] Untabify AHRSInputIOProcessor.cpp --- .../io_procs/AHRSInputIOProcessor.cpp | 216 +++++++++--------- 1 file changed, 108 insertions(+), 108 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 5f52fdc..91fae83 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -16,88 +16,88 @@ 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; - } + 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; @@ -114,34 +114,34 @@ void AHRSInputIOProcessor::loop() 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 yawArray[8]; - if (index != -1) // This puts all those bytes - { - for (int i=0; i < 8; i++) - yawArray[i] = szBuff[index + (8-i)]; - } - - double doubleYaw = *reinterpret_cast(yawArray); + 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 yawArray[8]; + if (index != -1) // This puts all those bytes + { + for (int i=0; i < 8; i++) + yawArray[i] = szBuff[index + (8-i)]; + } + + double doubleYaw = *reinterpret_cast(yawArray); Attribute attrib("yawAngle", 8, &doubleYaw); getComms()->sendAttribute(attrib); - // I am not sure if it will close autimatically when the process ends, at the moment I guess we just won't close it... + // 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); } \ No newline at end of file