eive-obsw/test/testtasks/PusTcInjector.cpp

#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw/tmtcpacket/pus/TcPacketBase.h>
#include <fsfw/tmtcpacket/pus/TcPacketStored.h>
#include <fsfw/tmtcservices/AcceptsTelecommandsIF.h>
#include <fsfw/tmtcservices/TmTcMessage.h>
#include <test/testtasks/PusTcInjector.h>

PusTcInjector::PusTcInjector(object_id_t objectId, object_id_t destination, object_id_t tcStore,
                             uint16_t defaultApid)
    : SystemObject(objectId),
      defaultApid(defaultApid),
      destination(destination),
      tcStoreId(tcStore) {}

PusTcInjector::~PusTcInjector() {}

// ReturnValue_t PusTcInjector::injectPusTelecommand(uint8_t service,
//		uint8_t subservice,const uint8_t* appData, size_t appDataLen) {
//	return injectPusTelecommand(service, subservice, defaultApid, appData,
//			appDataLen);
// }

// TODO: ACK flags
// ReturnValue_t PusTcInjector::injectPusTelecommand(uint8_t service,
//		uint8_t subservice,uint16_t apid, const uint8_t* appData,
//		size_t appDataLen) {
//	// Prepare TC packet. Store into TC store immediately.
//	TcPacketStored tcPacket(service, subservice, apid, sequenceCount++);
//
//	const uint8_t* packetPtr = nullptr;
//	size_t packetSize = 0;
//	tcPacket.getData(&packetPtr, &packetSize);
//	//arrayprinter::print(packetPtr, packetSize, OutputType::BIN);
//
//	// Send TC packet.
//	TmTcMessage tcMessage(tcPacket.getStoreAddress());
//	ReturnValue_t result = injectionQueue->sendToDefault(&tcMessage);
//	if(result != returnvalue::OK) {
//		sif::warning << "PusTcInjector: Sending TMTC message failed!" << std::endl;
//	}
//	return result;
//}

ReturnValue_t PusTcInjector::initialize() {
  // Prepare message queue which is used to send telecommands.
  injectionQueue = QueueFactory::instance()->createMessageQueue(INJECTION_QUEUE_DEPTH);
  AcceptsTelecommandsIF* targetQueue =
      ObjectManager::instance()->get<AcceptsTelecommandsIF>(destination);
  if (targetQueue == nullptr) {
    sif::error << "PusTcInjector: CCSDS distributor not initialized yet!" << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  } else {
    injectionQueue->setDefaultDestination(targetQueue->getRequestQueue());
  }

  // Prepare store used to store TC messages
  tcStore = ObjectManager::instance()->get<StorageManagerIF>(tcStoreId);
  if (tcStore == nullptr) {
    sif::error << "PusTcInjector: TC Store not initialized!" << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  return returnvalue::OK;
}