121 lines
4.1 KiB
C++
121 lines
4.1 KiB
C++
#include <fsfw/ipc/QueueFactory.h>
|
|
#include <fsfw/objectmanager/ObjectManager.h>
|
|
#include <fsfw/tmtcpacket/pus/TmPacketStored.h>
|
|
#include <fsfw/serviceinterface/ServiceInterface.h>
|
|
#include <fsfw/tmtcpacket/pus/TmPacketPusC.h>
|
|
#include <mission/utility/TmFunnel.h>
|
|
|
|
object_id_t TmFunnel::downlinkDestination = objects::NO_OBJECT;
|
|
object_id_t TmFunnel::storageDestination = objects::NO_OBJECT;
|
|
|
|
TmFunnel::TmFunnel(object_id_t objectId, uint32_t messageDepth):
|
|
SystemObject(objectId), messageDepth(messageDepth) {
|
|
tmQueue = QueueFactory::instance()->createMessageQueue(messageDepth,
|
|
MessageQueueMessage::MAX_MESSAGE_SIZE);
|
|
storageQueue = QueueFactory::instance()->createMessageQueue(messageDepth,
|
|
MessageQueueMessage::MAX_MESSAGE_SIZE);
|
|
}
|
|
|
|
TmFunnel::~TmFunnel() {
|
|
}
|
|
|
|
MessageQueueId_t TmFunnel::getReportReceptionQueue(uint8_t virtualChannel) {
|
|
return tmQueue->getId();
|
|
}
|
|
|
|
ReturnValue_t TmFunnel::performOperation(uint8_t operationCode) {
|
|
TmTcMessage currentMessage;
|
|
ReturnValue_t status = tmQueue->receiveMessage(¤tMessage);
|
|
while(status == HasReturnvaluesIF::RETURN_OK)
|
|
{
|
|
status = handlePacket(¤tMessage);
|
|
if(status != HasReturnvaluesIF::RETURN_OK){
|
|
break;
|
|
}
|
|
status = tmQueue->receiveMessage(¤tMessage);
|
|
}
|
|
|
|
if (status == MessageQueueIF::EMPTY) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
else {
|
|
return status;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t TmFunnel::handlePacket(TmTcMessage* message) {
|
|
uint8_t* packetData = nullptr;
|
|
size_t size = 0;
|
|
ReturnValue_t result = tmPool->modifyData(message->getStorageId(),
|
|
&packetData, &size);
|
|
if(result != HasReturnvaluesIF::RETURN_OK){
|
|
return result;
|
|
}
|
|
TmPacketPusC packet(packetData);
|
|
packet.setPacketSequenceCount(this->sourceSequenceCount);
|
|
sourceSequenceCount++;
|
|
sourceSequenceCount = sourceSequenceCount %
|
|
SpacePacketBase::LIMIT_SEQUENCE_COUNT;
|
|
packet.setErrorControl();
|
|
|
|
result = tmQueue->sendToDefault(message);
|
|
if(result != HasReturnvaluesIF::RETURN_OK){
|
|
tmPool->deleteData(message->getStorageId());
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "TmFunnel::handlePacket: Error sending to downlink handler" << std::endl;
|
|
#endif
|
|
return result;
|
|
}
|
|
|
|
if(storageDestination != objects::NO_OBJECT) {
|
|
result = storageQueue->sendToDefault(message);
|
|
if(result != HasReturnvaluesIF::RETURN_OK){
|
|
tmPool->deleteData(message->getStorageId());
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "TmFunnel::handlePacket: Error sending to storage handler" << std::endl;
|
|
#endif
|
|
return result;
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t TmFunnel::initialize() {
|
|
|
|
tmPool = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
|
|
if(tmPool == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "TmFunnel::initialize: TM store not set." << std::endl;
|
|
sif::error << "Make sure the tm store is set up properly and implements StorageManagerIF" <<
|
|
std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
AcceptsTelemetryIF* tmTarget = ObjectManager::instance()->
|
|
get<AcceptsTelemetryIF>(downlinkDestination);
|
|
if(tmTarget == nullptr){
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "TmFunnel::initialize: Downlink Destination not set." << std::endl;
|
|
sif::error << "Make sure the downlink destination object is set up properly and implements "
|
|
"AcceptsTelemetryIF" << std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
tmQueue->setDefaultDestination(tmTarget->getReportReceptionQueue());
|
|
|
|
// Storage destination is optional.
|
|
if(storageDestination == objects::NO_OBJECT) {
|
|
return SystemObject::initialize();
|
|
}
|
|
|
|
AcceptsTelemetryIF* storageTarget = ObjectManager::instance()->
|
|
get<AcceptsTelemetryIF>(storageDestination);
|
|
if(storageTarget != nullptr) {
|
|
storageQueue->setDefaultDestination(
|
|
storageTarget->getReportReceptionQueue());
|
|
}
|
|
|
|
return SystemObject::initialize();
|
|
}
|