82 lines
2.6 KiB
C++
82 lines
2.6 KiB
C++
|
#include <windows.h>
|
||
|
|
#include <iostream>
|
||
|
|
#include <string>
|
||
|
|
#include <regex>
|
||
|
|
#include <mutex>
|
||
|
|
#include <thread>
|
||
|
|
#include "serialcomm.hpp"
|
||
|
|
|
||
|
|
HANDLE initSerialPort (const char* portName, DWORD baudRate) {
|
||
|
|
|
||
|
|
// open serial port
|
||
|
|
HANDLE hSerial = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
|
||
|
|
|
||
|
|
if (hSerial == INVALID_HANDLE_VALUE) {
|
||
|
|
std::cerr << "Error opening serial port: " << GetLastError() << std::endl;
|
||
|
|
return INVALID_HANDLE_VALUE;
|
||
|
|
}
|
||
|
|
|
||
|
|
// configure the serial port
|
||
|
|
DCB dcbSerialParams = {0};
|
||
|
|
dcbSerialParams.DCBlength = sizeof(dcbSerialParams);
|
||
|
|
|
||
|
|
if (!GetCommState(hSerial, &dcbSerialParams)) {
|
||
|
|
std::cerr << "Error getting comm state: " << GetLastError() << std::endl;
|
||
|
|
CloseHandle(hSerial);
|
||
|
|
return INVALID_HANDLE_VALUE;
|
||
|
|
}
|
||
|
|
|
||
|
|
dcbSerialParams.BaudRate = baudRate;
|
||
|
|
dcbSerialParams.ByteSize = 8;
|
||
|
|
dcbSerialParams.StopBits = ONESTOPBIT;
|
||
|
|
dcbSerialParams.Parity = NOPARITY;
|
||
|
|
|
||
|
|
if (!SetCommState(hSerial, &dcbSerialParams)) {
|
||
|
|
std::cerr << "Error getting comm state: " << GetLastError() << std::endl;
|
||
|
|
CloseHandle(hSerial);
|
||
|
|
return INVALID_HANDLE_VALUE;
|
||
|
|
}
|
||
|
|
|
||
|
|
// set timeouts
|
||
|
|
COMMTIMEOUTS timeouts = {0};
|
||
|
|
timeouts.ReadIntervalTimeout = 50;
|
||
|
|
timeouts.ReadTotalTimeoutConstant = 0;
|
||
|
|
timeouts.ReadTotalTimeoutMultiplier = 0;
|
||
|
|
if (!SetCommTimeouts(hSerial, &timeouts)) {
|
||
|
|
std::cerr << "Error setting timeout: " << GetLastError() << std::endl;
|
||
|
|
CloseHandle(hSerial);
|
||
|
|
return INVALID_HANDLE_VALUE;
|
||
|
|
}
|
||
|
|
|
||
|
|
return hSerial;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
bool readSerialData (HANDLE hSerial, char* buffer, DWORD bufferSize, DWORD& bytesRead) {
|
||
|
|
if (!ReadFile(hSerial, buffer, bufferSize - 1, &bytesRead, NULL)){
|
||
|
|
std::cerr << "Error reading from serial port: " << GetLastError() << std::endl;
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
buffer[bytesRead] = '\0';
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool parseQuaternion(const std::string& data, Quaternion& quat) {
|
||
|
|
// regex pattern to match the floats in the uart data stream from stm32
|
||
|
|
std::regex pattern("qw: ([+-]?\\d*\\.?\\d+) qx: ([+-]?\\d*\\.?\\d+) qy: ([+-]?\\d*\\.?\\d+) qz: ([+-]?\\d*\\.?\\d+)");
|
||
|
|
std::smatch matches;
|
||
|
|
|
||
|
|
// match every occurance to the desired quaternion value
|
||
|
|
if (std::regex_search(data, matches, pattern) && matches.size() == 5) {
|
||
|
|
// access the array like object std::smatch with [0] being the entire matched string
|
||
|
|
quat.w = std::stof(matches[1]);
|
||
|
|
quat.x = std::stof(matches[2]);
|
||
|
|
quat.y = std::stof(matches[3]);
|
||
|
|
quat.z = std::stof(matches[4]);
|
||
|
|
return true;
|
||
|
|
} else {return false;}
|
||
|
|
}
|
||
|
|
|
||
|
|
|