From 17bdc02fd7c6dd6295a0a771747159fc9c3f2c27 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 13:45:03 -0400 Subject: [PATCH 01/16] Add AHRS IO Proc files into newer pull from dev --- .../include/AHRSInputIOProcessor.hpp | 26 +++ .../io_procs/AHRSInputIOProcessor.cpp | 150 ++++++++++++++++++ 2 files changed, 176 insertions(+) create mode 100644 bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp create mode 100644 bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp 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 From ac92532dc8e761def289e50ea54919af257d4c70 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 13:56:57 -0400 Subject: [PATCH 02/16] Fix names and built with python script --- .../breadcrumbs/gen/AHRSInputIOProcessor.cpp | 23 +++++++++++++++++++ .../include/AHRSInputIOProcessor.hpp | 6 ++--- .../io_procs/AHRSInputIOProcessor.cpp | 12 +++++----- 3 files changed, 32 insertions(+), 9 deletions(-) create mode 100644 bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp diff --git a/bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp new file mode 100644 index 0000000..fb8a3f3 --- /dev/null +++ b/bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp @@ -0,0 +1,23 @@ + +#include + +#include "DataSyncThread.hpp" +#include "AHRSInputIOProcessor.hpp" + + +int main() +{ + IOProcessor* client = new AHRSInputIOProcessor; + + if (!client->init()) + { + while (client->loopCondition()) + client->loop(); + + int result = client->close(); + delete client; + return result; + } + + return 0; +} diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp index 5e9eb3d..f3227cf 100644 --- a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -1,5 +1,5 @@ -#ifndef AHRS_OUTPUT_IO_PROCESSOR_HPP -#define AHRS_OUTPUT_IO_PROCESSOR_HPP +#ifndef AHRS_INPUT_IO_PROCESSOR_HPP +#define AHRS_INPUT_IO_PROCESSOR_HPP #include "IOProcessor.hpp" #include "DataSyncThread.hpp" @@ -7,7 +7,7 @@ #include #include -class AHRSOutputIOProcessor : public IOProcessor +class AHRSInputIOProcessor : public IOProcessor { public: using IOProcessor::IOProcessor; diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 24c4026..0f3ddd2 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -1,7 +1,7 @@ -#include "AHRSOutputIOProcessor.hpp" +#include "AHRSInputIOProcessor.hpp" -int AHRSOutputIOProcessor::configAHRS() +int AHRSInputIOProcessor::configAHRS() { DCB dcb = {0}; //HANDLE hSerial; @@ -92,12 +92,12 @@ int AHRSOutputIOProcessor::configAHRS() return 0; } -bool AHRSOutputIOProcessor::loopCondition() +bool AHRSInputIOProcessor::loopCondition() { return iterations > 0; } -void AHRSOutputIOProcessor::loop() +void AHRSInputIOProcessor::loop() { if (!configured) configAHRS(); @@ -108,7 +108,7 @@ void AHRSOutputIOProcessor::loop() if (!ReadFile(hSerial, szBuff, 64, &dwBytesRead, NULL)) { std::cout << "Error occured while trying to read bytes." << std::endl; - return 9; + //return 9; } int index = -1; @@ -142,7 +142,7 @@ void AHRSOutputIOProcessor::loop() //std::cout << "Yaw: " << doubleYaw << std::endl; Attribute attrib("yawAngle", 8, &doubleYaw); - getComs()->sendAttribute(attriv); + getComms()->sendAttribute(attrib); iterations--; if (iterations == 0) From f3f13b827f20eb1f8e77c575e69e194b219970dd Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 14:26:08 -0400 Subject: [PATCH 03/16] Fix pull request comments --- .../include/AHRSInputIOProcessor.hpp | 20 +++++++++++++++---- .../io_procs/AHRSInputIOProcessor.cpp | 14 +------------ 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp index f3227cf..a7a298f 100644 --- a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -1,11 +1,25 @@ +/* +* +* This is the IO Processor for the Xsens MTi-630 AHRS: +* https://www.xsens.com/products/mti-600-series +* The AHRS is able to calculate a lot of different orientational data, +* we are specfically using it to find the Yaw Euler Angle. +* Currently you must manually specify which COM port the device is connected +* to, this is right at the begining of the CreateFile() method call. +* By defualt it is set to COM8, you may have to change it to the correct port +* on your system. You can find the corrct port manually by opening Device Manager, +* under "Ports(COM & LPT)" you can see the ports detected by your system. +*/ + #ifndef AHRS_INPUT_IO_PROCESSOR_HPP #define AHRS_INPUT_IO_PROCESSOR_HPP +#include +#include + #include "IOProcessor.hpp" #include "DataSyncThread.hpp" #include "Attribute.hpp" -#include -#include class AHRSInputIOProcessor : public IOProcessor { @@ -15,12 +29,10 @@ public: 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 index 0f3ddd2..3363a98 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -112,7 +112,6 @@ void AHRSInputIOProcessor::loop() } 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) @@ -123,24 +122,13 @@ void AHRSInputIOProcessor::loop() } 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); getComms()->sendAttribute(attrib); iterations--; From 2f2939644417240d3934062f60c7d3086a4a50e8 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 15:45:13 -0400 Subject: [PATCH 04/16] Make edits for the pull request There is a problem that may be cause by these changes, that being not closing the handle, since the loop will run forever, I would have to close the loop on some sort of exit condition. I am not sure if the handle closes if the process is killed, if so, then I guess it should be fine. --- .../include/AHRSInputIOProcessor.hpp | 2 +- .../io_procs/AHRSInputIOProcessor.cpp | 53 +++++++++++-------- 2 files changed, 32 insertions(+), 23 deletions(-) diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp index a7a298f..728b646 100644 --- a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -35,4 +35,4 @@ private: HANDLE hSerial; }; -#endif \ No newline at end of file +#endif diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 3363a98..5f52fdc 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -1,11 +1,21 @@ #include "AHRSInputIOProcessor.hpp" +/* + * Return Values: + * 0 : No Errors. + * 1 : Serial port does not exist. + * 2 : Some other error occurred. + * 3 : Error getting state. + * 4 : Error setting serial port state + * 5 : Error occured while setting timoemouts. + * 6 : Error occured while trying to send GoToConfig message. + * 7 : Error occured while trying to send SetOutputConfiguration message. + * 8 : Error occured while trying to send GoToMeasurment message. +*/ + int AHRSInputIOProcessor::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, @@ -18,19 +28,20 @@ int AHRSInputIOProcessor::configAHRS() { if(GetLastError() == ERROR_FILE_NOT_FOUND) { - std::cout << "Serial port does not exist." << std::endl; - return 1; // serial port does not exist. + std::cerr << "Serial port does not exist." << std::endl; + return 1; } - std::cout << "Some other error occurred." << std::endl; - return 2; // some other error occurred. + std::cerr << "Some other error occurred." << std::endl; + return 2; } - + + DCB dcb = { 0 }; dcb.DCBlength = sizeof(dcb); if (!GetCommState(hSerial, &dcb)) { - std::cout << "Error getting state." << std::endl; - return 3; // error getting state. + std::cerr << "Error getting state." << std::endl; + return 3; } dcb.BaudRate = CBR_115200; // defualt baud rate of MT @@ -40,8 +51,8 @@ int AHRSInputIOProcessor::configAHRS() if (!SetCommState(hSerial, &dcb)) { - std::cout << "Error setting serial port state." << std::endl; - return 4; // error setting serial port state + std::cerr << "Error setting serial port state." << std::endl; + return 4; } COMMTIMEOUTS timeouts = {0}; @@ -54,7 +65,7 @@ int AHRSInputIOProcessor::configAHRS() if (!SetCommTimeouts(hSerial, &timeouts)) { - std::cout << "Error occured while setting timoemouts." << std::endl; + std::cerr << "Error occured while setting timoemouts." << std::endl; return 5; } @@ -66,7 +77,7 @@ int AHRSInputIOProcessor::configAHRS() if (!WriteFile(hSerial, wBuff1, 6, &dwBytesWritten, NULL)) { - std::cout << "Error occured while trying to send GoToConfig message." << std::endl; + std::cerr << "Error occured while trying to send GoToConfig message." << std::endl; return 6; } @@ -75,7 +86,7 @@ int AHRSInputIOProcessor::configAHRS() if (!WriteFile(hSerial, wBuff2, 10, &dwBytesWritten, NULL)) { - std::cout << "Error occured while trying to send SetOutputConfiguration message." << std::endl; + std::cerr << "Error occured while trying to send SetOutputConfiguration message." << std::endl; return 7; } @@ -84,7 +95,7 @@ int AHRSInputIOProcessor::configAHRS() if (!WriteFile(hSerial, wBuff3, 6, &dwBytesWritten, NULL)) { - std::cout << "Error occured while trying to send GoToMeasurment message." << std::endl; + std::cerr << "Error occured while trying to send GoToMeasurment message." << std::endl; return 8; } @@ -94,7 +105,7 @@ int AHRSInputIOProcessor::configAHRS() bool AHRSInputIOProcessor::loopCondition() { - return iterations > 0; + return true; } void AHRSInputIOProcessor::loop() @@ -107,8 +118,7 @@ void AHRSInputIOProcessor::loop() if (!ReadFile(hSerial, szBuff, 64, &dwBytesRead, NULL)) { - std::cout << "Error occured while trying to read bytes." << std::endl; - //return 9; + std::cerr << "Error occured while trying to read bytes." << std::endl; } int index = -1; @@ -131,8 +141,7 @@ void AHRSInputIOProcessor::loop() double doubleYaw = *reinterpret_cast(yawArray); Attribute attrib("yawAngle", 8, &doubleYaw); getComms()->sendAttribute(attrib); - iterations--; - if (iterations == 0) - CloseHandle(hSerial); + // 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 From 3cbcf6e0650a07a2db0acc483d78dff8d9736fb2 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 15:54:59 -0400 Subject: [PATCH 05/16] 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 From 5a9cb089a1776d449f8ef04eec6f8d10589bbea5 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 16:26:40 -0400 Subject: [PATCH 06/16] Hmmm --- .../io_procs/AHRSInputIOProcessor.cpp | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 91fae83..6201ab1 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -131,17 +131,29 @@ void AHRSInputIOProcessor::loop() } } + unsigned char rollArray[8]; + unsigned char pitchArray[8]; 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)]; + for (int i = 0; i < 8; i++) + { + rollArray[i] = szBuff[index - (i + 8)]; + pitchArray[i] = szBuff[index - i]; + yawArray[i] = szBuff[index + (8 - i)]; + } } + double doubleRoll = *reinterpret_cast(rollArray); + double doublePitch = *reinterpret_cast(pitchArray); double doubleYaw = *reinterpret_cast(yawArray); - Attribute attrib("yawAngle", 8, &doubleYaw); - getComms()->sendAttribute(attrib); + Attribute attRoll("rolAngle", 8, &doubleRoll); + getComms()->sendAttribute(attRoll); + Attribute attPitch("pitAngle", 8, &doublePitch); + getComms()->sendAttribute(attPitch); + Attribute attYaw("yawAngle", 8, &doubleYaw); + getComms()->sendAttribute(attYaw); // 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 +} From e2ea108b7b70deb8aa88b4c6dcdfd3edac5db2cf Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 16:28:49 -0400 Subject: [PATCH 07/16] Revert "Hmmm" This reverts commit 5a9cb089a1776d449f8ef04eec6f8d10589bbea5. --- .../io_procs/AHRSInputIOProcessor.cpp | 22 +++++-------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 6201ab1..91fae83 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -131,29 +131,17 @@ void AHRSInputIOProcessor::loop() } } - unsigned char rollArray[8]; - unsigned char pitchArray[8]; unsigned char yawArray[8]; if (index != -1) // This puts all those bytes { - for (int i = 0; i < 8; i++) - { - rollArray[i] = szBuff[index - (i + 8)]; - pitchArray[i] = szBuff[index - i]; - yawArray[i] = szBuff[index + (8 - i)]; - } + for (int i=0; i < 8; i++) + yawArray[i] = szBuff[index + (8-i)]; } - double doubleRoll = *reinterpret_cast(rollArray); - double doublePitch = *reinterpret_cast(pitchArray); double doubleYaw = *reinterpret_cast(yawArray); - Attribute attRoll("rolAngle", 8, &doubleRoll); - getComms()->sendAttribute(attRoll); - Attribute attPitch("pitAngle", 8, &doublePitch); - getComms()->sendAttribute(attPitch); - Attribute attYaw("yawAngle", 8, &doubleYaw); - getComms()->sendAttribute(attYaw); + 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... //CloseHandle(hSerial); -} +} \ No newline at end of file From 98e1ba6e49c7139d838887ed2f701c41748765d9 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 16:30:41 -0400 Subject: [PATCH 08/16] Add functionality to get roll and pitch angles This reverts commit e2ea108b7b70deb8aa88b4c6dcdfd3edac5db2cf. --- .../io_procs/AHRSInputIOProcessor.cpp | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 91fae83..6201ab1 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -131,17 +131,29 @@ void AHRSInputIOProcessor::loop() } } + unsigned char rollArray[8]; + unsigned char pitchArray[8]; 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)]; + for (int i = 0; i < 8; i++) + { + rollArray[i] = szBuff[index - (i + 8)]; + pitchArray[i] = szBuff[index - i]; + yawArray[i] = szBuff[index + (8 - i)]; + } } + double doubleRoll = *reinterpret_cast(rollArray); + double doublePitch = *reinterpret_cast(pitchArray); double doubleYaw = *reinterpret_cast(yawArray); - Attribute attrib("yawAngle", 8, &doubleYaw); - getComms()->sendAttribute(attrib); + Attribute attRoll("rolAngle", 8, &doubleRoll); + getComms()->sendAttribute(attRoll); + Attribute attPitch("pitAngle", 8, &doublePitch); + getComms()->sendAttribute(attPitch); + Attribute attYaw("yawAngle", 8, &doubleYaw); + getComms()->sendAttribute(attYaw); // 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 +} From 277cb31b8c15d653ad5aac657b632fa43adeb773 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Fri, 8 May 2020 17:24:19 -0400 Subject: [PATCH 09/16] Change order of #includes fixed build errors... I have no idea why exactly but the build errors only got fixed when I changed the order of the includes in AHRSInputIOProcessor.hpp. --- .../breadcrumbs/include/AHRSInputIOProcessor.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp index 728b646..97d268d 100644 --- a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -14,13 +14,15 @@ #ifndef AHRS_INPUT_IO_PROCESSOR_HPP #define AHRS_INPUT_IO_PROCESSOR_HPP -#include -#include + #include "IOProcessor.hpp" #include "DataSyncThread.hpp" #include "Attribute.hpp" +#include +#include + class AHRSInputIOProcessor : public IOProcessor { public: From c55c91ae3a2dbbcff4fcc041709251c0959b82cb Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Sun, 10 May 2020 17:08:42 -0400 Subject: [PATCH 10/16] Add Comments Explaining Byte Capture I added some comments explaining how I am capturing the bytes of the Euler Angles from the message in the buffer. Quick version here, I first capute a 2 times the size of the message into a buffer, I then search for the begining of the message, when I find it, I then jump to the index where the Angles are stored. I then add each group of 8 bytes for each angle backwords into arrays to then be converted. I add them in backwords since the AHRS is big endian and the computer we are converting them to doubles on is little endian. After each angle's 8 bytes are storred in an array, I then cast them into doubles, where they can then be sent. Hopefully that makes sense, feel free to ask me any questions about it. --- .../include/AHRSInputIOProcessor.hpp | 2 -- .../io_procs/AHRSInputIOProcessor.cpp | 27 ++++++++++++++----- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp index 97d268d..dde9ca8 100644 --- a/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -14,8 +14,6 @@ #ifndef AHRS_INPUT_IO_PROCESSOR_HPP #define AHRS_INPUT_IO_PROCESSOR_HPP - - #include "IOProcessor.hpp" #include "DataSyncThread.hpp" #include "Attribute.hpp" diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 6201ab1..7aeccff 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -1,6 +1,5 @@ #include "AHRSInputIOProcessor.hpp" - /* * Return Values: * 0 : No Errors. @@ -121,8 +120,26 @@ void AHRSInputIOProcessor::loop() std::cerr << "Error occured while trying to read bytes." << std::endl; } + /* This gets the index of the bytes for the Euler angles in captured buffer. + * The 22 index of the message is where the last byte of the pitch angle is. + * It finds the first byte of the message and then finds where the data is. + * This allows me loop backward through each group of bytes since the order + * needs to be reversed because the AHRS is in Big Endian. + * + * MESSAGE STRUCTURE: + * Roll Angle Bytes = RX, Pitch Angle Bytes = PX, Yaw Angle Bytes = YX + * +-------------------------+-------------------------+-------------------------+ + * | Roll | Pitch | Yaw | + * +-------------------------+-------------------------+-------------------------+ + * | R1 R2 R3 R4 R5 R6 R7 R8 | P1 P2 P3 P4 P5 P6 P7 P8 | Y1 Y2 Y3 Y4 Y5 Y6 Y7 Y8 | + * +-------------------------+-------------------------+-------------------------+ + * ... | 7 8 9 10 11 12 13 14 | 15 16 17 18 19 20 21 22 | 23 24 25 26 27 28 29 30 | ... + * | | ^ | | + * | | Index| | + * +-------------------------+-------------------------+-------------------------+ + */ int index = -1; - for (int i=0; i < 32; i++) // This gets the bytes for the Yaw angle + for (int i=0; i < 32; i++) { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) { @@ -134,7 +151,8 @@ void AHRSInputIOProcessor::loop() unsigned char rollArray[8]; unsigned char pitchArray[8]; unsigned char yawArray[8]; - if (index != -1) // This puts all those bytes + // Puts the bytes for each angle in their own array backwards, since they are sent in big endian + if (index != -1) { for (int i = 0; i < 8; i++) { @@ -153,7 +171,4 @@ void AHRSInputIOProcessor::loop() getComms()->sendAttribute(attPitch); Attribute attYaw("yawAngle", 8, &doubleYaw); getComms()->sendAttribute(attYaw); - - // 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); } From 6bb6f9c792c6009fd631977cead326a69a184b3e Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Sun, 10 May 2020 17:30:22 -0400 Subject: [PATCH 11/16] Fix confusing math I fixed the math for getting the bytes for the Angles to make a bit easier to understand. I also clarfied my comments a bit to try and make them clearer with the explanation. --- .../io_procs/AHRSInputIOProcessor.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 7aeccff..970d6af 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -120,11 +120,15 @@ void AHRSInputIOProcessor::loop() std::cerr << "Error occured while trying to read bytes." << std::endl; } - /* This gets the index of the bytes for the Euler angles in captured buffer. - * The 22 index of the message is where the last byte of the pitch angle is. - * It finds the first byte of the message and then finds where the data is. - * This allows me loop backward through each group of bytes since the order - * needs to be reversed because the AHRS is in Big Endian. + /* This gets the index of the bytes for the Euler angles in the captured buffer + * szBuff. The 14 index of the message is where the last byte of the roll angle is. + * It finds the first byte of the message and then finds where the data is. This + * allows me loop backward through each group of bytes since the order needs to + * be reversed because the AHRS is in Big Endian. + + * i is where the message starts in the buffer. + * index = i + 14, which is where the data we want starts (for Roll). + * To find Pitch and Yaw, we add 8 and 16 respectivly. * * MESSAGE STRUCTURE: * Roll Angle Bytes = RX, Pitch Angle Bytes = PX, Yaw Angle Bytes = YX @@ -134,8 +138,8 @@ void AHRSInputIOProcessor::loop() * | R1 R2 R3 R4 R5 R6 R7 R8 | P1 P2 P3 P4 P5 P6 P7 P8 | Y1 Y2 Y3 Y4 Y5 Y6 Y7 Y8 | * +-------------------------+-------------------------+-------------------------+ * ... | 7 8 9 10 11 12 13 14 | 15 16 17 18 19 20 21 22 | 23 24 25 26 27 28 29 30 | ... - * | | ^ | | - * | | Index| | + * | ^ | ^ | ^ | + * | index| index + 8| index + 16| * +-------------------------+-------------------------+-------------------------+ */ int index = -1; @@ -143,7 +147,7 @@ void AHRSInputIOProcessor::loop() { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) { - index = i + 22; + index = i + 14; break; } } @@ -156,9 +160,9 @@ void AHRSInputIOProcessor::loop() { for (int i = 0; i < 8; i++) { - rollArray[i] = szBuff[index - (i + 8)]; - pitchArray[i] = szBuff[index - i]; - yawArray[i] = szBuff[index + (8 - i)]; + rollArray[i] = szBuff[index - i]; + pitchArray[i] = szBuff[index - i + 8]; + yawArray[i] = szBuff[index - i + 16]; } } From 45ae10565b7cdd8193a9bdbf2e53535be25c45c2 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Sun, 10 May 2020 17:39:00 -0400 Subject: [PATCH 12/16] Fix Comments More I further clarified my comments and also renamed index to dataStart to make it more clear what it was. --- .../io_procs/AHRSInputIOProcessor.cpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 970d6af..f507517 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -120,14 +120,15 @@ void AHRSInputIOProcessor::loop() std::cerr << "Error occured while trying to read bytes." << std::endl; } - /* This gets the index of the bytes for the Euler angles in the captured buffer - * szBuff. The 14 index of the message is where the last byte of the roll angle is. - * It finds the first byte of the message and then finds where the data is. This - * allows me loop backward through each group of bytes since the order needs to - * be reversed because the AHRS is in Big Endian. + /* This gets the index of the bytes for the Euler angles in the captured + * buffer (szBuff). First, it finds the first byte of the message (i). Then, + * it finds the 14th index of the message (dataStart). This is where the last + * byte of the roll angle is. This allows me loop backward through each group + * of bytes, since the order needs to be reversed because the AHRS is in Big + * Endian. * i is where the message starts in the buffer. - * index = i + 14, which is where the data we want starts (for Roll). + * dataStart = i + 14, which is where the data we want starts (for Roll). * To find Pitch and Yaw, we add 8 and 16 respectivly. * * MESSAGE STRUCTURE: @@ -142,12 +143,12 @@ void AHRSInputIOProcessor::loop() * | index| index + 8| index + 16| * +-------------------------+-------------------------+-------------------------+ */ - int index = -1; + int dataStart = -1; for (int i=0; i < 32; i++) { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) { - index = i + 14; + dataStart = i + 14; break; } } @@ -156,13 +157,13 @@ void AHRSInputIOProcessor::loop() unsigned char pitchArray[8]; unsigned char yawArray[8]; // Puts the bytes for each angle in their own array backwards, since they are sent in big endian - if (index != -1) + if (dataStart != -1) { for (int i = 0; i < 8; i++) { - rollArray[i] = szBuff[index - i]; - pitchArray[i] = szBuff[index - i + 8]; - yawArray[i] = szBuff[index - i + 16]; + rollArray[i] = szBuff[dataStart - i]; + pitchArray[i] = szBuff[dataStart - i + 8]; + yawArray[i] = szBuff[dataStart - i + 16]; } } From f446d77bcc19e7358974a2b0697b8f8a4e38989b Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Sun, 10 May 2020 17:44:15 -0400 Subject: [PATCH 13/16] Rename Attribute Keys --- .../breadcrumbs/io_procs/AHRSInputIOProcessor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index f507517..67f90cb 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -170,10 +170,10 @@ void AHRSInputIOProcessor::loop() double doubleRoll = *reinterpret_cast(rollArray); double doublePitch = *reinterpret_cast(pitchArray); double doubleYaw = *reinterpret_cast(yawArray); - Attribute attRoll("rolAngle", 8, &doubleRoll); + Attribute attRoll("XSIMUROL", 8, &doubleRoll); getComms()->sendAttribute(attRoll); - Attribute attPitch("pitAngle", 8, &doublePitch); + Attribute attPitch("XSIMUPIT", 8, &doublePitch); getComms()->sendAttribute(attPitch); - Attribute attYaw("yawAngle", 8, &doubleYaw); + Attribute attYaw("XSIMUYAW", 8, &doubleYaw); getComms()->sendAttribute(attYaw); } From 58a3dec16f5fe5572d173cade14459cf3fd2e12b Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Sun, 10 May 2020 17:51:30 -0400 Subject: [PATCH 14/16] Fix type in comments Changed "index" to "dataStart" in diagram to make it more clear. --- .../breadcrumbs/io_procs/AHRSInputIOProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 67f90cb..0a8b3f5 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -140,7 +140,7 @@ void AHRSInputIOProcessor::loop() * +-------------------------+-------------------------+-------------------------+ * ... | 7 8 9 10 11 12 13 14 | 15 16 17 18 19 20 21 22 | 23 24 25 26 27 28 29 30 | ... * | ^ | ^ | ^ | - * | index| index + 8| index + 16| + * | dataStart| dataStart + 8| dataStart + 16| * +-------------------------+-------------------------+-------------------------+ */ int dataStart = -1; From 13c66ec7331c46fc40c8e3605e354cb8d3ff7827 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 10 May 2020 21:00:20 -0400 Subject: [PATCH 15/16] Add more specifics to Config message comments --- .../io_procs/AHRSInputIOProcessor.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 0a8b3f5..69bb90d 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -12,7 +12,6 @@ * 7 : Error occured while trying to send SetOutputConfiguration message. * 8 : Error occured while trying to send GoToMeasurment message. */ - int AHRSInputIOProcessor::configAHRS() { hSerial = CreateFile("COM8", // The COM port to connect to, may have to be changed @@ -71,7 +70,9 @@ int AHRSInputIOProcessor::configAHRS() 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 + // This is the GoToConfig message, MID 48 + // Switch the active state of the device from Measurment State to Config State. + unsigned char wBuff1[] = {0xFA,0xFF,0x30,0x00,0xD1}; DWORD dwBytesWritten = 0; // Number of bytes actually written if (!WriteFile(hSerial, wBuff1, 6, &dwBytesWritten, NULL)) @@ -79,8 +80,10 @@ int AHRSInputIOProcessor::configAHRS() 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 + + // This is the SetOutputConfiguration message, MID 192 + // Set the output configuration of the device. + unsigned char wBuff2[] = {0xFA,0xFF,0xC0,0x04,0x20,0x33,0x00,0x00,0xEA}; dwBytesWritten = 0; if (!WriteFile(hSerial, wBuff2, 10, &dwBytesWritten, NULL)) @@ -88,8 +91,10 @@ int AHRSInputIOProcessor::configAHRS() 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 + + // This is the GoToMeasurment message, MID 16 + // Switch the active state of the device from Config State to Measurment State. + unsigned char wBuff3[] = {0xFA,0xFF,0x10,0x00,0xF1}; dwBytesWritten = 0; if (!WriteFile(hSerial, wBuff3, 6, &dwBytesWritten, NULL)) From 99886151af9334ff1805b008da323f9e2966041f Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Mon, 11 May 2020 13:15:35 -0400 Subject: [PATCH 16/16] Change arround data byte capture math Changed the for loop to loop backwords itself so that the equations for actually getting the bytes only use +'s. They are still basically the same equations, but hopefully they are a little more intuitive... --- .../io_procs/AHRSInputIOProcessor.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 69bb90d..4d06a36 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -127,13 +127,13 @@ void AHRSInputIOProcessor::loop() /* This gets the index of the bytes for the Euler angles in the captured * buffer (szBuff). First, it finds the first byte of the message (i). Then, - * it finds the 14th index of the message (dataStart). This is where the last + * it finds the 7th index of the message (dataStart). This is where the first * byte of the roll angle is. This allows me loop backward through each group * of bytes, since the order needs to be reversed because the AHRS is in Big * Endian. * i is where the message starts in the buffer. - * dataStart = i + 14, which is where the data we want starts (for Roll). + * dataStart = i + 7, which is where the data we want starts (for Roll). * To find Pitch and Yaw, we add 8 and 16 respectivly. * * MESSAGE STRUCTURE: @@ -144,8 +144,8 @@ void AHRSInputIOProcessor::loop() * | R1 R2 R3 R4 R5 R6 R7 R8 | P1 P2 P3 P4 P5 P6 P7 P8 | Y1 Y2 Y3 Y4 Y5 Y6 Y7 Y8 | * +-------------------------+-------------------------+-------------------------+ * ... | 7 8 9 10 11 12 13 14 | 15 16 17 18 19 20 21 22 | 23 24 25 26 27 28 29 30 | ... - * | ^ | ^ | ^ | - * | dataStart| dataStart + 8| dataStart + 16| + * | ^ | ^ | ^ | + * |dataStart |dataStart + 8 |dataStart + 16 | * +-------------------------+-------------------------+-------------------------+ */ int dataStart = -1; @@ -153,7 +153,7 @@ void AHRSInputIOProcessor::loop() { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) { - dataStart = i + 14; + dataStart = i + 7; break; } } @@ -164,11 +164,11 @@ void AHRSInputIOProcessor::loop() // Puts the bytes for each angle in their own array backwards, since they are sent in big endian if (dataStart != -1) { - for (int i = 0; i < 8; i++) + for (int i = 7; i > -1; i--) { - rollArray[i] = szBuff[dataStart - i]; - pitchArray[i] = szBuff[dataStart - i + 8]; - yawArray[i] = szBuff[dataStart - i + 16]; + rollArray[i] = szBuff[dataStart + i]; + pitchArray[i] = szBuff[dataStart + i + 8]; + yawArray[i] = szBuff[dataStart + i + 16]; } }