Merge branch 'master' into mueller/HealthHelperIdFix

globalfunctions/timevalOperations.cpp

@@ -90,3 +90,10 @@ double timevalOperations::toDouble(const timeval timeval) {
 	double result = timeval.tv_sec * 1000000. + timeval.tv_usec;
 	return result / 1000000.;
 }
+
+timeval timevalOperations::toTimeval(const double seconds) {
+	timeval tval;
+	tval.tv_sec = seconds;
+	tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6);
+	return tval;
+}

globalfunctions/timevalOperations.h

@@ -41,6 +41,7 @@ namespace timevalOperations {
  * @return seconds
  */
 double toDouble(const timeval timeval);
+timeval toTimeval(const double seconds);
 }
 
 #endif /* TIMEVALOPERATIONS_H_ */

osal/linux/TcUnixUdpPollingTask.cpp

@@ -0,0 +1,138 @@
+#include "TcUnixUdpPollingTask.h"
+#include "../../globalfunctions/arrayprinter.h"
+
+TcUnixUdpPollingTask::TcUnixUdpPollingTask(object_id_t objectId,
+		object_id_t tmtcUnixUdpBridge, size_t frameSize,
+		double timeoutSeconds): SystemObject(objectId),
+		tmtcBridgeId(tmtcUnixUdpBridge) {
+
+	if(frameSize > 0) {
+		this->frameSize = frameSize;
+	}
+	else {
+		this->frameSize = DEFAULT_MAX_FRAME_SIZE;
+	}
+
+	// Set up reception buffer with specified frame size.
+	// For now, it is assumed that only one frame is held in the buffer!
+	receptionBuffer.reserve(this->frameSize);
+	receptionBuffer.resize(this->frameSize);
+
+	if(timeoutSeconds == -1) {
+		receptionTimeout = DEFAULT_TIMEOUT;
+	}
+	else {
+		receptionTimeout = timevalOperations::toTimeval(timeoutSeconds);
+	}
+}
+
+TcUnixUdpPollingTask::~TcUnixUdpPollingTask() {}
+
+ReturnValue_t TcUnixUdpPollingTask::performOperation(uint8_t opCode) {
+	// Poll for new UDP datagrams in permanent loop.
+	while(1) {
+		//! Sender Address is cached here.
+		struct sockaddr_in senderAddress;
+		socklen_t senderSockLen = 0;
+		ssize_t bytesReceived = recvfrom(serverUdpSocket,
+				receptionBuffer.data(), frameSize, receptionFlags,
+				reinterpret_cast<sockaddr*>(&senderAddress), &senderSockLen);
+		if(bytesReceived < 0) {
+			// handle error
+			sif::error << "TcSocketPollingTask::performOperation: Reception"
+					"error." << std::endl;
+			handleReadError();
+
+			continue;
+		}
+//		sif::debug << "TcSocketPollingTask::performOperation: " << bytesReceived
+//				<< " bytes received" << std::endl;
+
+		ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
+		if(result != HasReturnvaluesIF::RETURN_FAILED) {
+
+		}
+		tmtcBridge->registerCommConnect();
+		tmtcBridge->checkAndSetClientAddress(senderAddress);
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t TcUnixUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
+	store_address_t storeId;
+	ReturnValue_t result = tcStore->addData(&storeId,
+			receptionBuffer.data(), bytesRead);
+	// arrayprinter::print(receptionBuffer.data(), bytesRead);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		sif::error << "TcSerialPollingTask::transferPusToSoftwareBus: Data "
+				"storage failed" << std::endl;
+		sif::error << "Packet size: " << bytesRead << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	TmTcMessage message(storeId);
+
+	result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		sif::error << "Serial Polling: Sending message to queue failed"
+				<< std::endl;
+		tcStore->deleteData(storeId);
+	}
+	return result;
+}
+
+ReturnValue_t TcUnixUdpPollingTask::initialize() {
+	tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
+	if (tcStore == nullptr) {
+		sif::error << "TcSerialPollingTask::initialize: TC Store uninitialized!"
+				<< std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	tmtcBridge = objectManager->get<TmTcUnixUdpBridge>(tmtcBridgeId);
+	if(tmtcBridge == nullptr) {
+		sif::error << "TcSocketPollingTask::TcSocketPollingTask: Invalid"
+				" TMTC bridge object!" << std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	serverUdpSocket = tmtcBridge->serverSocket;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t TcUnixUdpPollingTask::initializeAfterTaskCreation() {
+	// Initialize the destination after task creation. This ensures
+	// that the destination will be set in the TMTC bridge.
+	targetTcDestination = tmtcBridge->getRequestQueue();
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void TcUnixUdpPollingTask::setTimeout(double timeoutSeconds) {
+	timeval tval;
+	tval = timevalOperations::toTimeval(timeoutSeconds);
+	int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO,
+			&tval, sizeof(receptionTimeout));
+	if(result == -1) {
+		sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting "
+				"receive timeout failed with " << strerror(errno) << std::endl;
+	}
+}
+
+// TODO: sleep after error detection to prevent spam
+void TcUnixUdpPollingTask::handleReadError() {
+	switch(errno) {
+	case(EAGAIN): {
+		// todo: When working in timeout mode, this will occur more often
+		// and is not an error.
+		sif::error << "TcUnixUdpPollingTask::handleReadError: Timeout."
+				<< std::endl;
+		break;
+	}
+	default: {
+		sif::error << "TcUnixUdpPollingTask::handleReadError: "
+				<< strerror(errno) << std::endl;
+	}
+	}
+}

osal/linux/TcUnixUdpPollingTask.h

@@ -0,0 +1,67 @@
+#ifndef FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
+#define FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
+
+#include "../../objectmanager/SystemObject.h"
+#include "../../osal/linux/TmTcUnixUdpBridge.h"
+#include "../../tasks/ExecutableObjectIF.h"
+
+#include <sys/socket.h>
+#include <vector>
+
+/**
+ * @brief 	This class can be used to implement the polling of a Unix socket,
+ * 			using UDP for now.
+ * @details
+ * The task will be blocked while the specified number of bytes has not been
+ * received, so TC reception is handled inside a separate task.
+ * This class caches the IP address of the sender. It is assumed there
+ * is only one sender for now.
+ */
+class TcUnixUdpPollingTask: public SystemObject,
+		public ExecutableObjectIF {
+	friend class TmTcUnixUdpBridge;
+public:
+	static constexpr size_t DEFAULT_MAX_FRAME_SIZE = 2048;
+	//! 0.5  default milliseconds timeout for now.
+	static constexpr timeval DEFAULT_TIMEOUT = {.tv_sec = 0, .tv_usec = 500};
+
+	TcUnixUdpPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
+			size_t frameSize = 0, double timeoutSeconds = -1);
+	virtual~ TcUnixUdpPollingTask();
+
+	/**
+	 * Turn on optional timeout for UDP polling. In the default mode,
+	 * the receive function will block until a packet is received.
+	 * @param timeoutSeconds
+	 */
+	void setTimeout(double timeoutSeconds);
+
+	virtual ReturnValue_t performOperation(uint8_t opCode) override;
+	virtual ReturnValue_t initialize() override;
+	virtual ReturnValue_t initializeAfterTaskCreation() override;
+
+protected:
+	StorageManagerIF* tcStore = nullptr;
+
+private:
+	//! TMTC bridge is cached.
+	object_id_t tmtcBridgeId = objects::NO_OBJECT;
+	TmTcUnixUdpBridge* tmtcBridge = nullptr;
+	MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
+	//! Reception flags: https://linux.die.net/man/2/recvfrom.
+	int receptionFlags = 0;
+
+	//! Server socket, which is member of TMTC bridge and is assigned in
+	//! constructor
+	int serverUdpSocket = 0;
+
+	std::vector<uint8_t> receptionBuffer;
+
+	size_t frameSize = 0;
+	timeval receptionTimeout;
+
+	ReturnValue_t handleSuccessfullTcRead(size_t bytesRead);
+	void handleReadError();
+};
+
+#endif /* FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ */

osal/linux/TmTcUnixUdpBridge.cpp

@@ -0,0 +1,170 @@
+#include "TmTcUnixUdpBridge.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../ipc/MutexHelper.h"
+
+#include <errno.h>
+#include <arpa/inet.h>
+
+
+TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
+		object_id_t tcDestination, object_id_t tmStoreId, object_id_t tcStoreId,
+		uint16_t serverPort, uint16_t clientPort):
+		TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
+	mutex = MutexFactory::instance()->createMutex();
+
+	uint16_t setServerPort = DEFAULT_UDP_SERVER_PORT;
+	if(serverPort != 0xFFFF) {
+		setServerPort = serverPort;
+	}
+
+	uint16_t setClientPort = DEFAULT_UDP_CLIENT_PORT;
+	if(clientPort != 0xFFFF) {
+		setClientPort = clientPort;
+	}
+
+	// Set up UDP socket: https://man7.org/linux/man-pages/man7/ip.7.html
+	//clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
+	serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if(serverSocket < 0) {
+		sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open"
+				" UDP socket!" << std::endl;
+		handleSocketError();
+		return;
+	}
+
+	serverAddress.sin_family = AF_INET;
+
+	// Accept packets from any interface.
+	//serverAddress.sin_addr.s_addr = inet_addr("127.73.73.0");
+	serverAddress.sin_addr.s_addr = htonl(INADDR_ANY);
+	serverAddress.sin_port = htons(setServerPort);
+	serverAddressLen = sizeof(serverAddress);
+	setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, &serverSocketOptions,
+			sizeof(serverSocketOptions));
+
+	clientAddress.sin_family = AF_INET;
+	clientAddress.sin_addr.s_addr = htonl(INADDR_ANY);
+	clientAddress.sin_port = htons(setClientPort);
+	clientAddressLen = sizeof(clientAddress);
+
+	int result = bind(serverSocket,
+			reinterpret_cast<struct sockaddr*>(&serverAddress),
+			serverAddressLen);
+	if(result == -1) {
+		sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not bind "
+				"local port " << setServerPort << " to server socket!"
+				<< std::endl;
+		handleBindError();
+		return;
+	}
+}
+
+TmTcUnixUdpBridge::~TmTcUnixUdpBridge() {
+}
+
+ReturnValue_t TmTcUnixUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
+	int flags = 0;
+
+	clientAddress.sin_addr.s_addr = htons(INADDR_ANY);
+	//clientAddress.sin_addr.s_addr = inet_addr("127.73.73.1");
+	clientAddressLen = sizeof(serverAddress);
+
+//	char ipAddress [15];
+//	sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
+//					&clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+
+	ssize_t bytesSent = sendto(serverSocket, data, dataLen, flags,
+			reinterpret_cast<sockaddr*>(&clientAddress), clientAddressLen);
+	if(bytesSent < 0) {
+		sif::error << "TmTcUnixUdpBridge::sendTm: Send operation failed."
+				<< std::endl;
+		handleSendError();
+	}
+//	sif::debug << "TmTcUnixUdpBridge::sendTm: " << bytesSent << " bytes were"
+//			" sent." << std::endl;
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void TmTcUnixUdpBridge::checkAndSetClientAddress(sockaddr_in newAddress) {
+	MutexHelper lock(mutex, MutexIF::TimeoutType::WAITING, 10);
+
+//	char ipAddress [15];
+//	sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
+//			&newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+//	sif::debug << "IP Address Old: " <<  inet_ntop(AF_INET,
+//			&clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+
+	// Set new IP address if it has changed.
+	if(clientAddress.sin_addr.s_addr != newAddress.sin_addr.s_addr) {
+		clientAddress.sin_addr.s_addr = newAddress.sin_addr.s_addr;
+		clientAddressLen = sizeof(clientAddress);
+	}
+}
+
+
+void TmTcUnixUdpBridge::handleSocketError() {
+	// See: https://man7.org/linux/man-pages/man2/socket.2.html
+	switch(errno) {
+	case(EACCES):
+	case(EINVAL):
+	case(EMFILE):
+	case(ENFILE):
+	case(EAFNOSUPPORT):
+	case(ENOBUFS):
+	case(ENOMEM):
+	case(EPROTONOSUPPORT):
+		sif::error << "TmTcUnixBridge::handleSocketError: Socket creation failed"
+				<< " with " << strerror(errno) << std::endl;
+		break;
+	default:
+		sif::error << "TmTcUnixBridge::handleSocketError: Unknown error"
+				<< std::endl;
+		break;
+	}
+}
+
+void TmTcUnixUdpBridge::handleBindError() {
+	// See: https://man7.org/linux/man-pages/man2/bind.2.html
+	switch(errno) {
+	case(EACCES): {
+		/*
+		 Ephermeral ports can be shown with following command:
+		 sysctl -A | grep ip_local_port_range
+		 */
+		sif::error << "TmTcUnixBridge::handleBindError: Port access issue."
+				"Ports 1-1024 are reserved on UNIX systems and require root "
+				"rights while ephermeral ports should not be used as well."
+				<< std::endl;
+	}
+	break;
+	case(EADDRINUSE):
+	case(EBADF):
+	case(EINVAL):
+	case(ENOTSOCK):
+	case(EADDRNOTAVAIL):
+	case(EFAULT):
+	case(ELOOP):
+	case(ENAMETOOLONG):
+	case(ENOENT):
+	case(ENOMEM):
+	case(ENOTDIR):
+	case(EROFS): {
+		sif::error << "TmTcUnixBridge::handleBindError: Socket creation failed"
+				<< " with " << strerror(errno) << std::endl;
+		break;
+	}
+	default:
+		sif::error << "TmTcUnixBridge::handleBindError: Unknown error"
+				<< std::endl;
+		break;
+	}
+}
+
+void TmTcUnixUdpBridge::handleSendError() {
+	switch(errno) {
+	default:
+		sif::error << "TmTcUnixBridge::handleSendError: "
+		        << strerror(errno) << std::endl;
+	}
+}
+

osal/linux/TmTcUnixUdpBridge.h

@@ -0,0 +1,48 @@
+#ifndef FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
+#define FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
+
+#include "../../tmtcservices/AcceptsTelecommandsIF.h"
+#include "../../tmtcservices/TmTcBridge.h"
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/udp.h>
+
+class TmTcUnixUdpBridge: public TmTcBridge {
+	friend class TcUnixUdpPollingTask;
+public:
+	// The ports chosen here should not be used by any other process.
+	// List of used ports on Linux: /etc/services
+	static constexpr uint16_t DEFAULT_UDP_SERVER_PORT = 7301;
+	static constexpr uint16_t DEFAULT_UDP_CLIENT_PORT = 7302;
+
+	TmTcUnixUdpBridge(object_id_t objectId, object_id_t tcDestination,
+			object_id_t tmStoreId, object_id_t tcStoreId,
+			uint16_t serverPort = 0xFFFF,uint16_t clientPort = 0xFFFF);
+	virtual~ TmTcUnixUdpBridge();
+
+	void checkAndSetClientAddress(sockaddr_in clientAddress);
+
+protected:
+	virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
+
+private:
+	int serverSocket = 0;
+
+	const int serverSocketOptions = 0;
+
+	struct sockaddr_in clientAddress;
+	socklen_t clientAddressLen = 0;
+
+	struct sockaddr_in serverAddress;
+	socklen_t serverAddressLen = 0;
+
+	//! Access to the client address is mutex protected as it is set
+	//! by another task.
+	MutexIF* mutex;
+
+	void handleSocketError();
+	void handleBindError();
+	void handleSendError();
+};
+
+#endif /* FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_ */

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;
 };