Angefangen zweiten Sensor Readout zu implementieren. Funktioniert auch teils

main.cpp

@@ -14,6 +14,9 @@
 #include <string>
 #include <mutex>
 #include "visualization.hpp"
+#include <iostream>
+#include <algorithm>
+#include <iomanip>
 #include <vector>
 #include "include/glm/glm.hpp"
 #include "include/glm/gtc/matrix_transform.hpp"
@@ -65,9 +68,11 @@ void serialParserThread() {
     // define communication variables
     const char* portName = "COM3"; 
     DWORD baudRate = CBR_115200;   
-    char buffer[64];
     DWORD bytesRead;
-    std::string incompleteDataBuffer;
+    //char buffer[64];
+    //std::string incompleteDataBuffer;
+    uint8_t buffer[64];
+    std::vector<uint8_t> binaryPacketBuffer;
 
     // establish communication to stm32
     g_serialHandle = initSerialPort(portName, baudRate);
@@ -77,24 +82,34 @@ void serialParserThread() {
         return;
     } else {std::cout << "Serial connection succesfully established" << std::endl;}
 
+
+// reinterpret_cast<char*>(buffer)
     // serial reading loop
     while (g_running) {
-        if (readSerialData(g_serialHandle, buffer, sizeof(buffer), bytesRead)) {
-                if (bytesRead > 0) {
-                // add newly arrived data to buffer
-                buffer[bytesRead] = '\0';
-                incompleteDataBuffer += buffer;
+        if (readSerialData(g_serialHandle, reinterpret_cast<uint8_t*>(buffer), sizeof(buffer), bytesRead)) {
+            if (bytesRead > 0) {
+                /*std::cout << "Received " << bytesRead << " bytes: ";
+                for (DWORD i = 0; i < bytesRead && i < 10; ++i) {  // Print first 10 bytes
+                    std::cout << std::hex << std::setfill('0') << std::setw(2) << (int)buffer[i] << " ";
+                }
+                std::cout << std::dec << std::endl; */
+                // add newly arrived binary data to buffer
+                //buffer[bytesRead] = '\0';
+                //incompleteDataBuffer += buffer;
+                binaryPacketBuffer.insert(binaryPacketBuffer.end(), buffer, buffer+bytesRead);
 
-                // process any complete lines in the buffer
-                size_t pos;
-                while ((pos = incompleteDataBuffer.find('\n')) != std::string::npos) {
-                    // create substring of completed line and erase it from incompleteDataBuffer
+                // process any complete packets
+                /* size_t pos; */
+                while (true) {
+/*                     // create substring of completed line and erase it from incompleteDataBuffer
                     std::string completeLine = incompleteDataBuffer.substr(0, pos);
-                    incompleteDataBuffer.erase(0, pos + 1);
+                    incompleteDataBuffer.erase(0, pos + 1); */
+                    std::vector<uint8_t> packet = extractPacket(binaryPacketBuffer);
+                    if (packet.empty()) {break;}
 
                     // now parse the quaternions from line
                     Quaternion quat;
-                    if (parseQuaternion(completeLine, quat)) {
+                    if (parseBinaryPacket(packet, quat)) {
                         // lock mutex while updating it
                         std::lock_guard<std::mutex> lock(g_mutex);
                         g_currQuat = quat;
@@ -109,6 +124,7 @@ void serialParserThread() {
                 
             } 
         }
+        //std::this_thread::sleep_for(std::chrono::milliseconds(5));
 
     }