125 lines
3.8 KiB
C++
125 lines
3.8 KiB
C++
#include "TmManager.h"
|
|
|
|
#include <fsfw/ipc/MessageQueueSenderIF.h>
|
|
#include <fsfw/objectmanager/ObjectManager.h>
|
|
|
|
#include "TmMessage.h"
|
|
|
|
TmManager::TmManager(object_id_t setObjectId) : SystemObject(setObjectId) {}
|
|
|
|
ReturnValue_t TmManager::initialize() {
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
|
if (IPCStore == nullptr) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t TmManager::sendTmPacket(object_id_t objectId,
|
|
FsfwProtocolHeader::InterfaceType_t interface,
|
|
FsfwProtocolHeader::FunctionType_t function,
|
|
SerializeIF* applicationData, store_address_t tc,
|
|
size_t tcOffset) {
|
|
ProtocolInformation information;
|
|
|
|
ReturnValue_t result = getProtocolInformation(tc, &information);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
const FsfwProtocolHeader tmHeader = FsfwProtocolHeader(objectId, interface, function);
|
|
|
|
const size_t storedTcSize =
|
|
information.offset + tmHeader.getSerializedSize() + applicationData->getSerializedSize();
|
|
|
|
store_address_t tm;
|
|
uint8_t* dataPointer;
|
|
result = IPCStore->getFreeElement(&tm, storedTcSize, &dataPointer);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
*dataPointer = information.protocol;
|
|
|
|
size_t serializedSize = information.offset;
|
|
dataPointer += information.offset;
|
|
|
|
result = tmHeader.serialize(&dataPointer, &serializedSize, storedTcSize,
|
|
SerializeIF::Endianness::NETWORK);
|
|
if (result != returnvalue::OK) {
|
|
IPCStore->deleteData(tm);
|
|
return result;
|
|
}
|
|
result = applicationData->serialize(&dataPointer, &serializedSize, storedTcSize,
|
|
SerializeIF::Endianness::NETWORK);
|
|
if (result != returnvalue::OK) {
|
|
IPCStore->deleteData(tm);
|
|
return result;
|
|
}
|
|
|
|
CommandMessage tmMessage;
|
|
TmMessage::setCommand(&tmMessage, tm, tc);
|
|
|
|
result = MessageQueueSenderIF::sendMessage(information.reportingQueue, &tmMessage);
|
|
if (result != returnvalue::OK) {
|
|
IPCStore->deleteData(tm);
|
|
return result;
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t TmManager::getProtocolInformation(store_address_t tc,
|
|
ProtocolInformation* information) const {
|
|
const uint8_t* tcData;
|
|
size_t tcDataSize;
|
|
ReturnValue_t result = IPCStore->getData(tc, &tcData, &tcDataSize);
|
|
if (result != returnvalue::OK) {
|
|
return getDefaultProtocolInformation(information);
|
|
}
|
|
|
|
neither_type tcProtocol = neither_type(*tcData);
|
|
|
|
if (not protocolMap.contains(tcProtocol)) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
auto protocolIter = protocolMap.find(tcProtocol);
|
|
|
|
information->reportingQueue = protocolIter->second.queue;
|
|
information->offset = protocolIter->second.offset;
|
|
information->protocol = tcProtocol;
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t TmManager::registerNetworkProtocolInterface(AcceptsTelemetryIF2* object,
|
|
neither_type protocol, size_t offset) {
|
|
if (protocolMap.contains(protocol)) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
protocolMap.emplace(protocol,
|
|
StoredProtocolInformation({object->getReportReceptionQueue(), offset}));
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t TmManager::getDefaultProtocolInformation(ProtocolInformation* information) const {
|
|
if (protocolMap.empty()) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
auto protocolIter = protocolMap.begin();
|
|
|
|
information->reportingQueue = protocolIter->second.queue;
|
|
information->offset = protocolIter->second.offset;
|
|
information->protocol = protocolIter->first;
|
|
return returnvalue::OK;
|
|
}
|