Robin Mueller
6771d656bb
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
80 lines
3.1 KiB
C++
80 lines
3.1 KiB
C++
#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;
|
|
// sif::debug << "tm size: " << tmSize << " for " << destinations.size() << " destinations" <<
|
|
// std::endl;
|
|
for (unsigned int idx = 0; idx < destinations.size(); idx++) {
|
|
const auto& dest = destinations[idx];
|
|
if (dest.isFull) {
|
|
continue;
|
|
}
|
|
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 if (result == StorageManagerIF::DATA_STORAGE_FULL) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
|
|
uint8_t fillCounts[16];
|
|
uint8_t written = 0;
|
|
tmStore.getFillCount(fillCounts, &written);
|
|
sif::error << "Fill counts: [";
|
|
for (uint8_t fillIdx = 0; fillIdx < written; fillIdx++) {
|
|
sif::error << fillCounts[fillIdx];
|
|
if (fillIdx < written - 1) {
|
|
sif::error << ", ";
|
|
}
|
|
}
|
|
sif::error << "]" << std::endl;
|
|
#endif
|
|
}
|
|
} else {
|
|
message.setStorageId(origStoreId);
|
|
}
|
|
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
|
|
<< ", failed with code 0x" << std::hex << std::setw(4) << result << std::dec
|
|
<< std::endl;
|
|
#endif
|
|
tmStore.deleteData(message.getStorageId());
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
uint32_t PusLiveDemux::addDestination(const char* name,
|
|
const AcceptsTelemetryIF& downlinkDestination, uint8_t vcid) {
|
|
return addDestinationByRawId(name, downlinkDestination.getReportReceptionQueue(vcid), vcid);
|
|
}
|
|
|
|
void PusLiveDemux::setDestFull(uint32_t listIndex) {
|
|
if (destinations.size() > 0 and listIndex <= destinations.size() - 1) {
|
|
destinations[listIndex].isFull = true;
|
|
}
|
|
}
|
|
|
|
void PusLiveDemux::setDestAvailable(uint32_t listIndex) {
|
|
if (destinations.size() > 0 and listIndex <= destinations.size() - 1) {
|
|
destinations[listIndex].isFull = false;
|
|
}
|
|
}
|
|
|
|
uint32_t PusLiveDemux::addDestinationByRawId(const char* name, MessageQueueId_t downlinkDestination,
|
|
uint8_t vcid) {
|
|
destinations.emplace_back(name, downlinkDestination, vcid);
|
|
return destinations.size() - 1;
|
|
}
|