Trinity-Control/serialcomm.cpp

#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 {};
 }