Angefangen zweiten Sensor Readout zu implementieren. Funktioniert auch teils

serialcomm.cpp

@@ -1,3 +1,5 @@
+#include <cstdint>
+#include <vector>
 #ifdef _WIN32
    #include <windows.h>
 #elif defined(__APPLE__)
@@ -14,6 +16,14 @@
 #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) {
@@ -62,13 +72,13 @@
 
 }
 
-bool readSerialData (HANDLE hSerial, char* buffer, DWORD bufferSize, DWORD& bytesRead) {
+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;
     }
-
-    buffer[bytesRead] = '\0';
+   // dont needed für binary stream denke ich
+    //buffer[bytesRead] = '\0';
     return true;
 }
 
@@ -136,4 +146,85 @@ bool parseQuaternion(const std::string& data, Quaternion& quat) {
    } 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;
+}