58 lines
2.0 KiB
C++
58 lines
2.0 KiB
C++
#include "CcsdsUnpacker.h"
|
|
|
|
#include "fsfw/tmtcservices/TmTcMessage.h"
|
|
|
|
CcsdsUnpacker::CcsdsUnpacker(MessageQueueIF& msgQueue, AcceptsTelecommandsIF& receiver,
|
|
StorageManagerIF& sourceStore)
|
|
: sourceStore(sourceStore), msgQueue(msgQueue), receiver(receiver) {
|
|
msgQueue.setDefaultDestination(receiver.getRequestQueue());
|
|
}
|
|
|
|
ReturnValue_t CcsdsUnpacker::performOperation(uint8_t operationCode) {
|
|
TmTcMessage msg;
|
|
ReturnValue_t result;
|
|
for (result = msgQueue.receiveMessage(&msg); result == HasReturnvaluesIF::RETURN_OK;
|
|
result = msgQueue.receiveMessage(&msg)) {
|
|
auto resultPair = sourceStore.getData(msg.getStorageId());
|
|
if (resultPair.first != HasReturnvaluesIF::RETURN_OK) {
|
|
continue;
|
|
}
|
|
if (resultPair.second.size() < 6) {
|
|
// TODO: This is a config error. Does it make sense to forward the message?
|
|
result = msgQueue.sendToDefault(&msg);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
}
|
|
continue;
|
|
}
|
|
StorageManagerIF* tgtStore;
|
|
if (targetStore != nullptr) {
|
|
tgtStore = targetStore;
|
|
} else {
|
|
tgtStore = &sourceStore;
|
|
}
|
|
store_address_t newId;
|
|
uint8_t* ptr;
|
|
result = tgtStore->getFreeElement(&newId, resultPair.second.size(), &ptr);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
// TODO: Implement error handling
|
|
}
|
|
std::memcpy(ptr, resultPair.second.data() + 6, resultPair.second.size() - 6);
|
|
result = sourceStore.deleteData(msg.getStorageId());
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
// TODO: Implement error handling (though this really should not happen)
|
|
}
|
|
TmTcMessage newMsg(newId);
|
|
result = msgQueue.sendToDefault(&newMsg);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void CcsdsUnpacker::setDifferentTargetStore(StorageManagerIF& otherTargetStore) {
|
|
targetStore = &otherTargetStore;
|
|
}
|
|
|
|
uint32_t CcsdsUnpacker::getIdentifier() const { return 0; }
|
|
MessageQueueId_t CcsdsUnpacker::getRequestQueue() const { return 0; }
|