Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
Untabify AHRSInputIOProcessor.cpp
  • Loading branch information
@mfs16101
mfs16101 committed May 8, 2020
1 parent 2f29396 commit 3cbcf6e
Showing 1 changed file with 108 additions and 108 deletions.
216 changes: 108 additions & 108 deletions bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
View file
Expand Up @@ -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;
Expand All @@ -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<double * const>(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<double * const>(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);
}

0 comments on commit 3cbcf6e

Please sign in to comment.