tmtc bridge update, uses dynamic FIFO now

tmtcservices/TmTcBridge.cpp

@@ -1,7 +1,7 @@
-#include "TmTcBridge.h"
+#include "../tmtcservices/TmTcBridge.h"
 
 #include "../ipc/QueueFactory.h"
-#include "AcceptsTelecommandsIF.h"
+#include "../tmtcservices/AcceptsTelecommandsIF.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../globalfunctions/arrayprinter.h"
 
@@ -66,6 +66,8 @@ ReturnValue_t TmTcBridge::initialize() {
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
+	tmFifo = new DynamicFIFO<store_address_t>(maxNumberOfPacketsStored);
+
 	tmTcReceptionQueue->setDefaultDestination(tcDistributor->getRequestQueue());
 	return RETURN_OK;
 }
@@ -90,102 +92,122 @@ ReturnValue_t TmTcBridge::handleTc() {
 }
 
 ReturnValue_t TmTcBridge::handleTm() {
+    ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
 	ReturnValue_t result = handleTmQueue();
 	if(result != RETURN_OK) {
-		sif::warning << "TmTcBridge: Reading TM Queue failed" << std::endl;
+		sif::error << "TmTcBridge::handleTm: Error handling TM queue!"
-		return RETURN_FAILED;
+		        << std::endl;
+		status = result;
 	}
 
-	if(tmStored and communicationLinkUp) {
+	if(tmStored and communicationLinkUp and
-		result = handleStoredTm();
+	        (packetSentCounter < sentPacketsPerCycle)) {
+	    result = handleStoredTm();
+	    if(result != RETURN_OK) {
+	        sif::error << "TmTcBridge::handleTm: Error handling stored TMs!"
+	                << std::endl;
+	        status = result;
+	    }
 	}
-	return result;
+	packetSentCounter = 0;
-
+	return status;
 }
 
 ReturnValue_t TmTcBridge::handleTmQueue() {
 	TmTcMessage message;
 	const uint8_t* data = nullptr;
 	size_t size = 0;
+	ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
 	for (ReturnValue_t result = tmTcReceptionQueue->receiveMessage(&message);
-		 result == RETURN_OK; result = tmTcReceptionQueue->receiveMessage(&message))
+		 result == HasReturnvaluesIF::RETURN_OK;
+		 result = tmTcReceptionQueue->receiveMessage(&message))
 	{
-		if(communicationLinkUp == false) {
+	    //sif::info << (int) packetSentCounter << std::endl;
-			result = storeDownlinkData(&message);
+		if(communicationLinkUp == false or
-			return result;
+		        packetSentCounter >= sentPacketsPerCycle) {
+			storeDownlinkData(&message);
+			continue;
 		}
 
 		result = tmStore->getData(message.getStorageId(), &data, &size);
 		if (result != HasReturnvaluesIF::RETURN_OK) {
+			status = result;
 			continue;
 		}
 
 		result = sendTm(data, size);
-		if (result != RETURN_OK) {
+		if (result != HasReturnvaluesIF::RETURN_OK) {
-			sif::warning << "TmTcBridge: Could not send TM packet" << std::endl;
+			status = result;
-			tmStore->deleteData(message.getStorageId());
+		}
-			return result;
+		else {
-
+		    tmStore->deleteData(message.getStorageId());
+		    packetSentCounter++;
 		}
-		tmStore->deleteData(message.getStorageId());
 	}
-	return RETURN_OK;
+	return status;
 }
 
 ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) {
 	store_address_t storeId = 0;
 
-	if(tmFifo.full()) {
+	if(tmFifo->full()) {
-		sif::error << "TmTcBridge::storeDownlinkData: TM downlink max. number "
+	    sif::debug << "TmTcBridge::storeDownlinkData: TM downlink max. number "
-		        << "of stored packet IDs reached! "
+	                    << "of stored packet IDs reached! " << std::endl;
-		        << "Overwriting old data" << std::endl;
+	    if(overwriteOld) {
-		tmFifo.retrieve(&storeId);
+	        tmFifo->retrieve(&storeId);
-		tmStore->deleteData(storeId);
+	        tmStore->deleteData(storeId);
+	    }
+	    else {
+	        return HasReturnvaluesIF::RETURN_FAILED;
+	    }
 	}
+
 	storeId = message->getStorageId();
-	tmFifo.insert(storeId);
+	tmFifo->insert(storeId);
 	tmStored = true;
 	return RETURN_OK;
 }
 
 ReturnValue_t TmTcBridge::handleStoredTm() {
-	uint8_t counter = 0;
+    ReturnValue_t status = RETURN_OK;
-	ReturnValue_t result = RETURN_OK;
+	while(not tmFifo->empty() and packetSentCounter < sentPacketsPerCycle) {
-	while(not tmFifo.empty() and counter < sentPacketsPerCycle) {
+		//sif::info << "TMTC Bridge: Sending stored TM data. There are "
-		//info << "TMTC Bridge: Sending stored TM data. There are "
+		//     << (int) tmFifo->size() << " left to send\r\n" << std::flush;
-		//     << (int) fifo.size() << " left to send\r\n" << std::flush;
+
 		store_address_t storeId;
 		const uint8_t* data = nullptr;
 		size_t size = 0;
-		tmFifo.retrieve(&storeId);
+		tmFifo->retrieve(&storeId);
-		result = tmStore->getData(storeId, &data, &size);
+		ReturnValue_t result = tmStore->getData(storeId, &data, &size);
-
+		if(result != HasReturnvaluesIF::RETURN_OK) {
-		sendTm(data,size);
+		    status = result;
+		}
 
+		result = sendTm(data,size);
 		if(result != RETURN_OK) {
 			sif::error << "TMTC Bridge: Could not send stored downlink data"
 			      << std::endl;
-			result = RETURN_FAILED;
+			status = result;
 		}
-		counter ++;
+		packetSentCounter ++;
 
-		if(tmFifo.empty()) {
+		if(tmFifo->empty()) {
 			tmStored = false;
 		}
 		tmStore->deleteData(storeId);
 	}
-	return result;
+	return status;
 }
 
 void TmTcBridge::registerCommConnect() {
 	if(not communicationLinkUp) {
-		//info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
+		//sif::info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
 		communicationLinkUp = true;
 	}
 }
 
 void TmTcBridge::registerCommDisconnect() {
-	//info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
+	//sif::info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
 	if(communicationLinkUp) {
 		communicationLinkUp = false;
 	}
@@ -209,3 +231,7 @@ MessageQueueId_t TmTcBridge::getRequestQueue() {
 	// Default implementation: Relay TC messages to TC distributor directly.
 	return tmTcReceptionQueue->getDefaultDestination();
 }
+
+void TmTcBridge::setFifoToOverwriteOldData(bool overwriteOld) {
+    this->overwriteOld = overwriteOld;
+}

tmtcservices/TmTcBridge.h

@@ -1,15 +1,15 @@
 #ifndef FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_
 #define FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_
 
+
 #include "../objectmanager/SystemObject.h"
-#include "AcceptsTelemetryIF.h"
+#include "../tmtcservices/AcceptsTelemetryIF.h"
 #include "../tasks/ExecutableObjectIF.h"
 #include "../ipc/MessageQueueIF.h"
 #include "../storagemanager/StorageManagerIF.h"
-#include "AcceptsTelecommandsIF.h"
+#include "../tmtcservices/AcceptsTelecommandsIF.h"
-
+#include "../container/DynamicFIFO.h"
-#include "../container/FIFO.h"
+#include "../tmtcservices/TmTcMessage.h"
-#include "TmTcMessage.h"
 
 class TmTcBridge : public AcceptsTelemetryIF,
 		public AcceptsTelecommandsIF,
@@ -46,6 +46,12 @@ public:
 	 */
 	ReturnValue_t setMaxNumberOfPacketsStored(uint8_t maxNumberOfPacketsStored);
 
+	/**
+	 * This will set up the bridge to overwrite old data in the FIFO.
+	 * @param overwriteOld
+	 */
+	void setFifoToOverwriteOldData(bool overwriteOld);
+
 	virtual void registerCommConnect();
 	virtual void registerCommDisconnect();
 
@@ -86,6 +92,8 @@ protected:
 	//! by default, so telemetry will be handled immediately.
 	bool communicationLinkUp = true;
 	bool tmStored = false;
+	bool overwriteOld = true;
+	uint8_t packetSentCounter = 0;
 
 	/**
 	 * @brief 	Handle TC reception
@@ -145,7 +153,7 @@ protected:
 	 * This fifo can be used to store downlink data
 	 * which can not be sent at the moment.
 	 */
-	FIFO<store_address_t, LIMIT_DOWNLINK_PACKETS_STORED> tmFifo;
+	DynamicFIFO<store_address_t>* tmFifo = nullptr;
     uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE;
     uint8_t maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED;
 };