eive-obsw/mission/tmtc/PusLiveDemux.cpp

47 lines
1.9 KiB
C++
Raw Normal View History

2023-03-09 11:46:13 +01:00
#include "PusLiveDemux.h"
#include <fsfw/storagemanager/storeAddress.h>
#include <fsfw/tmtcservices/TmTcMessage.h>
PusLiveDemux::PusLiveDemux(MessageQueueIF& ownerQueue) : ownerQueue(ownerQueue) {}
ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore,
store_address_t origStoreId, const uint8_t* tmData,
size_t tmSize) {
ReturnValue_t result = returnvalue::OK;
for (unsigned int idx = 0; idx < destinations.size(); idx++) {
const auto& dest = destinations[idx];
2023-06-23 17:53:19 +02:00
if ((destinations.size() > 1) and (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, tmData, tmSize);
if (result == returnvalue::OK) {
message.setStorageId(storeId);
} else {
2023-03-09 11:46:13 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2023-06-23 17:53:19 +02:00
sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
2023-03-09 11:46:13 +01:00
#endif
}
2023-06-23 17:44:18 +02:00
} else {
2023-06-23 17:47:18 +02:00
message.setStorageId(origStoreId);
2023-03-09 11:46:13 +01:00
}
result = ownerQueue.sendMessage(dest.queueId, &message);
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusLiveDemux::handlePacket: Error sending TM to downlink handler " << dest.name
2023-04-14 21:02:39 +02:00
<< ", failed with code 0x" << std::hex << std::setw(4) << result << std::dec
2023-03-09 11:46:13 +01:00
<< std::endl;
#endif
tmStore.deleteData(message.getStorageId());
}
}
return result;
}
void PusLiveDemux::addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid) {
auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
destinations.emplace_back(name, queueId, vcid);
}