From 64a5fc4787e97aee992f1a3f9f1320c823a9cf21 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Mon, 11 May 2020 13:18:01 -0400 Subject: [PATCH] Pull Request for AHRS IO Processor (#16) * Add AHRS IO Proc files into newer pull from dev * Fix names and built with python script * Fix pull request comments * 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. * Untabify AHRSInputIOProcessor.cpp * Hmmm * Revert "Hmmm" This reverts commit 5a9cb089a1776d449f8ef04eec6f8d10589bbea5. * Add functionality to get roll and pitch angles This reverts commit e2ea108b7b70deb8aa88b4c6dcdfd3edac5db2cf. * 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. * 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. * 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. * Fix Comments More I further clarified my comments and also renamed index to dataStart to make it more clear what it was. * Rename Attribute Keys * Fix type in comments Changed "index" to "dataStart" in diagram to make it more clear. * Add more specifics to Config message comments * 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... --- .../breadcrumbs/gen/AHRSInputIOProcessor.cpp | 23 +++ .../include/AHRSInputIOProcessor.hpp | 38 ++++ .../io_procs/AHRSInputIOProcessor.cpp | 184 ++++++++++++++++++ 3 files changed, 245 insertions(+) create mode 100644 bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp 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/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 new file mode 100644 index 0000000..dde9ca8 --- /dev/null +++ b/bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp @@ -0,0 +1,38 @@ +/* +* +* 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 "IOProcessor.hpp" +#include "DataSyncThread.hpp" +#include "Attribute.hpp" + +#include +#include + +class AHRSInputIOProcessor : 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 diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp new file mode 100644 index 0000000..4d06a36 --- /dev/null +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -0,0 +1,184 @@ +#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() +{ + 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; + + // 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)) + { + std::cerr << "Error occured while trying to send GoToConfig message." << std::endl; + return 6; + } + + // 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)) + { + std::cerr << "Error occured while trying to send SetOutputConfiguration message." << std::endl; + return 7; + } + + // 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)) + { + std::cerr << "Error occured while trying to send GoToMeasurment message." << std::endl; + return 8; + } + + configured = true; + return 0; +} + +bool AHRSInputIOProcessor::loopCondition() +{ + return true; +} + +void AHRSInputIOProcessor::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::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). First, it finds the first byte of the message (i). Then, + * 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 + 7, 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 + * +-------------------------+-------------------------+-------------------------+ + * | 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 | ... + * | ^ | ^ | ^ | + * |dataStart |dataStart + 8 |dataStart + 16 | + * +-------------------------+-------------------------+-------------------------+ + */ + int dataStart = -1; + for (int i=0; i < 32; i++) + { + if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) + { + dataStart = i + 7; + break; + } + } + + unsigned char rollArray[8]; + 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 (dataStart != -1) + { + for (int i = 7; i > -1; i--) + { + rollArray[i] = szBuff[dataStart + i]; + pitchArray[i] = szBuff[dataStart + i + 8]; + yawArray[i] = szBuff[dataStart + i + 16]; + } + } + + double doubleRoll = *reinterpret_cast(rollArray); + double doublePitch = *reinterpret_cast(pitchArray); + double doubleYaw = *reinterpret_cast(yawArray); + Attribute attRoll("XSIMUROL", 8, &doubleRoll); + getComms()->sendAttribute(attRoll); + Attribute attPitch("XSIMUPIT", 8, &doublePitch); + getComms()->sendAttribute(attPitch); + Attribute attYaw("XSIMUYAW", 8, &doubleYaw); + getComms()->sendAttribute(attYaw); +}