2021-07-14 00:54:39 +02:00
|
|
|
#include "fsfw/osal/freertos/MessageQueue.h"
|
|
|
|
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/osal/freertos/QueueMapManager.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2018-07-13 15:56:37 +02:00
|
|
|
|
2022-03-22 17:49:22 +01:00
|
|
|
MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args)
|
2022-04-11 11:19:38 +02:00
|
|
|
: MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args),
|
|
|
|
maxMessageSize(maxMessageSize) {
|
2022-02-02 10:29:30 +01:00
|
|
|
handle = xQueueCreate(messageDepth, maxMessageSize);
|
|
|
|
if (handle == nullptr) {
|
2021-05-27 13:38:40 +02:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "MessageQueue::MessageQueue: Creation failed" << std::endl;
|
|
|
|
sif::error << "Specified Message Depth: " << messageDepth << std::endl;
|
|
|
|
sif::error << "Specified Maximum Message Size: " << maxMessageSize << std::endl;
|
2021-05-27 13:38:40 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError("MessageQueue::MessageQueue: Creation failed\n");
|
|
|
|
sif::printError("Specified Message Depth: %d\n", messageDepth);
|
2022-04-11 11:19:38 +02:00
|
|
|
sif::printError("Specified Maximum Message Size: %d\n", maxMessageSize);
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2022-04-11 11:19:38 +02:00
|
|
|
QueueMapManager::instance()->addMessageQueue(handle, &id);
|
2018-07-13 15:56:37 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
MessageQueue::~MessageQueue() {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (handle != nullptr) {
|
|
|
|
vQueueDelete(handle);
|
|
|
|
}
|
2018-07-13 15:56:37 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
void MessageQueue::switchSystemContext(CallContext callContext) { this->callContext = callContext; }
|
2020-09-26 14:24:09 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
|
|
|
|
MessageQueueId_t sentFrom, bool ignoreFault) {
|
|
|
|
return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault, callContext);
|
2020-09-26 14:24:09 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
QueueHandle_t MessageQueue::getNativeQueueHandle() { return handle; }
|
2020-09-26 14:24:09 +02:00
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (result != pdPASS) {
|
|
|
|
if (not ignoreFault) {
|
|
|
|
InternalErrorReporterIF* internalErrorReporter =
|
|
|
|
ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
|
|
|
|
if (internalErrorReporter != nullptr) {
|
|
|
|
internalErrorReporter->queueMessageNotSent();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return MessageQueueIF::FULL;
|
|
|
|
}
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2020-09-26 14:24:09 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
|
2022-02-02 10:29:30 +01:00
|
|
|
BaseType_t result = xQueueReceive(handle, reinterpret_cast<void*>(message->getBuffer()), 0);
|
|
|
|
if (result == pdPASS) {
|
2022-04-11 11:19:38 +02:00
|
|
|
this->last = message->getSender();
|
2022-02-02 10:29:30 +01:00
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
} else {
|
|
|
|
return MessageQueueIF::EMPTY;
|
|
|
|
}
|
2018-07-13 15:56:37 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::flush(uint32_t* count) {
|
2022-02-02 10:29:30 +01:00
|
|
|
// TODO FreeRTOS does not support flushing partially
|
|
|
|
// Is always successful
|
|
|
|
xQueueReset(handle);
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2018-07-13 15:56:37 +02:00
|
|
|
}
|
|
|
|
|
2020-09-26 14:24:09 +02:00
|
|
|
// static core function to send messages.
|
2019-08-28 14:50:24 +02:00
|
|
|
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueMessageIF* message,
|
|
|
|
MessageQueueId_t sentFrom, bool ignoreFault,
|
|
|
|
CallContext callContext) {
|
|
|
|
BaseType_t result = pdFALSE;
|
|
|
|
if (sendTo == MessageQueueIF::NO_QUEUE) {
|
|
|
|
return MessageQueueIF::DESTINATION_INVALID;
|
|
|
|
}
|
|
|
|
|
|
|
|
QueueHandle_t destination = QueueMapManager::instance()->getMessageQueue(sendTo);
|
|
|
|
if (destination == nullptr) {
|
|
|
|
return MessageQueueIF::DESTINATION_INVALID;
|
|
|
|
}
|
|
|
|
|
|
|
|
message->setSender(sentFrom);
|
|
|
|
if (callContext == CallContext::TASK) {
|
|
|
|
result = xQueueSendToBack(destination, static_cast<const void*>(message->getBuffer()), 0);
|
|
|
|
} else {
|
|
|
|
/* If the call context is from an interrupt, request a context switch if a higher priority
|
|
|
|
task was blocked by the interrupt. */
|
|
|
|
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
|
|
|
result = xQueueSendFromISR(destination, static_cast<const void*>(message->getBuffer()),
|
|
|
|
&xHigherPriorityTaskWoken);
|
|
|
|
if (xHigherPriorityTaskWoken == pdTRUE) {
|
|
|
|
TaskManagement::requestContextSwitch(callContext);
|
2020-09-26 14:24:09 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return handleSendResult(result, ignoreFault);
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|