Merge pull request 'Better printer task & Bug fix' (#68) from blochm/fsfw:bloch/improve-printout into main

Reviewed-on: #68
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -1268,7 +1268,7 @@ ReturnValue_t DeviceHandlerBase::letChildHandleMessage(CommandMessage* message)
 
 void DeviceHandlerBase::handleDeviceTm(const uint8_t* rawData, size_t rawDataLen,
                                        DeviceCommandId_t replyId, bool forceDirectTm) {
-  SerialBufferAdapter bufferWrapper(rawData, rawDataLen);
+  SerialBufferAdapter<uint32_t> bufferWrapper(rawData, rawDataLen);
   handleDeviceTm(bufferWrapper, replyId, forceDirectTm);
 }
 

src/fsfw/serviceinterface/CMakeLists.txt

@@ -1,5 +1,4 @@
 target_sources(
   ${LIB_FSFW_NAME}
   PRIVATE ServiceInterfaceStream.cpp ServiceInterfaceBuffer.cpp
-          ServiceInterfacePrinter.cpp ServiceInterfacePrinterTask.cpp
-          ServiceInterfacePrinterMessage.cpp)
+          ServiceInterfacePrinter.cpp ServiceInterfacePrinterTask.cpp)

src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp

@@ -1,13 +1,12 @@
 #include "fsfw/serviceinterface/ServiceInterfacePrinter.h"
 
+#include <algorithm>
 #include <cstdarg>
+#include <cstring>
 
-#include "ServiceInterfacePrinterMessage.h"
-#include "etl/bitset.h"
-#include "etl/queue.h"
 #include "fsfw/FSFW.h"
+#include "fsfw/ipc/MutexFactory.h"
 #include "fsfw/ipc/MutexGuard.h"
-#include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
 #include "fsfw/timemanager/Clock.h"
 
@@ -18,33 +17,32 @@ static bool consoleInitialized = false;
 
 #if FSFW_DISABLE_PRINTOUT == 0
 
-typedef etl::bitset<fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT> bitset;
+namespace {
 
-static bool addCrAtEnd = false;
-static bool initializedAndReady = false;
-static bool replaceLastCharWithNewline = false;
+// Formatted messages are staged in a ring buffer and drained by the print task
+// in bounded chunks, so producers never block on the slow debug UART.
+constexpr size_t RING_SIZE =
+    fsfwconfig::FSFW_PRINT_BUFFER_SIZE * fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT;
 
-bitset bufferState{};
-std::array<char[fsfwconfig::FSFW_PRINT_BUFFER_SIZE], fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT>
-    printBufferArray = {};
+constexpr size_t MAX_BYTES_PER_CYCLE = 512;
 
-uint32_t droppedMessagesCounter = 0;
-uint32_t droppedMessagesCounterContinuous = 0;
+// The print ring buffer
+char ring[RING_SIZE];
+MutexIF* ringMutex = nullptr;
 
-MutexIF* bufferMutex;
-MessageQueueIF* bufferQueue;
+size_t readIdx = 0;    // Start of data to print out
+size_t bytesUsed = 0;  // Pending data amount to print out
+uint32_t droppedMessages = 0;
+uint32_t droppedMessagesTotal = 0;
 
-size_t selectFreeBuffer() {
-  MutexGuard guard(bufferMutex, MutexIF::TimeoutType::WAITING, 10);
-  const size_t bufferIdx = bufferState.find_first(false);
-  if (bufferIdx == bitset::npos) {
-    return bitset::npos;
-  }
-  bufferState.set(bufferIdx, true);
-  return bufferIdx;
-}
+bool addCrAtEnd = false;
+bool replaceLastCharWithNewline = false;
 
-void fsfwPrint(const sif::PrintLevel printType, const char* fmt, va_list arg) {
+// False until the print task runs.
+// Prints go directly to stdout before that.
+bool taskRunning = false;
+
+void fsfwPrint(sif::PrintLevel printType, const char* fmt, va_list arg) {
 #if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
   if (not consoleInitialized) {
     HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
@@ -56,89 +54,73 @@ void fsfwPrint(const sif::PrintLevel printType, const char* fmt, va_list arg) {
   consoleInitialized = true;
 #endif
 
-  /* Check logger level */
   if (printType == sif::PrintLevel::NONE or printType > printLevel) {
     return;
   }
 
-  const bool isReady = initializedAndReady;
-  const size_t bufferIdx = isReady ? selectFreeBuffer() : 0;
-  if (bufferIdx == bitset::npos) {
-    // This will lose updates, but we are mostly concerned with if we are dropping messages
-    ++droppedMessagesCounter;
-    return;
-  }
-
-  size_t len = 0;
-  char* bufferPosition = printBufferArray[bufferIdx];
-
-  /* Log message to terminal */
-
+  static const char* const labels[] = {"", "ERROR  ", "WARNING", "INFO   ", "DEBUG  "};
 #if FSFW_COLORED_OUTPUT == 1
-  if (printType == sif::PrintLevel::INFO_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_GREEN);
-  } else if (printType == sif::PrintLevel::DEBUG_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_CYAN);
-  } else if (printType == sif::PrintLevel::WARNING_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_YELLOW);
-  } else if (printType == sif::PrintLevel::ERROR_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_RED);
-  }
-#endif
-
-  if (printType == sif::PrintLevel::INFO_LEVEL) {
-    len += sprintf(bufferPosition + len, "INFO   ");
-  }
-  if (printType == sif::PrintLevel::DEBUG_LEVEL) {
-    len += sprintf(bufferPosition + len, "DEBUG  ");
-  }
-  if (printType == sif::PrintLevel::WARNING_LEVEL) {
-    len += sprintf(bufferPosition + len, "WARNING");
-  }
-  if (printType == sif::PrintLevel::ERROR_LEVEL) {
-    len += sprintf(bufferPosition + len, "ERROR  ");
-  }
-
-#if FSFW_COLORED_OUTPUT == 1
-  len += sprintf(bufferPosition + len, sif::ANSI_COLOR_RESET);
+  static const char* const colors[] = {"", sif::ANSI_COLOR_RED, sif::ANSI_COLOR_YELLOW,
+                                       sif::ANSI_COLOR_GREEN, sif::ANSI_COLOR_CYAN};
+  const char* color = colors[printType];
+  const char* reset = sif::ANSI_COLOR_RESET;
+#else
+  const char* color = "";
+  const char* reset = "";
 #endif
 
   Clock::TimeOfDay_t now;
   Clock::getDateAndTime(&now);
-  /*
-   * Log current time to terminal if desired.
-   */
-  len += sprintf(bufferPosition + len, " | %02lu:%02lu:%02lu.%03lu | ", (unsigned long)now.hour,
-                 (unsigned long)now.minute, (unsigned long)now.second,
-                 (unsigned long)now.usecond / 1000);
 
-  len += vsnprintf(bufferPosition + len, sizeof(printBufferArray[bufferIdx]) - len, fmt, arg);
+  char buf[fsfwconfig::FSFW_PRINT_BUFFER_SIZE + 2];  // slack for the CR/newline
 
-  if (addCrAtEnd) {
-    len += sprintf(bufferPosition + len, "\r");
+  // Need to clamp, since snprintf returns WOULD be written length (not actual; could be higher than
+  // really)
+  const auto prefixLen =
+      std::min(static_cast<size_t>(snprintf(
+                   buf, fsfwconfig::FSFW_PRINT_BUFFER_SIZE, "%s%s%s | %02u:%02u:%02u.%03u | ",
+                   color, labels[printType], reset, static_cast<unsigned>(now.hour),
+                   static_cast<unsigned>(now.minute), static_cast<unsigned>(now.second),
+                   static_cast<unsigned>(now.usecond / 1000))),
+               fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1);
+
+  // Same here
+  const auto msgLen =
+      std::min(static_cast<size_t>(vsnprintf(
+                   buf + prefixLen, fsfwconfig::FSFW_PRINT_BUFFER_SIZE - prefixLen, fmt, arg)),
+               fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1 - prefixLen);
+
+  size_t textLen = prefixLen + msgLen;
+  if (addCrAtEnd and buf[textLen - 1] == '\n') {
+    buf[textLen++] = '\r';
+  }
+  if (replaceLastCharWithNewline and buf[textLen - 1] != '\n' and buf[textLen - 1] != '\r') {
+    buf[textLen++] = '\n';
   }
 
-  printBufferArray[bufferIdx][fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1] = 0;
-
-  if (replaceLastCharWithNewline) {
-    const size_t stringLength = strlen(bufferPosition);
-    const size_t lastCharPosition =
-        etl::min(stringLength - 1, fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 3);
-    const char lastChar = printBufferArray[bufferIdx][lastCharPosition];
-    if (!(lastChar == '\n' or lastChar == '\r')) {
-      printBufferArray[bufferIdx][lastCharPosition + 1] = '\n';
-      printBufferArray[bufferIdx][lastCharPosition + 2] = 0;
-    }
+  if (not taskRunning) {
+    fwrite(buf, 1, textLen, stdout);
+    return;
   }
 
-  if (isReady) {
-    ServiceInterfacePrinterMessage message(bufferIdx);
-    bufferQueue->sendToDefault(&message);
-  } else {
-    printf("%s", printBufferArray[bufferIdx]);
+  MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+
+  if (textLen > RING_SIZE - bytesUsed) {
+    ++droppedMessages;
+    return;
   }
+
+  const size_t writeIdx = (readIdx + bytesUsed) % RING_SIZE;
+  const size_t firstPart = std::min(textLen, RING_SIZE - writeIdx);
+
+  std::memcpy(ring + writeIdx, buf, firstPart);
+  std::memcpy(ring, buf + firstPart, textLen - firstPart);
+
+  bytesUsed += textLen;
 }
 
+}  // namespace
+
 void sif::setToAddCrAtEnd(const bool addCrAtEnd_) { addCrAtEnd = addCrAtEnd_; }
 
 void sif::setReplaceLastCharWithNewline(const bool replace) {
@@ -174,35 +156,43 @@ void sif::printError(const char* fmt, ...) {
 }
 
 void sif::printCallback() {
-  if (!initializedAndReady) {
-    initializedAndReady = true;
-  }
-  uint32_t bitmask = 0;
-  ServiceInterfacePrinterMessage message;
-  while (bufferQueue->receiveMessage(&message) == returnvalue::OK) {
-    const size_t bufferIdx = message.getBufferIndex();
-    bitmask |= 1 << bufferIdx;
-    printf("%s", printBufferArray[bufferIdx]);
-  }
+  taskRunning = true;
+
+  size_t chunk;
   {
-    MutexGuard guard(bufferMutex, MutexIF::TimeoutType::BLOCKING);
-    bufferState &= ~bitmask;
+    MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+    chunk = std::min(bytesUsed, MAX_BYTES_PER_CYCLE);
   }
-  const uint32_t droppedMessages = droppedMessagesCounter;
-  droppedMessagesCounter = 0;
-  droppedMessagesCounterContinuous += droppedMessages;
-  if (droppedMessages != 0) {
-    sif::printError("ServiceInterfacePrinter: Dropped ca. %i messages\n", droppedMessages);
+
+  if (chunk > 0) {
+    // Safe without the mutex: producers only touch the ring beyond
+    // readIdx + bytesUsed and only the print task advances readIdx.
+    const size_t firstPart = std::min(chunk, RING_SIZE - readIdx);
+    fwrite(ring + readIdx, 1, firstPart, stdout);
+    fwrite(ring, 1, chunk - firstPart, stdout);
+    fflush(stdout);
+  }
+
+  uint32_t dropped;
+  {
+    MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+    readIdx = (readIdx + chunk) % RING_SIZE;
+    bytesUsed -= chunk;
+    dropped = droppedMessages;
+    droppedMessages = 0;
+  }
+
+  droppedMessagesTotal += dropped;
+
+  if (dropped != 0) {
+    sif::printError("ServiceInterfacePrinter: Dropped %lu messages\n",
+                    static_cast<unsigned long>(dropped));
   }
 }
 
-void sif::init() {
-  bufferMutex = MutexFactory::instance()->createMutex();
-  bufferQueue = QueueFactory::instance()->createMessageQueue(fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT);
-  bufferQueue->setDefaultDestination(bufferQueue->getId());
-}
+void sif::init() { ringMutex = MutexFactory::instance()->createMutex(); }
 
-uint32_t sif::getDroppedMessagesCount() { return droppedMessagesCounterContinuous; }
+uint32_t sif::getDroppedMessagesCount() { return droppedMessagesTotal; }
 
 #else
 

src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp

@@ -1,18 +0,0 @@
-#include "ServiceInterfacePrinterMessage.h"
-
-#include <cstring>
-
-ServiceInterfacePrinterMessage::ServiceInterfacePrinterMessage(const size_t bufferIdx) {
-  this->MessageQueueMessage::setMessageSize(sizeof(bufferIdx));
-  this->setBufferIndex(bufferIdx);
-}
-
-void ServiceInterfacePrinterMessage::setBufferIndex(const size_t bufferIdx) {
-  std::memcpy(this->getData(), &bufferIdx, sizeof(bufferIdx));
-}
-
-size_t ServiceInterfacePrinterMessage::getBufferIndex() {
-  size_t tempIdx;
-  std::memcpy(&tempIdx, this->getData(), sizeof(size_t));
-  return tempIdx;
-}

src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h

@@ -1,12 +0,0 @@
-#pragma once
-#include "fsfw/ipc/MessageQueueMessage.h"
-
-class ServiceInterfacePrinterMessage : public MessageQueueMessage {
- public:
-  ServiceInterfacePrinterMessage() = default;
-  explicit ServiceInterfacePrinterMessage(size_t bufferIdx);
-  size_t getBufferIndex();
-
- private:
-  void setBufferIndex(size_t bufferIdx);
-};