eive-obsw/mission/utility/TmFunnel.cpp

120 lines
4.1 KiB
C++
Raw Normal View History

2020-09-16 16:22:36 +02:00
#include <fsfw/ipc/QueueFactory.h>
2021-06-08 16:45:25 +02:00
#include <fsfw/objectmanager/ObjectManager.h>
2020-09-16 16:22:36 +02:00
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
2022-01-17 15:58:27 +01:00
#include <fsfw/tmtcpacket/pus/tm.h>
2020-09-30 17:17:01 +02:00
#include <mission/utility/TmFunnel.h>
2020-09-16 16:22:36 +02:00
2022-01-17 15:58:27 +01:00
#include "OBSWConfig.h"
2020-09-16 16:22:36 +02:00
object_id_t TmFunnel::downlinkDestination = objects::NO_OBJECT;
object_id_t TmFunnel::storageDestination = objects::NO_OBJECT;
2022-01-17 15:58:27 +01:00
TmFunnel::TmFunnel(object_id_t objectId, uint32_t messageDepth)
: SystemObject(objectId), messageDepth(messageDepth) {
2022-02-22 20:27:13 +01:00
auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
tmQueue = QueueFactory::instance()->createMessageQueue(
messageDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
2022-01-17 15:58:27 +01:00
storageQueue = QueueFactory::instance()->createMessageQueue(
2022-02-22 20:27:13 +01:00
messageDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
2020-09-16 16:22:36 +02:00
}
2022-01-17 15:58:27 +01:00
TmFunnel::~TmFunnel() {}
2020-09-16 16:22:36 +02:00
MessageQueueId_t TmFunnel::getReportReceptionQueue(uint8_t virtualChannel) {
2022-01-17 15:58:27 +01:00
return tmQueue->getId();
2020-09-16 16:22:36 +02:00
}
ReturnValue_t TmFunnel::performOperation(uint8_t operationCode) {
2022-01-17 15:58:27 +01:00
TmTcMessage currentMessage;
ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
while (status == HasReturnvaluesIF::RETURN_OK) {
status = handlePacket(&currentMessage);
if (status != HasReturnvaluesIF::RETURN_OK) {
break;
}
status = tmQueue->receiveMessage(&currentMessage);
}
if (status == MessageQueueIF::EMPTY) {
return HasReturnvaluesIF::RETURN_OK;
} else {
return status;
}
2020-09-16 16:22:36 +02:00
}
ReturnValue_t TmFunnel::handlePacket(TmTcMessage* message) {
2022-01-17 15:58:27 +01:00
uint8_t* packetData = nullptr;
size_t size = 0;
ReturnValue_t result = tmStore->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) {
tmStore->deleteData(message->getStorageId());
sif::error << "TmFunnel::handlePacket: Error sending to downlink "
"handler"
<< std::endl;
return result;
}
if (storageDestination != objects::NO_OBJECT) {
result = storageQueue->sendToDefault(message);
if (result != HasReturnvaluesIF::RETURN_OK) {
tmStore->deleteData(message->getStorageId());
sif::error << "TmFunnel::handlePacket: Error sending to storage "
"handler"
<< std::endl;
return result;
}
}
return result;
2020-09-16 16:22:36 +02:00
}
ReturnValue_t TmFunnel::initialize() {
2022-01-17 15:58:27 +01:00
tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
if (tmStore == nullptr) {
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;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
AcceptsTelemetryIF* tmTarget =
ObjectManager::instance()->get<AcceptsTelemetryIF>(downlinkDestination);
if (tmTarget == nullptr) {
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;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
2021-09-19 12:27:48 +02:00
2021-09-26 08:29:30 +02:00
#if OBSW_TM_TO_PTME == 1
2022-01-17 15:58:27 +01:00
// Live TM will be sent via the virtual channel 0
tmQueue->setDefaultDestination(tmTarget->getReportReceptionQueue(config::LIVE_TM));
2021-09-19 12:27:48 +02:00
#else
2022-01-17 15:58:27 +01:00
tmQueue->setDefaultDestination(tmTarget->getReportReceptionQueue());
2021-09-26 08:29:30 +02:00
#endif /* OBSW_TM_TO_PTME == 1 */
2020-09-16 16:22:36 +02:00
2022-01-17 15:58:27 +01:00
// Storage destination is optional.
if (storageDestination == objects::NO_OBJECT) {
return SystemObject::initialize();
}
2020-09-30 17:17:01 +02:00
2022-01-17 15:58:27 +01:00
AcceptsTelemetryIF* storageTarget =
ObjectManager::instance()->get<AcceptsTelemetryIF>(storageDestination);
if (storageTarget != nullptr) {
storageQueue->setDefaultDestination(storageTarget->getReportReceptionQueue());
}
2020-09-16 16:22:36 +02:00
2022-01-17 15:58:27 +01:00
return SystemObject::initialize();
2020-09-16 16:22:36 +02:00
}