Robin Mueller
d9453c3b83
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
61 lines
2.3 KiB
C++
61 lines
2.3 KiB
C++
#include "TmFunnelBase.h"
|
|
|
|
#include <fsfw/tmtcservices/TmTcMessage.h>
|
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
|
|
TmFunnelBase::TmFunnelBase(object_id_t objectId, StorageManagerIF &tmStore, uint32_t tmMsgDepth,
|
|
uint32_t tcMsgDepth, StorageManagerIF &ipcStore)
|
|
: SystemObject(objectId), tmStore(tmStore), ipcStore(ipcStore) {
|
|
tmQueue = QueueFactory::instance()->createMessageQueue(tmMsgDepth);
|
|
tcQueue = QueueFactory::instance()->createMessageQueue(tcMsgDepth);
|
|
}
|
|
|
|
MessageQueueId_t TmFunnelBase::getCommandQueue() const { return tcQueue->getId(); }
|
|
|
|
TmFunnelBase::~TmFunnelBase() { QueueFactory::instance()->deleteMessageQueue(tmQueue); }
|
|
|
|
MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) const {
|
|
return tmQueue->getId();
|
|
}
|
|
|
|
void TmFunnelBase::addDestination(const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) {
|
|
auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
|
|
destinations.emplace_back(queueId, vcid);
|
|
}
|
|
|
|
ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId,
|
|
TmTcMessage &message,
|
|
const uint8_t *packetData, size_t size) {
|
|
ReturnValue_t result;
|
|
for (unsigned int idx = 0; idx < destinations.size(); idx++) {
|
|
const auto &destVcidPair = destinations[idx];
|
|
if (destinations.size() > 1) {
|
|
if (idx < destinations.size() - 1) {
|
|
// Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
|
|
// to bother with send order and where the data is deleted.
|
|
store_address_t storeId;
|
|
result = tmStore.addData(&storeId, packetData, size);
|
|
if (result == returnvalue::OK) {
|
|
message.setStorageId(storeId);
|
|
} else {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
|
|
<< std::endl;
|
|
#endif
|
|
}
|
|
} else {
|
|
message.setStorageId(origStoreId);
|
|
}
|
|
}
|
|
result = tmQueue->sendMessage(destVcidPair.first, &message);
|
|
if (result != returnvalue::OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
|
|
#endif
|
|
tmStore.deleteData(message.getStorageId());
|
|
}
|
|
}
|
|
return result;
|
|
}
|