eive-obsw/mission/tmtc/CfdpTmFunnel.cpp

91 lines
3.3 KiB
C++

#include "CfdpTmFunnel.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
#include "fsfw/tmtcservices/TmTcMessage.h"
CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid,
StorageManagerIF& tmStore)
: TmFunnelBase(objectId, tmStore, 10), cfdpInCcsdsApid(cfdpInCcsdsApid) {}
const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }
ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
TmTcMessage currentMessage;
ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
while (status == returnvalue::OK) {
status = handlePacket(currentMessage);
if (status != returnvalue::OK) {
sif::warning << "CfdpTmFunnel packet handling failed" << std::endl;
break;
}
status = tmQueue->receiveMessage(&currentMessage);
}
if (status == MessageQueueIF::EMPTY) {
return returnvalue::OK;
}
return status;
}
ReturnValue_t CfdpTmFunnel::initialize() { return returnvalue::OK; }
ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
const uint8_t* cfdpPacket = nullptr;
size_t cfdpPacketLen = 0;
ReturnValue_t result = tmStore.getData(msg.getStorageId(), &cfdpPacket, &cfdpPacketLen);
if (result != returnvalue::OK) {
return result;
}
auto spacePacketHeader =
SpacePacketCreator(ccsds::PacketType::TM, false, cfdpInCcsdsApid,
ccsds::SequenceFlags::UNSEGMENTED, sourceSequenceCount++, 0);
sourceSequenceCount = sourceSequenceCount & ccsds::LIMIT_SEQUENCE_COUNT;
spacePacketHeader.setCcsdsLenFromTotalDataFieldLen(cfdpPacketLen);
uint8_t* newPacketData = nullptr;
store_address_t newStoreId{};
result =
tmStore.getFreeElement(&newStoreId, spacePacketHeader.getFullPacketLen(), &newPacketData);
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "CfdpTmFunnel::handlePacket: Error getting TM store element of size "
<< spacePacketHeader.getFullPacketLen() << std::endl;
#endif
return result;
}
size_t serSize = 0;
result =
spacePacketHeader.serializeBe(&newPacketData, &serSize, spacePacketHeader.getFullPacketLen());
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CfdpTmFunnel::handlePacket: Error serializing packet" << std::endl;
#endif
return result;
}
std::memcpy(newPacketData, cfdpPacket, cfdpPacketLen);
// Delete old packet
tmStore.deleteData(msg.getStorageId());
msg.setStorageId(newStoreId);
store_address_t origStoreId = newStoreId;
for (unsigned int idx = 0; idx < destinations.size(); idx++) {
const auto& destVcidPair = destinations[idx];
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, newPacketData, serSize);
msg.setStorageId(storeId);
} else {
msg.setStorageId(origStoreId);
}
result = tmQueue->sendMessage(destVcidPair.first, &msg);
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(msg.getStorageId());
}
}
return result;
}