231 lines
7.5 KiB
C++
231 lines
7.5 KiB
C++
#include <cstdint>
|
|
#include <vector>
|
|
#ifdef _WIN32
|
|
#include <windows.h>
|
|
#elif defined(__APPLE__)
|
|
#elif defined(__linux__)
|
|
#include <fcntl.h>
|
|
#include <errno.h>
|
|
#include <termios.h>
|
|
#include <unistd.h>
|
|
#endif
|
|
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <regex>
|
|
#include <mutex>
|
|
#include <thread>
|
|
#include "serialcomm.hpp"
|
|
#include <iomanip>
|
|
#include <algorithm>
|
|
|
|
// constants for the uart packet of quaterniondata
|
|
constexpr uint8_t PACKET_START_BYTE = 0xDE;
|
|
constexpr uint8_t PACKET_END_BYTE = 0xAD;
|
|
constexpr size_t PACKET_SIZE = sizeof(QuaternionData);
|
|
|
|
|
|
#ifdef _WIN32
|
|
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, uint8_t* 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;
|
|
}
|
|
// dont needed für binary stream denke ich
|
|
//buffer[bytesRead] = '\0';
|
|
return true;
|
|
}
|
|
|
|
#elif defined(__APPLE__)
|
|
#elif defined(__linux__)
|
|
int initSerialPort(){
|
|
struct termios tty;
|
|
|
|
int linux_serialPort = open("/dev/ttyUSB0", 0_RDWR);
|
|
|
|
// read in existing struct settings and handle errors
|
|
if (tcgetattr(linux_serialPort, &tty) !0 0) {
|
|
std::cout << "Error %i opening serial port: %s\n", errno, strerror(errno) << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
// set the serial settings
|
|
tty.c_flag &= ~PARENB;
|
|
tty.c_flag &= ~CSTOPB;
|
|
tty.c_flag &= ~CSIZE;
|
|
tty.c_flag |= CS8;
|
|
tty.c_flag &= ~CRTSCTS;
|
|
tty.c_flag |= CREAD | CLOCAL;
|
|
|
|
tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds), returning as soon as any data is received.
|
|
tty.c_cc[VMIN] = 0;
|
|
|
|
// Set in/out baud rate to be 9600
|
|
cfsetispeed(&tty, B115200);
|
|
cfsetospeed(&tty, B115200);
|
|
|
|
// save the settings and check for errors
|
|
if (tcsetattr(linux_serialPort, TCSANOW, &tty) != 0 ) {
|
|
std::cout << "Error %i saving serial port settings: %s\n", errno, strerror(errno) << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
return linux_serialPort;
|
|
}
|
|
|
|
bool linux_readSerialData (int serialPort, char* buffer) {
|
|
if (read(serialPort, &buffer, sizeof(buffer)) < 0 ) {
|
|
std::cout << "Error reading serial port data: %s", strerror(errno)) << std::endl;
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
|
|
#endif
|
|
|
|
|
|
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;}
|
|
}
|
|
|
|
bool parseBinaryPacket(std::vector<uint8_t> &packetData, Quaternion &quat) {
|
|
if (packetData.size() != PACKET_SIZE) {
|
|
std::cout << "Wrong packet size detected" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
// cast raw bytes to struct
|
|
QuaternionData* packet = reinterpret_cast<QuaternionData*>(packetData.data());
|
|
|
|
// verify start and end bytes
|
|
if (packet->StartByte != PACKET_START_BYTE || packet->EndByte != PACKET_END_BYTE) {
|
|
std::cout << "Mismatch in packet end- or startbyte size" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
quat.w = packet->qw;
|
|
quat.x = packet->qx;
|
|
quat.y = packet->qy;
|
|
quat.z = packet->qz;
|
|
|
|
return true;
|
|
}
|
|
|
|
std::vector<uint8_t> extractPacket(std::vector<uint8_t> &buffer) {
|
|
while (buffer.size() >= PACKET_SIZE) {
|
|
// find start byte
|
|
auto startIT = std::find(buffer.begin(), buffer.end(), PACKET_START_BYTE);
|
|
if (startIT == buffer.end()) {
|
|
// clear buffer because no start byte found
|
|
buffer.clear();
|
|
std::cout << "Buffer cleared - no start byte found" << std::endl;
|
|
return {};
|
|
}
|
|
|
|
//remove data before the start byte
|
|
if (startIT != buffer.begin()){
|
|
buffer.erase(buffer.begin(), startIT);
|
|
return {};
|
|
}
|
|
|
|
// check for enough enough bytes for whole packet
|
|
if (buffer.size() < PACKET_SIZE) {
|
|
return {};
|
|
}
|
|
|
|
// check for end byte at correct position
|
|
if (buffer[PACKET_SIZE -1] != PACKET_END_BYTE) {
|
|
buffer.erase((buffer.begin()));
|
|
return {};
|
|
}
|
|
|
|
// now extract the valid packet
|
|
std::vector<uint8_t> validPacket(buffer.begin(), buffer.begin() + PACKET_SIZE);
|
|
buffer.erase(buffer.begin(), buffer.begin() + PACKET_SIZE);
|
|
return validPacket;
|
|
}
|
|
return {};
|
|
}
|
|
|
|
void checkPacketSize() {
|
|
std::cout << "QuaternionData struct size: " << sizeof(QuaternionData) << " bytes" << std::endl;
|
|
std::cout << "Expected layout:" << std::endl;
|
|
std::cout << " StartByte: " << sizeof(uint8_t) << " byte" << std::endl;
|
|
std::cout << " SensorAddress: " << sizeof(uint8_t) << " byte" << std::endl;
|
|
std::cout << " qw: " << sizeof(float) << " bytes" << std::endl;
|
|
std::cout << " qx: " << sizeof(float) << " bytes" << std::endl;
|
|
std::cout << " qy: " << sizeof(float) << " bytes" << std::endl;
|
|
std::cout << " qz: " << sizeof(float) << " bytes" << std::endl;
|
|
std::cout << " EndByte: " << sizeof(uint8_t) << " byte" << std::endl;
|
|
std::cout << " Total expected: " << (3 * sizeof(uint8_t) + 4 * sizeof(float)) << " bytes" << std::endl;
|
|
}
|
|
|
|
// Add this debug function to serialcomm.cpp
|
|
void printBufferHex(const std::vector<uint8_t>& buffer, const std::string& label) {
|
|
std::cout << label << " (" << buffer.size() << " bytes): ";
|
|
for (size_t i = 0; i < buffer.size() && i < 60; ++i) { // Print first 20 bytes
|
|
std::cout << std::hex << std::setfill('0') << std::setw(2) << (int)buffer[i] << " ";
|
|
}
|
|
if (buffer.size() > 20) std::cout << "...";
|
|
std::cout << std::dec << std::endl;
|
|
}
|
|
|