diff --git a/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp b/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp index 6f249667..3b183f09 100644 --- a/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp @@ -29,7 +29,8 @@ TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service, initializeTmPacket(apid, service, subservice, packetSubcounter); memcpy(getSourceData(), headerData, headerSize); memcpy(getSourceData() + headerSize, data, size); - setPacketDataLength(size + headerSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1); + setPacketDataLength( + size + headerSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1); } TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service, @@ -41,32 +42,60 @@ TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service, return; } size_t sourceDataSize = 0; - if (content != NULL) { + if (content != nullptr) { sourceDataSize += content->getSerializedSize(); } - if (header != NULL) { + if (header != nullptr) { sourceDataSize += header->getSerializedSize(); } - uint8_t *p_data = NULL; + uint8_t *pData = nullptr; + size_t sizeToReserve = getPacketMinimumSize() + sourceDataSize; ReturnValue_t returnValue = store->getFreeElement(&storeAddress, - (getPacketMinimumSize() + sourceDataSize), &p_data); + sizeToReserve, &pData); if (returnValue != store->RETURN_OK) { TmPacketStoredBase::checkAndReportLostTm(); +#if FSFW_VERBOSE_LEVEL >= 1 + switch(returnValue) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + case(StorageManagerIF::DATA_STORAGE_FULL): { + sif::warning << "TmPacketStoredPusA::TmPacketStoredPusC: Store full for packet with " + "size " << sizeToReserve << std::endl; + break; + } + case(StorageManagerIF::DATA_TOO_LARGE): { + sif::warning << "TmPacketStoredPusA::TmPacketStoredPusC: Data with size " << + sizeToReserve << " too large" << std::endl; + break; + } +#else + case(StorageManagerIF::DATA_STORAGE_FULL): { + sif::printWarning("TmPacketStoredPusA::TmPacketStoredPusC: Store full for packet with " + "size %d\n", sizeToReserve); + break; + } + case(StorageManagerIF::DATA_TOO_LARGE): { + sif::printWarning("TmPacketStoredPusA::TmPacketStoredPusC: Data with size " + "%d too large\n", sizeToReserve); + break; + } +#endif +#endif + } + return; } - setData(p_data); + setData(pData); initializeTmPacket(apid, service, subservice, packetSubcounter); uint8_t *putDataHere = getSourceData(); size_t size = 0; - if (header != NULL) { + if (header != nullptr) { header->serialize(&putDataHere, &size, sourceDataSize, SerializeIF::Endianness::BIG); } - if (content != NULL) { + if (content != nullptr) { content->serialize(&putDataHere, &size, sourceDataSize, SerializeIF::Endianness::BIG); } - setPacketDataLength( - sourceDataSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1); + setPacketDataLength(sourceDataSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1); } uint8_t* TmPacketStoredPusA::getAllTmData() {