fsfw/osal/FreeRTOS/MessageQueue.cpp

116 lines
3.4 KiB
C++
Raw Normal View History

2018-07-13 15:56:37 +02:00
#include "MessageQueue.h"
#include <framework/serviceinterface/ServiceInterfaceStream.h>
// TODO I guess we should have a way of checking if we are in an ISR and then use the "fromISR" versions of all calls
MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
defaultDestination(0),lastPartner(0) {
2018-07-13 15:56:37 +02:00
handle = xQueueCreate(message_depth, max_message_size);
if (handle == NULL) {
sif::error << "MessageQueue creation failed" << std::endl;
2018-07-13 15:56:37 +02:00
}
}
MessageQueue::~MessageQueue() {
if (handle != NULL) {
vQueueDelete(handle);
}
}
ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
MessageQueueMessage* message, bool ignoreFault) {
2018-07-13 18:28:26 +02:00
return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
2018-07-13 15:56:37 +02:00
}
ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessage* message) {
2018-07-13 18:28:26 +02:00
return sendToDefaultFrom(message, this->getId());
2018-07-13 15:56:37 +02:00
}
ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
if (this->lastPartner != 0) {
2018-07-13 18:28:26 +02:00
return sendMessageFrom(this->lastPartner, message, this->getId());
2018-07-13 15:56:37 +02:00
} else {
return NO_REPLY_PARTNER;
2018-07-13 15:56:37 +02:00
}
}
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
MessageQueueId_t* receivedFrom) {
ReturnValue_t status = this->receiveMessage(message);
if(status == HasReturnvaluesIF::RETURN_OK) {
*receivedFrom = this->lastPartner;
}
2018-07-13 15:56:37 +02:00
return status;
}
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) {
BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(message->getBuffer()), 0);
2018-07-13 15:56:37 +02:00
if (result == pdPASS){
this->lastPartner = message->getSender();
2018-07-13 15:56:37 +02:00
return HasReturnvaluesIF::RETURN_OK;
} else {
2018-07-13 18:28:26 +02:00
return MessageQueueIF::EMPTY;
2018-07-13 15:56:37 +02:00
}
}
MessageQueueId_t MessageQueue::getLastPartner() const {
return lastPartner;
}
ReturnValue_t MessageQueue::flush(uint32_t* count) {
//TODO FreeRTOS does not support flushing partially
//Is always successful
xQueueReset(handle);
return HasReturnvaluesIF::RETURN_OK;
}
MessageQueueId_t MessageQueue::getId() const {
return (MessageQueueId_t) handle;
}
void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
this->defaultDestination = defaultDestination;
}
2018-07-13 18:28:26 +02:00
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
2018-07-13 15:56:37 +02:00
MessageQueueMessage* message, MessageQueueId_t sentFrom,
bool ignoreFault) {
return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
2018-07-13 15:56:37 +02:00
}
2018-07-13 18:28:26 +02:00
ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message,
2018-07-13 15:56:37 +02:00
MessageQueueId_t sentFrom, bool ignoreFault) {
2018-07-13 18:28:26 +02:00
return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault);
2018-07-13 15:56:37 +02:00
}
MessageQueueId_t MessageQueue::getDefaultDestination() const {
return defaultDestination;
}
bool MessageQueue::isDefaultDestinationSet() const {
return 0;
}
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
MessageQueueMessage *message, MessageQueueId_t sentFrom,
bool ignoreFault) {
message->setSender(sentFrom);
BaseType_t result = xQueueSendToBack(reinterpret_cast<QueueHandle_t>(sendTo),
reinterpret_cast<const void*>(message->getBuffer()), 0);
if (result != pdPASS) {
if (!ignoreFault) {
InternalErrorReporterIF* internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
objects::INTERNAL_ERROR_REPORTER);
if (internalErrorReporter != NULL) {
internalErrorReporter->queueMessageNotSent();
}
}
return MessageQueueIF::FULL;
}
return HasReturnvaluesIF::RETURN_OK;
}
2018-07-13 15:56:37 +02:00