Rebuild trinity visualizer from bare SDL and OpenGL to using Raylib. Added Code for serial parsing on linux. Current functionality reads incoming quaternion packet data coming in over serial and displays the values and also visualizes with a cube, connect and disconnect is implemented. Essentially rebuild the functionality of the old version with added linux support.
This commit is contained in:
+236
@@ -0,0 +1,236 @@
|
||||
#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>
|
||||
#include <cstring>
|
||||
#endif
|
||||
|
||||
#include <iostream>
|
||||
#include "raymath.h"
|
||||
#include <string.h>
|
||||
#include <regex>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include "serialcomm.hpp"
|
||||
#include <iomanip>
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
|
||||
// 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 linux_initSerialPort(){
|
||||
struct termios tty;
|
||||
|
||||
int linux_serialPort = open("/dev/ttyACM1", O_RDWR);
|
||||
|
||||
// read in existing struct settings and handle errors
|
||||
if (tcgetattr(linux_serialPort, &tty) != 0) {
|
||||
std::cout << "Error " << errno << " opening serial port: " << strerror(errno) << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// set the serial settings
|
||||
tty.c_cflag &= ~PARENB;
|
||||
tty.c_cflag &= ~CSTOPB;
|
||||
tty.c_cflag &= ~CSIZE;
|
||||
tty.c_cflag |= CS8;
|
||||
tty.c_cflag &= ~CRTSCTS;
|
||||
tty.c_cflag |= CREAD | CLOCAL;
|
||||
// set to raw mode - disable all interpretation/special handling of stream binary data
|
||||
cfmakeraw(&tty);
|
||||
|
||||
tty.c_cc[VTIME] = 5; // 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 " << errno << " saving serial port settings: " << strerror(errno) << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
return linux_serialPort;
|
||||
}
|
||||
|
||||
bool linux_readSerialData (int serialPort, std::vector<uint8_t> &buffer, size_t& bytes_read) {
|
||||
ssize_t read_return = read(serialPort, buffer.data(), buffer.size());
|
||||
|
||||
if (read_return < 0 ) {
|
||||
std::cout << "Error " << errno << " reading serial port data: " << strerror(errno) << std::endl;
|
||||
bytes_read = 0;
|
||||
return false;
|
||||
}
|
||||
bytes_read = read_return;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool linux_close_serial_port(int serial_port) {
|
||||
return close(serial_port);
|
||||
}
|
||||
|
||||
void print_vector_content(std::vector<uint8_t> &vec, std::string vector_name) {
|
||||
std::cout << vector_name << ": ";
|
||||
for (int i = 0; i < vec.size(); i++) {
|
||||
// std::cout << buff[i] << " ";
|
||||
// }
|
||||
// std::cout << std::endl;
|
||||
std::cout << std::hex << std::setw(2) << std::setfill('0')
|
||||
<< std::uppercase << (static_cast<unsigned int>(vec[i]) & 0xFF) << " ";
|
||||
if ((i + 1) % 16 == 0) std::cout << "\n";
|
||||
}
|
||||
std::cout << std::dec;
|
||||
}
|
||||
|
||||
#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 {};
|
||||
//continue;
|
||||
}
|
||||
|
||||
// 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 {};
|
||||
continue;
|
||||
}
|
||||
|
||||
// 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 {};
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user