diff --git a/misc/defaultcfg/fsfwconfig/FSFWConfig.h b/misc/defaultcfg/fsfwconfig/FSFWConfig.h index e80abf24..5ca0a76e 100644 --- a/misc/defaultcfg/fsfwconfig/FSFWConfig.h +++ b/misc/defaultcfg/fsfwconfig/FSFWConfig.h @@ -71,6 +71,7 @@ static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS = 120; static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6; static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124; +static constexpr size_t FSFW_PRINT_BUFFER_AMOUNT = 32; static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048; diff --git a/src/fsfw/objectmanager/frameworkObjects.h b/src/fsfw/objectmanager/frameworkObjects.h index bf153cae..7eba9bb0 100644 --- a/src/fsfw/objectmanager/frameworkObjects.h +++ b/src/fsfw/objectmanager/frameworkObjects.h @@ -36,6 +36,8 @@ enum framework_objects : object_id_t { TIME_STAMPER = 0x53500010, VERIFICATION_REPORTER = 0x53500020, + SIF_PRINT_TASK = 0x53600000, + FSFW_OBJECTS_END = 0x53ffffff, NO_OBJECT = 0xFFFFFFFF }; diff --git a/src/fsfw/osal/host/TaskFactory.cpp b/src/fsfw/osal/host/TaskFactory.cpp index d9552923..c2efb359 100644 --- a/src/fsfw/osal/host/TaskFactory.cpp +++ b/src/fsfw/osal/host/TaskFactory.cpp @@ -52,6 +52,6 @@ void TaskFactory::printMissedDeadline() { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "TaskFactory::printMissedDeadline: " << name << std::endl; #else - sif::printWarning("TaskFactory::printMissedDeadline: %s\n", name); + sif::printWarning("TaskFactory::printMissedDeadline: %s\n", name.c_str()); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ } diff --git a/src/fsfw/serviceinterface/CMakeLists.txt b/src/fsfw/serviceinterface/CMakeLists.txt index df3f074e..586fdadb 100644 --- a/src/fsfw/serviceinterface/CMakeLists.txt +++ b/src/fsfw/serviceinterface/CMakeLists.txt @@ -1,4 +1,5 @@ target_sources( ${LIB_FSFW_NAME} PRIVATE ServiceInterfaceStream.cpp ServiceInterfaceBuffer.cpp - ServiceInterfacePrinter.cpp) + ServiceInterfacePrinter.cpp ServiceInterfacePrinterTask.cpp + ServiceInterfacePrinterMessage.cpp) diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp b/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp index b83eda3c..373aa7d7 100644 --- a/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp @@ -1,9 +1,13 @@ #include "fsfw/serviceinterface/ServiceInterfacePrinter.h" #include -#include +#include "ServiceInterfacePrinterMessage.h" +#include "etl/bitset.h" +#include "etl/queue.h" #include "fsfw/FSFW.h" +#include "fsfw/ipc/MutexGuard.h" +#include "fsfw/ipc/QueueFactory.h" #include "fsfw/serviceinterface/serviceInterfaceDefintions.h" #include "fsfw/timemanager/Clock.h" @@ -14,11 +18,33 @@ static bool consoleInitialized = false; #if FSFW_DISABLE_PRINTOUT == 0 +typedef etl::bitset bitset; + static bool addCrAtEnd = false; +static bool initializedAndReady = false; +static bool replaceLastCharWithNewline = false; -uint8_t printBuffer[fsfwconfig::FSFW_PRINT_BUFFER_SIZE]; +bitset bufferState{}; +std::array + printBufferArray = {}; -void fsfwPrint(sif::PrintLevel printType, const char *fmt, va_list arg) { +uint32_t droppedMessagesCounter = 0; +uint32_t droppedMessagesCounterContinuous = 0; + +MutexIF* bufferMutex; +MessageQueueIF* bufferQueue; + +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; +} + +void fsfwPrint(const 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); @@ -30,14 +56,22 @@ void fsfwPrint(sif::PrintLevel printType, const char *fmt, va_list arg) { consoleInitialized = true; #endif - size_t len = 0; - char *bufferPosition = reinterpret_cast(printBuffer); - /* 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 */ #if FSFW_COLORED_OUTPUT == 1 @@ -61,7 +95,6 @@ void fsfwPrint(sif::PrintLevel printType, const char *fmt, va_list arg) { if (printType == sif::PrintLevel::WARNING_LEVEL) { len += sprintf(bufferPosition + len, "WARNING"); } - if (printType == sif::PrintLevel::ERROR_LEVEL) { len += sprintf(bufferPosition + len, "ERROR "); } @@ -79,51 +112,108 @@ void fsfwPrint(sif::PrintLevel printType, const char *fmt, va_list arg) { (unsigned long)now.minute, (unsigned long)now.second, (unsigned long)now.usecond / 1000); - len += vsnprintf(bufferPosition + len, sizeof(printBuffer) - len, fmt, arg); + len += vsnprintf(bufferPosition + len, sizeof(printBufferArray[bufferIdx]) - len, fmt, arg); if (addCrAtEnd) { len += sprintf(bufferPosition + len, "\r"); } - printf("%s", printBuffer); + 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 (isReady) { + ServiceInterfacePrinterMessage message(bufferIdx); + bufferQueue->sendToDefault(&message); + } else { + printf("%s", printBufferArray[bufferIdx]); + } } -void sif::printInfo(const char *fmt, ...) { +void sif::setToAddCrAtEnd(const bool addCrAtEnd_) { addCrAtEnd = addCrAtEnd_; } + +void sif::setReplaceLastCharWithNewline(const bool replace) { + replaceLastCharWithNewline = replace; +} + +void sif::printInfo(const char* fmt, ...) { va_list args; va_start(args, fmt); fsfwPrint(sif::PrintLevel::INFO_LEVEL, fmt, args); va_end(args); } -void sif::printWarning(const char *fmt, ...) { +void sif::printWarning(const char* fmt, ...) { va_list args; va_start(args, fmt); fsfwPrint(sif::PrintLevel::WARNING_LEVEL, fmt, args); va_end(args); } -void sif::printDebug(const char *fmt, ...) { +void sif::printDebug(const char* fmt, ...) { va_list args; va_start(args, fmt); fsfwPrint(sif::PrintLevel::DEBUG_LEVEL, fmt, args); va_end(args); } -void sif::setToAddCrAtEnd(bool addCrAtEnd_) { addCrAtEnd = addCrAtEnd_; } - -void sif::printError(const char *fmt, ...) { +void sif::printError(const char* fmt, ...) { va_list args; va_start(args, fmt); fsfwPrint(sif::PrintLevel::ERROR_LEVEL, fmt, args); va_end(args); } +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]); + } + { + MutexGuard guard(bufferMutex, MutexIF::TimeoutType::BLOCKING); + bufferState &= ~bitmask; + } + const uint32_t droppedMessages = droppedMessagesCounter; + droppedMessagesCounter = 0; + droppedMessagesCounterContinuous += droppedMessages; + if (droppedMessages != 0) { + sif::printError("ServiceInterfacePrinter: Dropped ca. %i messages\n", droppedMessages); + } +} + +void sif::init() { + bufferMutex = MutexFactory::instance()->createMutex(); + bufferQueue = QueueFactory::instance()->createMessageQueue(fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT); + bufferQueue->setDefaultDestination(bufferQueue->getId()); +} + +uint32_t sif::getDroppedMessagesCount() { return droppedMessagesCounterContinuous; } + #else -void sif::printInfo(const char *fmt, ...) {} -void sif::printWarning(const char *fmt, ...) {} -void sif::printDebug(const char *fmt, ...) {} -void sif::printError(const char *fmt, ...) {} +void sif::printInfo(const char* fmt, ...) {} +void sif::printWarning(const char* fmt, ...) {} +void sif::printDebug(const char* fmt, ...) {} +void sif::printError(const char* fmt, ...) {} + +void sif::printCallback() {} +void sif::init() {} +uint32_t sif::getDroppedMessagesCount() { return 0; } #endif /* FSFW_DISABLE_PRINTOUT == 0 */ diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinter.h b/src/fsfw/serviceinterface/ServiceInterfacePrinter.h index 65f7250a..28ad800b 100644 --- a/src/fsfw/serviceinterface/ServiceInterfacePrinter.h +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinter.h @@ -39,6 +39,11 @@ PrintLevel getPrintLevel(); void setToAddCrAtEnd(bool addCrAtEnd_); +/** + * Replaces the last char of a print buffer with a newline + */ +void setReplaceLastCharWithNewline(bool replace); + /** * These functions can be used like the C stdio printf and forward the * supplied formatted string arguments to a printf function. @@ -51,6 +56,21 @@ void printWarning(const char* fmt, ...); void printDebug(const char* fmt, ...); void printError(const char* fmt, ...); +/** + * This function is to be called periodically by a dedicated print task. + */ +void printCallback(); + +/** + * Initializes the global state for the print task. + */ +void init(); + +/** + * Gets the total estimated number of dropped messages + */ +uint32_t getDroppedMessagesCount(); + } // namespace sif #endif /* FSFW_SERVICEINTERFACE_SERVICEINTERFACEPRINTER */ diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp b/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp new file mode 100644 index 00000000..8753f14e --- /dev/null +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp @@ -0,0 +1,18 @@ +#include "ServiceInterfacePrinterMessage.h" + +#include + +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; +} diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h b/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h new file mode 100644 index 00000000..f013c665 --- /dev/null +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h @@ -0,0 +1,12 @@ +#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); +}; diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinterTask.cpp b/src/fsfw/serviceinterface/ServiceInterfacePrinterTask.cpp new file mode 100644 index 00000000..a89c546a --- /dev/null +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinterTask.cpp @@ -0,0 +1,16 @@ +#include "ServiceInterfacePrinterTask.h" + +#include "ServiceInterfacePrinter.h" +#include "fsfw/objectmanager/SystemObject.h" + +#include "mission/utility/LoggingHelper.h" + +ServiceInterfacePrinterTask::ServiceInterfacePrinterTask(object_id_t objectId) + : SystemObject(objectId) { + sif::init(); +} + +ReturnValue_t ServiceInterfacePrinterTask::performOperation(uint8_t operationCode) { + sif::printCallback(); + return returnvalue::OK; +} diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinterTask.h b/src/fsfw/serviceinterface/ServiceInterfacePrinterTask.h new file mode 100644 index 00000000..6e89819c --- /dev/null +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinterTask.h @@ -0,0 +1,10 @@ +#pragma once + +#include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/tasks/ExecutableObjectIF.h" + +class ServiceInterfacePrinterTask : public ExecutableObjectIF, public SystemObject { + public: + explicit ServiceInterfacePrinterTask(object_id_t objectId); + ReturnValue_t performOperation(uint8_t operationCode) override; +};