Permalink
Cannot retrieve contributors at this time
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
bfs/bfs/implementations/breadcrumbs/io_procs/AHRSOutputIOProcessor.cpp
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
150 lines (120 sloc)
3.99 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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); | |
} |