Pull Request AHRS IO proc into dev #15
Changes from all commits
041d9d1
2bf62d7
d74d937
eb72ddf
9c10356
56606fd
ce5067f
d742a0e
078ad70
1769611
db6d035
ae2a8a5
dc725ab
d709efa
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <windows.h> | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. C++ library includes (the ones with <>) should be above the other ones with a space in between for consistency's sake |
||
#include <iostream> | ||
|
||
class AHRSOutputIOProcessor : public IOProcessor | ||
{ | ||
public: | ||
using IOProcessor::IOProcessor; | ||
|
||
void loop(); | ||
bool loopCondition(); | ||
int configAHRS(); // Initialize the AHRS for the first time | ||
|
||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Extra space |
||
private: | ||
int iterations = 10; | ||
bool configured = false; | ||
HANDLE hSerial; | ||
|
||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Get rid of this extra space |
||
}; | ||
|
||
#endif | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. You need a newline at the end of the file |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,150 @@ | ||
#include "AHRSOutputIOProcessor.hpp" | ||
|
||
|
||
int AHRSOutputIOProcessor::configAHRS() | ||
{ | ||
DCB dcb = {0}; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Declare this when you need it, not here at the top. It makes it less confusing as to it's purpose |
||
//HANDLE hSerial; | ||
mfs16101 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
||
hSerial = CreateFile("COM8", // The COM port to connect to, may have to be changed | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This "COM8" value should not be hard coded. At the very least, it should be a const somewhere. Com ports are also machine specific and their numbers depend on what com port number is available at the time based on how many are in use by your machine at one time. COM ports start at 0 so what you should do is try each one until the device is configured properly if the default (in this case com8) does not work. |
||
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. | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. These return values should be specified in a block comment above this function, not in inline comments. Get rid of these comments and add a block comment at the beginning of the function with this info |
||
} | ||
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. | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Same as above regarding comments on return statements |
||
} | ||
|
||
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 | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Same as above regarding comments on return statements |
||
} | ||
|
||
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; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Don't forget to add this one to the comment above |
||
} | ||
|
||
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 | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Instead of saying that this is the config here, make this a constant in the io processor class definition and talk in more detail about what each configured bit/byte does. Otherwise it is impossible to tell what it does without combing through the docs. Add a reference to the page(s) in the docs where it is explained also. |
||
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; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Same as above |
||
} | ||
|
||
unsigned char wBuff2[] = {0xFA,0xFF,0xC0,0x04,0x20,0x33,0x00,0x00,0xEA}; // This is the SetOutputConfiguration message | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Make this a const and leave a better comment like I described above. |
||
dwBytesWritten = 0; | ||
|
||
if (!WriteFile(hSerial, wBuff2, 10, &dwBytesWritten, NULL)) | ||
{ | ||
std::cout << "Error occured while trying to send SetOutputConfiguration message." << std::endl; | ||
return 7; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Same as above |
||
} | ||
|
||
unsigned char wBuff3[] = {0xFA,0xFF,0x10,0x00,0xF1}; // This is the GoToMeasurment message | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Make this a const and leave a better comment like I described above. |
||
dwBytesWritten = 0; | ||
|
||
if (!WriteFile(hSerial, wBuff3, 6, &dwBytesWritten, NULL)) | ||
{ | ||
std::cout << "Error occured while trying to send GoToMeasurment message." << std::endl; | ||
return 8; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Same as above |
||
} | ||
|
||
configured = true; | ||
return 0; | ||
} | ||
|
||
bool AHRSOutputIOProcessor::loopCondition() | ||
{ | ||
return iterations > 0; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Why do you need this iterations variable? Shouldn't this just return true to loop forever? |
||
} | ||
|
||
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; | ||
mfs16101 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
return 9; | ||
mfs16101 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
} | ||
|
||
int index = -1; | ||
|
||
mfs16101 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
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]; | ||
|
||
mfs16101 marked this conversation as resolved.
Show resolved
Hide resolved
|
||
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); | ||
|
||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. It looks like this is just getting the yaw value, we need the pitch roll and yaw though. |
||
//std::cout << "Yaw: " << doubleYaw << std::endl; | ||
|
||
Attribute attrib("yawAngle", 8, &doubleYaw); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This key should be declared inside a header file as a #define. Leave it for now, because Nate has the header file inside his branch. When his merges, you can add this and any other attribute keys to the header file. |
||
getComs()->sendAttribute(attriv); | ||
iterations--; | ||
|
||
if (iterations == 0) | ||
CloseHandle(hSerial); | ||
} | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Need a newline at the end of this file |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,51 @@ | ||
/* | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This is really old code that I wrote a long time ago, not sure why it is in here. Get rid of it. |
||
* | ||
* Stores the interface for building a sensor thread | ||
* implementation. | ||
* | ||
*/ | ||
|
||
#include <Windows.h> | ||
#include <stdio.h> | ||
|
||
|
||
class SensorThread | ||
{ | ||
public: | ||
|
||
// Thread start 'caller' function | ||
static DWORD __stdcall threadStart(LPVOID pUserData) { | ||
((SensorThread*) pUserData)->threadRuntime(); | ||
return 0; | ||
} | ||
// Thread routine for implementation to override | ||
virtual void threadRuntime() = 0; | ||
|
||
// Async control | ||
UINT8 startThread(LPVOID targs)s | ||
{ | ||
threadArgs = targs; | ||
hThread = CreateThread( | ||
NULL, // default security attributes | ||
0, // use default stack size | ||
threadStart, // thread function name | ||
this, // argument to thread function | ||
0, // use default creation flags | ||
&dwThreadId); // returns the thread identifier | ||
return TRUE; | ||
}; | ||
BOOL waitForThread() | ||
{ | ||
WaitForSingleObject(hThread, INFINITE); | ||
return TRUE; | ||
}; | ||
|
||
// Sync control | ||
BOOLEAN bufferDataAvailable() { return TRUE; }; | ||
SIZE_T getBufferData(LPCSTR* bufferKeyArray, LPCSTR* bufferValueArray) { return 0; }; | ||
VOID setDataStoreValue(LPCSTR key, LPCVOID value, SIZE_T valueSize) {}; | ||
protected: | ||
LPCVOID threadArgs; | ||
DWORD dwThreadId; | ||
HANDLE hThread; | ||
}; |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,8 @@ | ||
|
||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. You shouldn't be changing the virtual output processor. |
||
#include "VirtualOutputProcessor.hpp" | ||
|
||
|
||
void VirtualOutputProcessor::threadRuntime() | ||
{ | ||
printf("Hello from thread %d!\n", *((int*) threadArgs)); | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,11 @@ | ||
|
||
#include <stdio.h> | ||
|
||
#include "../SensorInterface.hpp" | ||
|
||
|
||
class VirtualOutputProcessor : public SensorThread | ||
{ | ||
public: | ||
virtual void threadRuntime(); | ||
}; |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
#include <stdio.h> | ||
|
||
#include "io/out_procs/VirtualOutputProcessor.hpp" | ||
|
||
|
||
#define THREADS 5 | ||
|
||
|
||
int main() { | ||
printf("Hello World!\n"); | ||
|
||
SensorThread* procs[THREADS]; | ||
int targs[THREADS]; | ||
|
||
for (int i = 0; i < THREADS; i++) { | ||
procs[i] = new VirtualOutputProcessor; | ||
targs[i] = i + 1; | ||
procs[i]->startThread(targs + i); | ||
} | ||
|
||
for (int i = 0; i < THREADS; i++) { | ||
procs[i]->waitForThread(); | ||
delete procs[i]; | ||
} | ||
return 0; | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,66 @@ | ||
#include <iostream> | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Why is this serialPortComms folder here? If it is a library then it is in the wrong place |
||
#include <vector> | ||
using namespace std; | ||
|
||
struct HexCharStruct // Fixes char to hex stuff? | ||
{ | ||
unsigned char c; | ||
HexCharStruct(unsigned char _c) : c(_c) { } | ||
}; | ||
|
||
inline ostream& operator<<(ostream& o, const HexCharStruct& hs) | ||
{ | ||
return (o << hex << (int)hs.c); | ||
} | ||
|
||
inline HexCharStruct hex(unsigned char _c) | ||
{ | ||
return HexCharStruct(_c); | ||
} | ||
|
||
unsigned char calChecksum(const vector<unsigned char> msg) | ||
{ | ||
unsigned char sum = 0; | ||
|
||
for (int i=1; i < msg.size(); i++) | ||
{ | ||
sum += msg[i]; | ||
} | ||
|
||
unsigned char check = 0x100 - (sum & 0xFF); | ||
|
||
return check; | ||
} | ||
|
||
|
||
// int main() // Use vector so that it can be trivialy long. | ||
// { | ||
// unsigned char preamb = 0xFA; | ||
// unsigned char BID = 0xFF; | ||
// unsigned char MID = 0x30; | ||
// unsigned char LEN = 0x00; | ||
|
||
// //unsigned char check1 = 0x100 - ((BID + MID + LEN) & 0xFF); // This outputs the correct checksum. | ||
|
||
// vector<unsigned char> mess; | ||
|
||
// mess.push_back(preamb); | ||
// mess.push_back(BID); | ||
// mess.push_back(MID); | ||
// mess.push_back(LEN); | ||
// // Data chars would go here | ||
|
||
// unsigned char check = calChecksum(mess); | ||
|
||
// //cout << "Checksum is: " << hex(check) << endl; | ||
// mess.push_back(check); | ||
|
||
// cout << "GoToConfig message: "; | ||
|
||
// for (unsigned char c: mess) // Could instead be adding these to the buffer | ||
// cout << hex(c) << " "; | ||
|
||
// cout << endl; | ||
|
||
// return 0; | ||
// } |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,12 @@ | ||
#include <iostream> | ||
|
||
using namespace std; | ||
|
||
int main() | ||
{ | ||
unsigned char array[] = {0x00, 0x00, 0x00, 0x20, 0xDB, 0xE2, 0xC4, 0x3F}; //0x3F, 0xC4, 0xE2, 0xDB, 0x20, 0x00, 0x00, 0x00 | ||
|
||
double doubleVal = *reinterpret_cast<double * const>(array); | ||
|
||
cout << doubleVal << endl; | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Add a block comment at the top explaining what AHRS is, what the hardware does, and a link to the product page or something.