|
|
|
@ -64,9 +64,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
|
|
|
|
|
return MessageQueueIF::EMPTY;
|
|
|
|
|
}
|
|
|
|
|
MutexGuard mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20);
|
|
|
|
|
MessageQueueMessage* currentMessage = &messageQueue.front();
|
|
|
|
|
std::copy(currentMessage->getBuffer(),
|
|
|
|
|
currentMessage->getBuffer() + messageSize, message->getBuffer());
|
|
|
|
|
std::copy(messageQueue.front().data(), messageQueue.front().data() + messageSize,
|
|
|
|
|
message->getBuffer());
|
|
|
|
|
messageQueue.pop();
|
|
|
|
|
// The last partner is the first uint32_t field in the message
|
|
|
|
|
this->lastPartner = message->getSender();
|
|
|
|
@ -80,7 +79,7 @@ MessageQueueId_t MessageQueue::getLastPartner() const {
|
|
|
|
|
ReturnValue_t MessageQueue::flush(uint32_t* count) {
|
|
|
|
|
*count = messageQueue.size();
|
|
|
|
|
// Clears the queue.
|
|
|
|
|
messageQueue = std::queue<MessageQueueMessage>();
|
|
|
|
|
messageQueue = std::queue<std::vector<uint8_t>>();
|
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
@ -132,20 +131,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
|
|
|
|
|
}
|
|
|
|
|
if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
|
|
|
|
|
MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
|
|
|
|
|
// TODO: Would be nice to support other message types, but this would require
|
|
|
|
|
// create the message on the heap from an upper layer and simply storing
|
|
|
|
|
// MessageQueueMessageIF pointers in the queue.
|
|
|
|
|
MessageQueueMessage* mqmMessage = dynamic_cast<MessageQueueMessage*>(message);
|
|
|
|
|
if(mqmMessage != nullptr) {
|
|
|
|
|
targetQueue->messageQueue.push(*mqmMessage);
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
|
|
|
sif::error << "MessageQueue::sendMessageFromMessageQueue: Message"
|
|
|
|
|
"is not MessageQueueMessage!" << std::endl;
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
targetQueue->messageQueue.push(std::vector<uint8_t>(message->getMaximumMessageSize()));
|
|
|
|
|
memcpy(targetQueue->messageQueue.back().data(), message->getBuffer(),
|
|
|
|
|
message->getMaximumMessageSize());
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
return MessageQueueIF::FULL;
|
|
|
|
|