fsfw/osal/FreeRTOS/MessageQueue.cpp

159 lines
5.3 KiB
C++
Raw Normal View History

2018-07-13 15:56:37 +02:00
#include "MessageQueue.h"
2021-05-27 13:38:40 +02:00
#include "QueueMapManager.h"
2021-06-05 19:52:38 +02:00
#include "../../objectmanager/ObjectManager.h"
#include "../../serviceinterface/ServiceInterface.h"
2018-07-13 15:56:37 +02:00
2020-09-26 14:24:09 +02:00
MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
2021-05-27 13:47:22 +02:00
maxMessageSize(maxMessageSize) {
handle = xQueueCreate(messageDepth, maxMessageSize);
if (handle == nullptr) {
2021-05-27 13:38:40 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-05-27 13:47:22 +02: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
2021-05-27 13:47:22 +02:00
sif::printError("MessageQueue::MessageQueue: Creation failed\n");
sif::printError("Specified Message Depth: %d\n", messageDepth);
sif::printError("Specified MAximum Message Size: %d\n", maxMessageSize);
#endif
2021-05-27 13:47:22 +02:00
}
2021-05-27 13:38:40 +02:00
QueueMapManager::instance()->addMessageQueue(handle, &queueId);
2018-07-13 15:56:37 +02:00
}
MessageQueue::~MessageQueue() {
2021-05-27 13:47:22 +02:00
if (handle != nullptr) {
vQueueDelete(handle);
}
2018-07-13 15:56:37 +02:00
}
2020-09-26 14:24:09 +02:00
void MessageQueue::switchSystemContext(CallContext callContext) {
2021-05-27 13:47:22 +02:00
this->callContext = callContext;
2020-09-26 14:24:09 +02:00
}
2018-07-13 15:56:37 +02:00
ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
2021-05-27 13:47:22 +02:00
MessageQueueMessageIF* message, bool ignoreFault) {
return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
2018-07-13 15:56:37 +02:00
}
2020-09-26 14:24:09 +02:00
ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
2021-05-27 13:47:22 +02:00
return sendToDefaultFrom(message, this->getId());
2018-07-13 15:56:37 +02:00
}
2020-09-26 14:24:09 +02:00
ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
2021-05-27 13:47:22 +02:00
MessageQueueId_t sentFrom, bool ignoreFault) {
return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault);
2020-09-26 14:24:09 +02:00
}
ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
2021-05-27 13:47:22 +02:00
if (this->lastPartner != MessageQueueIF::NO_QUEUE) {
return sendMessageFrom(this->lastPartner, message, this->getId());
} else {
return NO_REPLY_PARTNER;
}
2018-07-13 15:56:37 +02:00
}
2020-09-26 14:24:09 +02:00
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
2021-05-27 13:47:22 +02:00
MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
bool ignoreFault) {
return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault,
callContext);
2020-09-26 14:24:09 +02:00
}
2021-05-27 13:38:40 +02:00
QueueHandle_t MessageQueue::getNativeQueueHandle() {
return handle;
}
2020-09-26 14:24:09 +02:00
ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) {
if (result != pdPASS) {
if (not ignoreFault) {
2021-06-05 19:52:38 +02:00
InternalErrorReporterIF* internalErrorReporter = ObjectManager::instance()->
2021-05-27 13:38:40 +02:00
get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
2020-09-26 14:24:09 +02:00
if (internalErrorReporter != nullptr) {
internalErrorReporter->queueMessageNotSent();
}
}
return MessageQueueIF::FULL;
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
2021-05-27 13:47:22 +02:00
MessageQueueId_t* receivedFrom) {
ReturnValue_t status = this->receiveMessage(message);
if(status == HasReturnvaluesIF::RETURN_OK) {
*receivedFrom = this->lastPartner;
}
return status;
2018-07-13 15:56:37 +02:00
}
2020-09-26 14:24:09 +02:00
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
2021-05-27 13:47:22 +02:00
BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(
message->getBuffer()), 0);
if (result == pdPASS){
this->lastPartner = message->getSender();
return HasReturnvaluesIF::RETURN_OK;
} else {
return MessageQueueIF::EMPTY;
}
2018-07-13 15:56:37 +02:00
}
MessageQueueId_t MessageQueue::getLastPartner() const {
2021-05-27 13:47:22 +02:00
return lastPartner;
2018-07-13 15:56:37 +02:00
}
ReturnValue_t MessageQueue::flush(uint32_t* count) {
2021-05-27 13:47:22 +02: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
}
MessageQueueId_t MessageQueue::getId() const {
2021-05-27 13:47:22 +02:00
return queueId;
2018-07-13 15:56:37 +02:00
}
void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
2021-05-27 13:47:22 +02:00
defaultDestinationSet = true;
this->defaultDestination = defaultDestination;
2018-07-13 15:56:37 +02:00
}
MessageQueueId_t MessageQueue::getDefaultDestination() const {
2021-05-27 13:47:22 +02:00
return defaultDestination;
2018-07-13 15:56:37 +02:00
}
bool MessageQueue::isDefaultDestinationSet() const {
2021-05-27 13:47:22 +02:00
return defaultDestinationSet;
2018-07-13 15:56:37 +02:00
}
2020-09-26 14:24:09 +02:00
// static core function to send messages.
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
2020-09-26 14:24:09 +02:00
MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
bool ignoreFault, CallContext callContext) {
2021-05-27 13:47:22 +02:00
BaseType_t result = pdFALSE;
if(sendTo == MessageQueueIF::NO_QUEUE) {
2021-05-27 13:38:40 +02:00
return MessageQueueIF::DESTINATION_INVALID;
2021-05-27 13:47:22 +02:00
}
2021-05-27 13:38:40 +02:00
2021-05-27 13:47:22 +02:00
QueueHandle_t destination = QueueMapManager::instance()->getMessageQueue(sendTo);
if(destination == nullptr) {
2021-05-27 13:38:40 +02:00
return MessageQueueIF::DESTINATION_INVALID;
2021-05-27 13:47:22 +02:00
}
2020-09-26 14:24:09 +02:00
message->setSender(sentFrom);
if(callContext == CallContext::TASK) {
2021-05-27 13:38:40 +02:00
result = xQueueSendToBack(destination, static_cast<const void*>(message->getBuffer()), 0);
2020-09-26 14:24:09 +02:00
}
else {
2021-05-27 13:38:40 +02:00
/* If the call context is from an interrupt, request a context switch if a higher priority
task was blocked by the interrupt. */
2020-09-26 14:24:09 +02:00
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
2021-05-27 13:40:01 +02:00
result = xQueueSendFromISR(destination, static_cast<const void*>(message->getBuffer()),
&xHigherPriorityTaskWoken);
2020-09-26 14:24:09 +02:00
if(xHigherPriorityTaskWoken == pdTRUE) {
TaskManagement::requestContextSwitch(callContext);
}
}
return handleSendResult(result, ignoreFault);
}