Serial read working, but doesn't do anything

.vscode/settings.json

@@ -1,4 +1,5 @@
 {
+  "cmake.configureSettings": {"CMAKE_TOOLCHAIN_FILE": "../utils/armv8-rpi3-linux-gnueabihf.cmake"},
   "editor.tabSize": 2,
   "files.eol": "\n",
   "files.insertFinalNewline": true,

src/SerialLineSensor/SerialLineSensor.cpp

@@ -63,72 +63,84 @@ SerialLineSensor::~SerialLineSensor() {
 
 void SerialLineSensor::ReceiverThread() {
   while (m_runThread.load()) {
-    std::cout << "Receive thread loop\n";
     // Connect
     while (m_runThread.load() && !m_connected) {
-      std::cout << "Connect loop\n";
-      m_serialPort = open(m_serialDeviceName.c_str(), O_RDONLY);
+      m_serialPort = open(m_serialDeviceName.c_str(), O_RDWR | O_NOCTTY);
       if (m_serialPort >= 0) {
         struct termios tty;
+
         if (tcgetattr(m_serialPort, &tty) != 0) {
-          std::cout << "Could not get attributes\n";
+          std::cerr << "Could not get attributes\n";
           close(m_serialPort);
           continue;
         }
 
-        tty.c_cflag &= ~PARENB;  // No parity bit
-        tty.c_cflag &= ~CSTOPB;  // One stop bit
+        // Set baudrate
+        cfsetispeed(&tty, B115200);
+        cfsetospeed(&tty, B115200);
+
+        // 8N1
+        tty.c_cflag &= ~PARENB;
+        tty.c_cflag &= ~CSTOPB;
         tty.c_cflag &= ~CSIZE;
-        tty.c_cflag |= CS8;                      // 8 data bits
-        tty.c_cflag &= ~CRTSCTS;                 // Disable RTS/CTS hardware flow control
-        tty.c_cflag |= CREAD | CLOCAL;           // Turn on READ & ignore ctrl lines (CLOCAL = 1)
-        tty.c_lflag &= ~ICANON;                  // Read lines
-        tty.c_iflag &= ~(IXON | IXOFF | IXANY);  // Turn off s/w flow ctrl
-        tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR |
-                         ICRNL);  // Disable any special handling of received bytes
-        tty.c_cc[VTIME] = 1;      // 100ms
-        tty.c_cc[VMIN] = 0;
-
-        cfsetispeed(&tty, B1152000);
-
-        if (tcsetattr(m_serialPort, TCSANOW, &tty) != 0) {
-          std::cout << "Could not set attributes\n";
+        tty.c_cflag |= CS8;
+
+        // Disable hardware based flow control
+        tty.c_cflag &= ~CRTSCTS;
+
+        // Enable receiver
+        tty.c_cflag |= CREAD | CLOCAL;
+
+        // Disable software based flow control
+        tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+        // Termois Non Canonical Mode
+        tty.c_lflag |= ICANON;
+
+        // Timeout in deciseconds for read
+        tty.c_cc[VTIME] = 1;
+
+        // Save tty
+        if (tcsetattr(m_serialPort, TCSANOW, &tty) < 0) {
           close(m_serialPort);
+          std::cerr << "Failed to configure port!\n";
+          continue;
+        }
+
+        // Flush RX Buffer
+        if (tcflush(m_serialPort, TCIFLUSH) < 0) {
+          close(m_serialPort);
+          std::cerr << "Failed to flush buffer!\n";
+          continue;
         }
 
         m_connected = true;
 
       } else {
-        std::cout << "Could not connect\n";
+        std::cerr << "Could not connect\n";
         std::this_thread::sleep_for(std::chrono::milliseconds(50));
       }
     }
 
     // Receive data
     while (m_runThread.load() && m_connected) {
-      std::cout << "Receive loop\n";
       char buf[256];
 
       int nBytes = read(m_serialPort, &buf, sizeof(buf));
 
       if (nBytes < 0) {
-        std::cout << "Bad data received\n";
+        std::cerr << "Bad data received\n";
         close(m_serialPort);
         m_connected = false;
       } else if (nBytes > 0) {
-        std::cout << "Raw: \"" << std::string_view(buf, nBytes) << "\"\n";
-        std::cout << "Raw2: \"";
-        for (int i = 0; i < nBytes; ++i) {
-          std::cout << (int)buf[i];
-        }
-        std::cout << "\"\n";
         auto rawStates = ParseMessage(std::string_view(buf, nBytes));
         if (rawStates) {
           std::scoped_lock lock(m_dataMutex);
           m_currentLeft = rawStates.value().left;
           m_currentCenter = rawStates.value().center;
           m_currentRight = rawStates.value().right;
           m_lastUpdateTime = std::chrono::steady_clock::now();
+          // std::cout << rawStates.value().left << ' ' << rawStates.value().center << ' ' << rawStates.value().right << '\n';
         }
       }
     }
@@ -142,11 +154,11 @@ void SerialLineSensor::ReceiverThread() {
 
 [[nodiscard]] std::optional<RawSensorArrayStatus> SerialLineSensor::ParseMessage(std::string_view message) {
   if (message.size() < 25) {
-    std::cout << "Not enough data (" << message.size() << ")\n";
+    std::cerr << "Not enough data (" << message.size() << ")\n";
     return std::nullopt;
   }
   if (message.substr(0, 3) != "l: " || message.substr(7, 5) != ", c: " || message.substr(16, 5) != ", r: ") {
-    std::cout << "Didn't find stuff\n";
+    std::cerr << "Didn't find stuff\n";
     return std::nullopt;
   }
   try {
@@ -155,10 +167,10 @@ void SerialLineSensor::ReceiverThread() {
         .center = static_cast<uint16_t>(std::stoul(std::string(message.substr(12, 4)), nullptr, 10)),
         .right = static_cast<uint16_t>(std::stoul(std::string(message.substr(21, 4)), nullptr, 10))};
   } catch (std::invalid_argument&) {
-    std::cout << "Invalid argument\n";
+    std::cerr << "Invalid argument\n";
     return std::nullopt;
   } catch (std::out_of_range&) {
-    std::cout << "Out of range\n";
+    std::cerr << "Out of range\n";
     return std::nullopt;
   }
 }