bugfix for TM packet stored PUS C

src/fsfw/tmtcpacket/SpacePacketBase.cpp

@@ -3,8 +3,8 @@
 
 #include <cstring>
 
-SpacePacketBase::SpacePacketBase( const uint8_t* set_address ) {
+SpacePacketBase::SpacePacketBase(const uint8_t* setAddress) {
-	this->data = (SpacePacketPointer*) set_address;
+	this->data = reinterpret_cast<SpacePacketPointer*>(const_cast<uint8_t*>(setAddress));
 }
 
 SpacePacketBase::~SpacePacketBase() {
@@ -15,10 +15,21 @@ uint8_t SpacePacketBase::getPacketVersionNumber( void ) {
 	return (this->data->header.packet_id_h & 0b11100000) >> 5;
 }
 
-void SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
+ReturnValue_t SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
 		bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
+    if(data == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "SpacePacketBase::initSpacePacketHeader: Data pointer is invalid"
+                << std::endl;
+#else
+        sif::printWarning("SpacePacketBase::initSpacePacketHeader: Data pointer is invalid!\n");
+#endif
+#endif
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 	//reset header to zero:
-	memset(data,0, sizeof(this->data->header) );
+	memset(data, 0, sizeof(this->data->header) );
 	//Set TC/TM bit.
 	data->header.packet_id_h = ((isTelecommand? 1 : 0)) << 4;
 	//Set secondaryHeader bit
@@ -27,7 +38,7 @@ void SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
 	//Always initialize as standalone packets.
 	data->header.sequence_control_h = 0b11000000;
 	setPacketSequenceCount(sequenceCount);
-
+	return HasReturnvaluesIF::RETURN_OK;
 }
 
 bool SpacePacketBase::isTelecommand( void ) {

src/fsfw/tmtcpacket/SpacePacketBase.h

@@ -2,6 +2,8 @@
 #define FSFW_TMTCPACKET_SPACEPACKETBASE_H_
 
 #include "ccsds_header.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+
 #include <cstddef>
 
 /**
@@ -82,7 +84,7 @@ public:
 	 */
 	bool isTelecommand( void );
 
-	void initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
+	ReturnValue_t initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
 	        uint16_t apid, uint16_t sequenceCount = 0);
 	/**
 	 * The CCSDS header provides a secondary header flag (the fifth-highest bit),

src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp

@@ -53,11 +53,14 @@ uint8_t* TmPacketPusC::getPacketTimeRaw() const{
 
 }
 
-void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
+ReturnValue_t TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
         uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId,
         uint8_t timeRefField) {
     //Set primary header:
-    initSpacePacketHeader(false, true, apid);
+    ReturnValue_t result = initSpacePacketHeader(false, true, apid);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
     //Set data Field Header:
     //First, set to zero.
     memset(&tmData->dataField, 0, sizeof(tmData->dataField));
@@ -76,6 +79,7 @@ void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
         timeStamper->addTimeStamp(tmData->dataField.time,
                 sizeof(tmData->dataField.time));
     }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void TmPacketPusC::setSourceDataSize(uint16_t size) {

src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h

@@ -100,7 +100,7 @@ protected:
      * @param subservice PUS Subservice
      * @param packetSubcounter Additional subcounter used.
      */
-    void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
+    ReturnValue_t initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
             uint16_t packetSubcounter, uint16_t destinationId = 0, uint8_t timeRefField = 0);
 
     /**

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp

@@ -43,27 +43,56 @@ TmPacketStoredPusC::TmPacketStoredPusC(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 *p_data = nullptr;
+    size_t sizeToReserve = getPacketMinimumSize() + sourceDataSize;
     ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
-            (getPacketMinimumSize() + sourceDataSize), &p_data);
+            sizeToReserve, &p_data);
     if (returnValue != store->RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+        switch(returnValue) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        case(StorageManagerIF::DATA_STORAGE_FULL): {
+            sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with "
+                    "size " << sizeToReserve << std::endl;
+            break;
+        }
+        case(StorageManagerIF::DATA_TOO_LARGE): {
+            sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Data with size " <<
+                    sizeToReserve << " too large" <<  std::endl;
+            break;
+        }
+#else
+        case(StorageManagerIF::DATA_STORAGE_FULL): {
+            sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with "
+                    "size %d\n", sizeToReserve);
+            break;
+        }
+        case(StorageManagerIF::DATA_TOO_LARGE): {
+            sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Data with size "
+                    "%d too large\n", sizeToReserve);
+            break;
+        }
+#endif
+#endif
+        }
         TmPacketStoredBase::checkAndReportLostTm();
+        return;
     }
     setData(p_data);
     initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
     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);
     }