esbo_ds/fsfw
3
0
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'development' into gaisser_make_linux_realtime_optional

Browse Source
This commit is contained in:
Steffen Gaisser 2021-04-20 14:39:56 +02:00
commit 91c3692f6d
8 changed files with 169 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
osal/common/UdpTcPollingTask.cpp
View File

@ -15,7 +15,7 @@
#endif #endif
//! Debugging preprocessor define. //! Debugging preprocessor define.
#define FSFW_UDP_RCV_WIRETAPPING_ENABLED 0 #define FSFW_UDP_RECV_WIRETAPPING_ENABLED 0
UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId, UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId,
object_id_t tmtcUnixUdpBridge, size_t maxRecvSize, object_id_t tmtcUnixUdpBridge, size_t maxRecvSize,
@ -66,10 +66,13 @@ ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) {
tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::RECVFROM_CALL, 1000); tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::RECVFROM_CALL, 1000);
continue; continue;
} }
#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1 #if FSFW_UDP_RECV_WIRETAPPING_ENABLED == 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << "UdpTcPollingTask::performOperation: " << bytesReceived << sif::debug << "UdpTcPollingTask::performOperation: " << bytesReceived <<
" bytes received" << std::endl; " bytes received" << std::endl;
#else
#endif #endif
#endif /* FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1 */
ReturnValue_t result = handleSuccessfullTcRead(bytesReceived); ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
if(result != HasReturnvaluesIF::RETURN_FAILED) { if(result != HasReturnvaluesIF::RETURN_FAILED) {
@ -84,7 +87,7 @@ ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) {
ReturnValue_t UdpTcPollingTask::handleSuccessfullTcRead(size_t bytesRead) { ReturnValue_t UdpTcPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
store_address_t storeId; store_address_t storeId;
#if FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1 #if FSFW_UDP_RECV_WIRETAPPING_ENABLED == 1
arrayprinter::print(receptionBuffer.data(), bytesRead); arrayprinter::print(receptionBuffer.data(), bytesRead);
#endif #endif

22
osal/common/UdpTmTcBridge.cpp
View File

@ -70,6 +70,7 @@ ReturnValue_t UdpTmTcBridge::initialize() {
hints.ai_family = AF_INET; hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_DGRAM; hints.ai_socktype = SOCK_DGRAM;
hints.ai_protocol = IPPROTO_UDP; hints.ai_protocol = IPPROTO_UDP;
hints.ai_flags = AI_PASSIVE;
/* Set up UDP socket: /* Set up UDP socket:
https://en.wikipedia.org/wiki/Getaddrinfo https://en.wikipedia.org/wiki/Getaddrinfo
@ -95,6 +96,10 @@ ReturnValue_t UdpTmTcBridge::initialize() {
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
} }
#if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
tcpip::printAddress(addrResult->ai_addr);
#endif
retval = bind(serverSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen)); retval = bind(serverSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
if(retval != 0) { if(retval != 0) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -103,6 +108,7 @@ ReturnValue_t UdpTmTcBridge::initialize() {
#endif #endif
freeaddrinfo(addrResult); freeaddrinfo(addrResult);
tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::BIND_CALL); tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::BIND_CALL);
return HasReturnvaluesIF::RETURN_FAILED;
} }
freeaddrinfo(addrResult); freeaddrinfo(addrResult);
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
@ -120,10 +126,8 @@ ReturnValue_t UdpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) {
/* The target address can be set by different threads so this lock ensures thread-safety */ /* The target address can be set by different threads so this lock ensures thread-safety */
MutexGuard lock(mutex, timeoutType, mutexTimeoutMs); MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1 #if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
char ipAddress [15]; tcpip::printAddress(&clientAddress);
sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
&clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
#endif #endif
int bytesSent = sendto( int bytesSent = sendto(
@ -151,13 +155,11 @@ void UdpTmTcBridge::checkAndSetClientAddress(sockaddr& newAddress) {
/* The target address can be set by different threads so this lock ensures thread-safety */ /* The target address can be set by different threads so this lock ensures thread-safety */
MutexGuard lock(mutex, timeoutType, mutexTimeoutMs); MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1 #if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
char ipAddress [15]; tcpip::printAddress(&newAddress);
sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET, tcpip::printAddress(&clientAddress);
&newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
sif::debug << "IP Address Old: " << inet_ntop(AF_INET,
&clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
#endif #endif
registerCommConnect(); registerCommConnect();
/* Set new IP address to reply to */ /* Set new IP address to reply to */

39
osal/common/tcpipCommon.cpp
View File

@ -1,4 +1,9 @@
#include "tcpipCommon.h" #include "tcpipCommon.h"
#include <fsfw/serviceinterface/ServiceInterface.h>
#ifdef _WIN32
#include <ws2tcpip.h>
#endif
void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string &protStr, void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string &protStr,
std::string &srcString) { std::string &srcString) {
@ -34,3 +39,37 @@ void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std:
srcString = "unknown call"; srcString = "unknown call";
} }
} }
void tcpip::printAddress(struct sockaddr* addr) {
char ipAddress[INET6_ADDRSTRLEN] = {};
const char* stringPtr = NULL;
switch(addr->sa_family) {
case AF_INET: {
struct sockaddr_in *addrIn = reinterpret_cast<struct sockaddr_in*>(addr);
stringPtr = inet_ntop(AF_INET, &(addrIn->sin_addr), ipAddress, INET_ADDRSTRLEN);
break;
}
case AF_INET6: {
struct sockaddr_in6 *addrIn = reinterpret_cast<struct sockaddr_in6*>(addr);
stringPtr = inet_ntop(AF_INET6, &(addrIn->sin6_addr), ipAddress, INET6_ADDRSTRLEN);
break;
}
}
#if FSFW_CPP_OSTREAM_ENABLED == 1
if(stringPtr == NULL) {
sif::debug << "Could not convert IP address to text representation, error code "
<< errno << std::endl;
}
else {
sif::debug << "IP Address Sender: " << ipAddress << std::endl;
}
#else
if(stringPtr == NULL) {
sif::printDebug("Could not convert IP address to text representation, error code %d\n",
errno);
}
else {
sif::printDebug("IP Address Sender: %s\n", ipAddress);
}
#endif
}

11
osal/common/tcpipCommon.h
View File

@ -4,6 +4,13 @@
#include "../../timemanager/clockDefinitions.h" #include "../../timemanager/clockDefinitions.h"
#include <string> #include <string>
#ifdef _WIN32
#include <winsock2.h>
#else
#include <netdb.h>
#include <arpa/inet.h>
#endif
namespace tcpip { namespace tcpip {
const char* const DEFAULT_SERVER_PORT = "7301"; const char* const DEFAULT_SERVER_PORT = "7301";
@ -28,8 +35,8 @@ enum class ErrorSources {
void determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string& protStr, void determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string& protStr,
std::string& srcString); std::string& srcString);
void printAddress(struct sockaddr* addr);
} }
#endif /* FSFW_OSAL_COMMON_TCPIPCOMMON_H_ */ #endif /* FSFW_OSAL_COMMON_TCPIPCOMMON_H_ */

29
osal/host/MessageQueue.cpp
View File

@ -64,9 +64,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
return MessageQueueIF::EMPTY; return MessageQueueIF::EMPTY;
} }
MutexGuard mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20); MutexGuard mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20);
MessageQueueMessage* currentMessage = &messageQueue.front(); std::copy(messageQueue.front().data(), messageQueue.front().data() + messageSize,
std::copy(currentMessage->getBuffer(), message->getBuffer());
currentMessage->getBuffer() + messageSize, message->getBuffer());
messageQueue.pop(); messageQueue.pop();
// The last partner is the first uint32_t field in the message // The last partner is the first uint32_t field in the message
this->lastPartner = message->getSender(); this->lastPartner = message->getSender();
@ -80,7 +79,7 @@ MessageQueueId_t MessageQueue::getLastPartner() const {
ReturnValue_t MessageQueue::flush(uint32_t* count) { ReturnValue_t MessageQueue::flush(uint32_t* count) {
*count = messageQueue.size(); *count = messageQueue.size();
// Clears the queue. // Clears the queue.
messageQueue = std::queue<MessageQueueMessage>(); messageQueue = std::queue<std::vector<uint8_t>>();
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
@ -106,6 +105,9 @@ bool MessageQueue::isDefaultDestinationSet() const {
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
MessageQueueMessageIF* message, MessageQueueId_t sentFrom, MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
bool ignoreFault) { bool ignoreFault) {
if(message == nullptr) {
return HasReturnvaluesIF::RETURN_FAILED;
}
message->setSender(sentFrom); message->setSender(sentFrom);
if(message->getMessageSize() > message->getMaximumMessageSize()) { if(message->getMessageSize() > message->getMaximumMessageSize()) {
// Actually, this should never happen or an error will be emitted // Actually, this should never happen or an error will be emitted
@ -128,21 +130,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
} }
if(targetQueue->messageQueue.size() < targetQueue->messageDepth) { if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
MutexGuard mutexLock(targetQueue->queueLock, MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
MutexIF::TimeoutType::WAITING, 20); targetQueue->messageQueue.push(std::vector<uint8_t>(message->getMaximumMessageSize()));
// not ideal, works for now though. memcpy(targetQueue->messageQueue.back().data(), message->getBuffer(),
MessageQueueMessage* mqmMessage = message->getMaximumMessageSize());
dynamic_cast<MessageQueueMessage*>(message);
if(message != nullptr) {
targetQueue->messageQueue.push(*mqmMessage);
}
else {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "MessageQueue::sendMessageFromMessageQueue: Message"
"is not MessageQueueMessage!" << std::endl;
#endif
}
} }
else { else {
return MessageQueueIF::FULL; return MessageQueueIF::FULL;

2
osal/host/MessageQueue.h
View File

@ -212,7 +212,7 @@ protected:
//static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); //static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
private: private:
std::queue<MessageQueueMessage> messageQueue; std::queue<std::vector<uint8_t>> messageQueue;
/** /**
* @brief The class stores the queue id it got assigned. * @brief The class stores the queue id it got assigned.
* If initialization fails, the queue id is set to zero. * If initialization fails, the queue id is set to zero.

31
tcdistribution/CCSDSDistributor.cpp
View File

@ -1,8 +1,10 @@
#include "CCSDSDistributor.h" #include "CCSDSDistributor.h"
#include "../serviceinterface/ServiceInterfaceStream.h" #include "../serviceinterface/ServiceInterface.h"
#include "../tmtcpacket/SpacePacketBase.h" #include "../tmtcpacket/SpacePacketBase.h"
#define CCSDS_DISTRIBUTOR_DEBUGGING 0
CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
object_id_t setObjectId): object_id_t setObjectId):
TcDistributor(setObjectId), defaultApid( setDefaultApid ) { TcDistributor(setObjectId), defaultApid( setDefaultApid ) {
@ -11,26 +13,36 @@ CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
CCSDSDistributor::~CCSDSDistributor() {} CCSDSDistributor::~CCSDSDistributor() {}
TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() { TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
#if CCSDS_DISTRIBUTOR_DEBUGGING == 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
// sif::debug << "CCSDSDistributor::selectDestination received: " << sif::debug << "CCSDSDistributor::selectDestination received: " <<
// this->currentMessage.getStorageId().pool_index << ", " << this->currentMessage.getStorageId().poolIndex << ", " <<
// this->currentMessage.getStorageId().packet_index << std::endl; this->currentMessage.getStorageId().packetIndex << std::endl;
#else
sif::printDebug("CCSDSDistributor::selectDestination received: %d, %d\n",
currentMessage.getStorageId().poolIndex, currentMessage.getStorageId().packetIndex);
#endif
#endif #endif
const uint8_t* packet = nullptr; const uint8_t* packet = nullptr;
size_t size = 0; size_t size = 0;
ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(), ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(),
&packet, &size ); &packet, &size );
if(result != HasReturnvaluesIF::RETURN_OK) { if(result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CCSDSDistributor::selectDestination: Getting data from" sif::error << "CCSDSDistributor::selectDestination: Getting data from"
" store failed!" << std::endl; " store failed!" << std::endl;
#else
sif::printError("CCSDSDistributor::selectDestination: Getting data from"
" store failed!\n");
#endif
#endif #endif
} }
SpacePacketBase currentPacket(packet); SpacePacketBase currentPacket(packet);
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
// sif:: info << "CCSDSDistributor::selectDestination has packet with APID " sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex <<
// << std::hex << currentPacket.getAPID() << std::dec << std::endl; currentPacket.getAPID() << std::dec << std::endl;
#endif #endif
TcMqMapIter position = this->queueMap.find(currentPacket.getAPID()); TcMqMapIter position = this->queueMap.find(currentPacket.getAPID());
if ( position != this->queueMap.end() ) { if ( position != this->queueMap.end() ) {
@ -76,9 +88,14 @@ ReturnValue_t CCSDSDistributor::initialize() {
ReturnValue_t status = this->TcDistributor::initialize(); ReturnValue_t status = this->TcDistributor::initialize();
this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE ); this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE );
if (this->tcStore == nullptr) { if (this->tcStore == nullptr) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CCSDSDistributor::initialize: Could not initialize" sif::error << "CCSDSDistributor::initialize: Could not initialize"
" TC store!" << std::endl; " TC store!" << std::endl;
#else
sif::printError("CCSDSDistributor::initialize: Could not initialize"
" TC store!\n");
#endif
#endif #endif
status = RETURN_FAILED; status = RETURN_FAILED;
} }

53
tcdistribution/PUSDistributor.cpp
View File

@ -1,10 +1,12 @@
#include "CCSDSDistributorIF.h" #include "CCSDSDistributorIF.h"
#include "PUSDistributor.h" #include "PUSDistributor.h"
#include "../serviceinterface/ServiceInterfaceStream.h" #include "../serviceinterface/ServiceInterface.h"
#include "../tmtcpacket/pus/TcPacketStored.h" #include "../tmtcpacket/pus/TcPacketStored.h"
#include "../tmtcservices/PusVerificationReport.h" #include "../tmtcservices/PusVerificationReport.h"
#define PUS_DISTRIBUTOR_DEBUGGING 0
PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId, PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
object_id_t setPacketSource) : object_id_t setPacketSource) :
TcDistributor(setObjectId), checker(setApid), verifyChannel(), TcDistributor(setObjectId), checker(setApid), verifyChannel(),
@ -13,10 +15,10 @@ PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
PUSDistributor::~PUSDistributor() {} PUSDistributor::~PUSDistributor() {}
PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() { PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
// sif:: debug << "PUSDistributor::handlePacket received: " store_address_t storeId = this->currentMessage.getStorageId());
// << this->current_packet_id.store_index << ", " sif:: debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", " <<
// << this->current_packet_id.packet_index << std::endl; storeId.packetIndex << std::endl;
#endif #endif
TcMqMapIter queueMapIt = this->queueMap.end(); TcMqMapIter queueMapIt = this->queueMap.end();
if(this->currentPacket == nullptr) { if(this->currentPacket == nullptr) {
@ -25,15 +27,17 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
this->currentPacket->setStoreAddress(this->currentMessage.getStorageId()); this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
if (currentPacket->getWholeData() != nullptr) { if (currentPacket->getWholeData() != nullptr) {
tcStatus = checker.checkPacket(currentPacket); tcStatus = checker.checkPacket(currentPacket);
#ifdef DEBUG
if(tcStatus != HasReturnvaluesIF::RETURN_OK) { if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << "PUSDistributor::handlePacket: Packet format " sif::debug << "PUSDistributor::handlePacket: Packet format invalid, code " <<
<< "invalid, code "<< static_cast<int>(tcStatus) static_cast<int>(tcStatus) << std::endl;
<< std::endl; #else
sif::printDebug("PUSDistributor::handlePacket: Packet format invalid, code %d\n",
static_cast<int>(tcStatus));
#endif
#endif #endif
} }
#endif
uint32_t queue_id = currentPacket->getService(); uint32_t queue_id = currentPacket->getService();
queueMapIt = this->queueMap.find(queue_id); queueMapIt = this->queueMap.find(queue_id);
} }
@ -43,11 +47,12 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
if (queueMapIt == this->queueMap.end()) { if (queueMapIt == this->queueMap.end()) {
tcStatus = DESTINATION_NOT_FOUND; tcStatus = DESTINATION_NOT_FOUND;
#ifdef DEBUG #if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << "PUSDistributor::handlePacket: Destination not found, " sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
<< "code "<< static_cast<int>(tcStatus) << std::endl; #else
#endif sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
#endif #endif
} }
@ -63,15 +68,23 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) { ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
uint16_t serviceId = service->getIdentifier(); uint16_t serviceId = service->getIdentifier();
#if PUS_DISTRIBUTOR_DEBUGGING == 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
// sif::info << "Service ID: " << (int)serviceId << std::endl; sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
#else
sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
#endif
#endif #endif
MessageQueueId_t queue = service->getRequestQueue(); MessageQueueId_t queue = service->getRequestQueue();
auto returnPair = queueMap.emplace(serviceId, queue); auto returnPair = queueMap.emplace(serviceId, queue);
if (not returnPair.second) { if (not returnPair.second) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PUSDistributor::registerService: Service ID already" sif::error << "PUSDistributor::registerService: Service ID already"
" exists in map." << std::endl; " exists in map" << std::endl;
#else
sif::printError("PUSDistributor::registerService: Service ID already exists in map\n");
#endif
#endif #endif
return SERVICE_ID_ALREADY_EXISTS; return SERVICE_ID_ALREADY_EXISTS;
} }
@ -115,9 +128,11 @@ ReturnValue_t PUSDistributor::initialize() {
objectManager->get<CCSDSDistributorIF>(packetSource); objectManager->get<CCSDSDistributorIF>(packetSource);
if (ccsdsDistributor == nullptr) { if (ccsdsDistributor == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PUSDistributor::initialize: Packet source invalid." sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl;
<< " Make sure it exists and implements CCSDSDistributorIF!" sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
<< std::endl; #else
sif::printError("PUSDistributor::initialize: Packet source invalid\n");
sif::printError("Make sure it exists and implements CCSDSDistributorIF\n");
#endif #endif
return RETURN_FAILED; return RETURN_FAILED;
} }