Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
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 5a9cb08.

* Add functionality to get roll and pitch angles

This reverts commit e2ea108.

* 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...
  • Loading branch information
@mfs16101
mfs16101 committed May 11, 2020
1 parent c3619f3 commit 64a5fc4
Show file tree
Hide file tree
Showing 3 changed files with 245 additions and 0 deletions.
23 changes: 23 additions & 0 deletions bfs/implementations/breadcrumbs/gen/AHRSInputIOProcessor.cpp
View file
@@ -0,0 +1,23 @@

#include <iostream>

#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;
}
38 changes: 38 additions & 0 deletions bfs/implementations/breadcrumbs/include/AHRSInputIOProcessor.hpp
View file
@@ -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 <windows.h>
#include <iostream>

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
184 changes: 184 additions & 0 deletions bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp
View file
@@ -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<double* const>(rollArray);
double doublePitch = *reinterpret_cast<double* const>(pitchArray);
double doubleYaw = *reinterpret_cast<double * const>(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);
}

0 comments on commit 64a5fc4

Please sign in to comment.