Merge remote-tracking branch 'upstream/master' into mueller/dledoc

container/ArrayList.h

@@ -72,7 +72,11 @@ public:
 			return tmp;
 		}
 
-		T operator*() {
+		T& operator*(){
+			return *value;
+		}
+
+		const T& operator*() const{
 			return *value;
 		}
 

container/DynamicFIFO.h

@@ -27,14 +27,27 @@ public:
 
 	/**
 	 * @brief	Custom copy constructor which prevents setting the
-	 * 	        underlying pointer wrong.
+	 * 	        underlying pointer wrong. This function allocates memory!
+	 * @details This is a very heavy operation so try to avoid this!
+	 *
 	 */
 	DynamicFIFO(const DynamicFIFO& other): FIFOBase<T>(other),
 			fifoVector(other.maxCapacity) {
+		this->fifoVector = other.fifoVector;
 		this->setContainer(fifoVector.data());
 	}
 
-
+	/**
+	 * @brief Custom assignment operator
+	 * @details This is a very heavy operation so try to avoid this!
+	 * @param other DyamicFIFO to copy from
+	 */
+	DynamicFIFO& operator=(const DynamicFIFO& other){
+		FIFOBase<T>::operator=(other);
+		this->fifoVector = other.fifoVector;
+		this->setContainer(fifoVector.data());
+		return *this;
+	}
 private:
 	std::vector<T> fifoVector;
 };

container/FIFO.h

@@ -25,9 +25,21 @@ public:
 	 * @param other
 	 */
 	FIFO(const FIFO& other): FIFOBase<T>(other) {
+		this->fifoArray = other.fifoArray;
 		this->setContainer(fifoArray.data());
 	}
 
+	/**
+	 * @brief Custom assignment operator
+	 * @param other
+	 */
+	FIFO& operator=(const FIFO& other){
+		FIFOBase<T>::operator=(other);
+		this->fifoArray = other.fifoArray;
+		this->setContainer(fifoArray.data());
+		return *this;
+	}
+
 private:
 	std::array<T, capacity> fifoArray;
 };

container/FixedMap.h

@@ -1,15 +1,20 @@
-#ifndef FIXEDMAP_H_
+#ifndef FSFW_CONTAINER_FIXEDMAP_H_
-#define FIXEDMAP_H_
+#define FSFW_CONTAINER_FIXEDMAP_H_
 
 #include "ArrayList.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include <utility>
+#include <type_traits>
 
 /**
- * \ingroup container
+ * @warning Iterators return a non-const key_t in the pair.
+ * @warning A User is not allowed to change the key, otherwise the map is corrupted.
+ * @ingroup container
  */
 template<typename key_t, typename T>
 class FixedMap: public SerializeIF {
+	static_assert (std::is_trivially_copyable<T>::value or std::is_base_of<SerializeIF, T>::value,
+			"Types used in FixedMap must either be trivial copy-able or a derived Class from SerializeIF to be serialize-able");
 public:
 	static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
 	static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
@@ -47,15 +52,6 @@ public:
 		Iterator(std::pair<key_t, T> *pair) :
 				ArrayList<std::pair<key_t, T>, uint32_t>::Iterator(pair) {
 		}
-
-		T operator*() {
-			return ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
-		}
-
-		T *operator->() {
-			return &ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
-		}
-
 	};
 
 	Iterator begin() const {
@@ -70,7 +66,7 @@ public:
 		return _size;
 	}
 
-	ReturnValue_t insert(key_t key, T value, Iterator *storedValue = NULL) {
+	ReturnValue_t insert(key_t key, T value, Iterator *storedValue = nullptr) {
 		if (exists(key) == HasReturnvaluesIF::RETURN_OK) {
 			return KEY_ALREADY_EXISTS;
 		}
@@ -79,7 +75,7 @@ public:
 		}
 		theMap[_size].first = key;
 		theMap[_size].second = value;
-		if (storedValue != NULL) {
+		if (storedValue != nullptr) {
 			*storedValue = Iterator(&theMap[_size]);
 		}
 		++_size;
@@ -87,7 +83,7 @@ public:
 	}
 
 	ReturnValue_t insert(std::pair<key_t, T> pair) {
-		return insert(pair.fist, pair.second);
+		return insert(pair.first, pair.second);
 	}
 
 	ReturnValue_t exists(key_t key) const {
@@ -196,4 +192,4 @@ public:
 
 };
 
-#endif /* FIXEDMAP_H_ */
+#endif /* FSFW_CONTAINER_FIXEDMAP_H_ */

container/FixedOrderedMultimap.h

@@ -48,7 +48,7 @@ private:
 		if (_size <= position) {
 			return;
 		}
-		memmove(&theMap[position], &theMap[position + 1],
+		memmove(static_cast<void*>(&theMap[position]), static_cast<void*>(&theMap[position + 1]),
 				(_size - position - 1) * sizeof(std::pair<key_t,T>));
 		--_size;
 	}
@@ -68,15 +68,6 @@ public:
 		Iterator(std::pair<key_t, T> *pair) :
 				ArrayList<std::pair<key_t, T>, uint32_t>::Iterator(pair) {
 		}
-
-		T operator*() {
-			return ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
-		}
-
-		T *operator->() {
-			return &ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
-		}
-
 	};
 
 	Iterator begin() const {
@@ -91,17 +82,17 @@ public:
 		return _size;
 	}
 
-	ReturnValue_t insert(key_t key, T value, Iterator *storedValue = NULL) {
+	ReturnValue_t insert(key_t key, T value, Iterator *storedValue = nullptr) {
 		if (_size == theMap.maxSize()) {
 			return MAP_FULL;
 		}
 		uint32_t position = findNicePlace(key);
-		memmove(&theMap[position + 1], &theMap[position],
+		memmove(static_cast<void*>(&theMap[position + 1]),static_cast<void*>(&theMap[position]),
 				(_size - position) * sizeof(std::pair<key_t,T>));
 		theMap[position].first = key;
 		theMap[position].second = value;
 		++_size;
-		if (storedValue != NULL) {
+		if (storedValue != nullptr) {
 			*storedValue = Iterator(&theMap[position]);
 		}
 		return HasReturnvaluesIF::RETURN_OK;
@@ -145,12 +136,6 @@ public:
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 
-	//This is potentially unsafe
-//	T *findValue(key_t key) const {
-//		return &theMap[findFirstIndex(key)].second;
-//	}
-
-
 	Iterator find(key_t key) const {
 		ReturnValue_t result = exists(key);
 		if (result != HasReturnvaluesIF::RETURN_OK) {

container/IsDerivedFrom.h

@@ -1,41 +0,0 @@
-#ifndef ISDERIVEDFROM_H_
-#define ISDERIVEDFROM_H_
-
-template<typename D, typename B>
-class IsDerivedFrom {
-	class No {
-	};
-	class Yes {
-		No no[3];
-	};
-
-	static Yes Test(B*); // declared, but not defined
-	static No Test(... ); // declared, but not defined
-
-public:
-	enum {
-		Is = sizeof(Test(static_cast<D*>(0))) == sizeof(Yes)
-	};
-};
-
-template<typename, typename>
-struct is_same {
-    static bool const value = false;
-};
-
-template<typename A>
-struct is_same<A, A> {
-    static bool const value = true;
-};
-
-
-template<bool C, typename T = void>
-struct enable_if {
-  typedef T type;
-};
-
-template<typename T>
-struct enable_if<false, T> { };
-
-
-#endif /* ISDERIVEDFROM_H_ */

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_ */

serialize/SerializeAdapter.h

@@ -1,11 +1,11 @@
-#ifndef SERIALIZEADAPTER_H_
+#ifndef _FSFW_SERIALIZE_SERIALIZEADAPTER_H_
-#define SERIALIZEADAPTER_H_
+#define _FSFW_SERIALIZE_SERIALIZEADAPTER_H_
 
-#include "../container/IsDerivedFrom.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "EndianConverter.h"
 #include "SerializeIF.h"
-#include <string.h>
+#include <cstddef>
+#include <type_traits>
 
 /**
  * \ingroup serialize
@@ -13,36 +13,91 @@
 
 class SerializeAdapter {
 public:
+	/***
+	 * This function can be used to serialize a trivial copy-able type or a child of SerializeIF.
+	 * The right template to be called is determined in the function itself.
+	 * For objects of non trivial copy-able type this function is almost never called by the user directly.
+	 * Instead helpers for specific types like SerialArrayListAdapter or SerialLinkedListAdapter is the right choice here.
+	 *
+	 * @param[in] object Object to serialize, the used type is deduced from this pointer
+	 * @param[in/out] buffer Buffer to serialize into. Will be moved by the function.
+	 * @param[in/out] size Size of current written buffer. Will be incremented by the function.
+	 * @param[in] maxSize Max size of Buffer
+	 * @param[in] streamEndianness Endianness of serialized element as in according to SerializeIF::Endianness
+	 * @return
+	 * 		- @c BUFFER_TOO_SHORT The given buffer in is too short
+	 * 		- @c RETURN_FAILED Generic Error
+	 * 		- @c RETURN_OK Successful serialization
+	 */
 	template<typename T>
 	static ReturnValue_t serialize(const T *object, uint8_t **buffer,
-			size_t *size, size_t maxSize, SerializeIF::Endianness streamEndianness) {
+			size_t *size, size_t maxSize,
-		InternalSerializeAdapter<T, IsDerivedFrom<T, SerializeIF>::Is> adapter;
+			SerializeIF::Endianness streamEndianness) {
+		InternalSerializeAdapter<T, std::is_base_of<SerializeIF, T>::value> adapter;
 		return adapter.serialize(object, buffer, size, maxSize,
 				streamEndianness);
 	}
+	/**
+	 * Function to return the serialized size of the object in the pointer.
+	 * May be a trivially copy-able object or a Child of SerializeIF
+	 *
+	 * @param object Pointer to Object
+	 * @return Serialized size of object
+	 */
 	template<typename T>
-	static uint32_t getSerializedSize(const T *object) {
+	static size_t getSerializedSize(const T *object){
-		InternalSerializeAdapter<T, IsDerivedFrom<T, SerializeIF>::Is> adapter;
+		InternalSerializeAdapter<T, std::is_base_of<SerializeIF, T>::value> adapter;
 		return adapter.getSerializedSize(object);
 	}
+	/**
+	 * @brief
+	 * Deserializes a object from a given buffer of given size.
+	 * Object Must be trivially copy-able or a child of SerializeIF.
+	 *
+	 * @details
+	 * Buffer will be moved to the current read location. Size will be decreased by the function.
+	 *
+	 * @param[in/out] buffer Buffer to deSerialize from. Will be moved by the function.
+	 * @param[in/out] size Remaining size of the buffer to read from. Will be decreased by function.
+	 * @param[in] streamEndianness Endianness as in according to SerializeIF::Endianness
+	 * @return
+	 * 	- @c STREAM_TOO_SHORT The input stream is too short to deSerialize the object
+	 * 	- @c TOO_MANY_ELEMENTS The buffer has more inputs than expected
+	 * 	- @c RETURN_FAILED Generic Error
+	 * 	- @c RETURN_OK Successful deserialization
+	 */
 	template<typename T>
 	static ReturnValue_t deSerialize(T *object, const uint8_t **buffer,
 			size_t *size, SerializeIF::Endianness streamEndianness) {
-		InternalSerializeAdapter<T, IsDerivedFrom<T, SerializeIF>::Is> adapter;
+		InternalSerializeAdapter<T, std::is_base_of<SerializeIF, T>::value> adapter;
 		return adapter.deSerialize(object, buffer, size, streamEndianness);
 	}
 private:
-	template<typename T, int>
+	/**
-	class InternalSerializeAdapter {
+	 * Internal template to deduce the right function calls at compile time
+	 */
+	template<typename T, bool> class InternalSerializeAdapter;
+
+	/**
+	 * Template to be used if T is not a child of SerializeIF
+	 *
+	 * @tparam T T must be trivially_copyable
+	 */
+	template<typename T>
+	class InternalSerializeAdapter<T, false> {
+		static_assert (std::is_trivially_copyable<T>::value,
+				"If a type needs to be serialized it must be a child of SerializeIF or trivially copy-able");
 	public:
 		static ReturnValue_t serialize(const T *object, uint8_t **buffer,
-				size_t *size, size_t max_size, SerializeIF::Endianness streamEndianness) {
+				size_t *size, size_t max_size,
+				SerializeIF::Endianness streamEndianness) {
 			size_t ignoredSize = 0;
-			if (size == NULL) {
+			if (size == nullptr) {
 				size = &ignoredSize;
 			}
-			//TODO check integer overflow of *size
+			//Check remaining size is large enough and check integer overflow of *size
-			if (sizeof(T) + *size <= max_size) {
+			size_t newSize = sizeof(T) + *size;
+			if ((newSize <= max_size) and (newSize > *size)) {
 				T tmp;
 				switch (streamEndianness) {
 				case SerializeIF::Endianness::BIG:
@@ -94,22 +149,26 @@ private:
 		uint32_t getSerializedSize(const T *object) {
 			return sizeof(T);
 		}
-
 	};
 
+	/**
+	 * Template for objects that inherit from SerializeIF
+	 *
+	 * @tparam T A child of SerializeIF
+	 */
 	template<typename T>
-	class InternalSerializeAdapter<T, 1> {
+	class InternalSerializeAdapter<T, true> {
 	public:
-		ReturnValue_t serialize(const T *object, uint8_t **buffer,
+		ReturnValue_t serialize(const T *object, uint8_t **buffer, size_t *size,
-				size_t *size, size_t max_size,
+				size_t max_size,
 				SerializeIF::Endianness streamEndianness) const {
 			size_t ignoredSize = 0;
-			if (size == NULL) {
+			if (size == nullptr) {
 				size = &ignoredSize;
 			}
 			return object->serialize(buffer, size, max_size, streamEndianness);
 		}
-		uint32_t getSerializedSize(const T *object) const {
+		size_t getSerializedSize(const T *object) const {
 			return object->getSerializedSize();
 		}
 
@@ -120,4 +179,4 @@ private:
 	};
 };
 
-#endif /* SERIALIZEADAPTER_H_ */
+#endif /* _FSFW_SERIALIZE_SERIALIZEADAPTER_H_ */

serialize/SerializeIF.h

@@ -43,7 +43,7 @@ public:
 	 * @param[in] maxSize The size of the buffer that is allowed to be used for serialize.
 	 * @param[in] streamEndianness Endianness of the serialized data according to SerializeIF::Endianness
 	 * @return
-	 * 		- @¢ BUFFER_TOO_SHORT The given buffer in is too short
+	 * 		- @c BUFFER_TOO_SHORT The given buffer in is too short
 	 * 		- @c RETURN_FAILED Generic error
 	 * 		- @c RETURN_OK Successful serialization
 	 */

subsystem/Subsystem.cpp

@@ -550,7 +550,7 @@ Mode_t Subsystem::getFallbackSequence(Mode_t sequence) {
 	for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
 			iter != modeSequences.end(); ++iter) {
 		if (iter.value->first == sequence) {
-			return iter->fallbackSequence;
+			return iter->second.fallbackSequence;
 		}
 	}
 	return -1;
@@ -559,7 +559,7 @@ Mode_t Subsystem::getFallbackSequence(Mode_t sequence) {
 bool Subsystem::isFallbackSequence(Mode_t SequenceId) {
 	for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
 			iter != modeSequences.end(); iter++) {
-		if (iter->fallbackSequence == SequenceId) {
+		if (iter->second.fallbackSequence == SequenceId) {
 			return true;
 		}
 	}

tmtcservices/CommandingServiceBase.cpp

@@ -122,8 +122,8 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
 
 
 	// Implemented by child class, specifies what to do with reply.
-	ReturnValue_t result = handleReply(reply, iter->command, &iter->state,
+	ReturnValue_t result = handleReply(reply, iter->second.command, &iter->second.state,
-			&nextCommand, iter->objectId, &isStep);
+			&nextCommand, iter->second.objectId, &isStep);
 
 	/* If the child implementation does not implement special handling for
 	 * rejected replies (RETURN_FAILED or INVALID_REPLY is returned), a
@@ -132,7 +132,7 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
 	if((reply->getCommand() == CommandMessage::REPLY_REJECTED) and
 			(result == RETURN_FAILED or result == INVALID_REPLY)) {
 	    result = reply->getReplyRejectedReason();
-	    failureParameter1 = iter->command;
+	    failureParameter1 = iter->second.command;
 	}
 
 	switch (result) {
@@ -149,14 +149,14 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
 	default:
 		if (isStep) {
 			verificationReporter.sendFailureReport(
-					TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
+					TC_VERIFY::PROGRESS_FAILURE, iter->second.tcInfo.ackFlags,
-					iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
+					iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
-					result, ++iter->step, failureParameter1,
+					result, ++iter->second.step, failureParameter1,
 					failureParameter2);
 		} else {
 			verificationReporter.sendFailureReport(
-					TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
+					TC_VERIFY::COMPLETION_FAILURE, iter->second.tcInfo.ackFlags,
-					iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
+					iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
 					result, 0, failureParameter1, failureParameter2);
 		}
 		failureParameter1 = 0;
@@ -170,7 +170,7 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
 void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result,
 		CommandMapIter iter, CommandMessage* nextCommand,
 		CommandMessage* reply, bool& isStep) {
-	iter->command = nextCommand->getCommand();
+	iter->second.command = nextCommand->getCommand();
 
 	// In case a new command is to be sent immediately, this is performed here.
 	// If no new command is sent, only analyse reply result by initializing
@@ -185,14 +185,14 @@ void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result,
 		if (isStep and result != NO_STEP_MESSAGE) {
 			verificationReporter.sendSuccessReport(
 					TC_VERIFY::PROGRESS_SUCCESS,
-					iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
+					iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
-					iter->tcInfo.tcSequenceControl, ++iter->step);
+					iter->second.tcInfo.tcSequenceControl, ++iter->second.step);
 		}
 		else {
 			verificationReporter.sendSuccessReport(
 					TC_VERIFY::COMPLETION_SUCCESS,
-					iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
+					iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
-					iter->tcInfo.tcSequenceControl, 0);
+					iter->second.tcInfo.tcSequenceControl, 0);
 			checkAndExecuteFifo(iter);
 		}
 	}
@@ -200,16 +200,16 @@ void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result,
 		if (isStep) {
 			nextCommand->clearCommandMessage();
 			verificationReporter.sendFailureReport(
-					TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
+					TC_VERIFY::PROGRESS_FAILURE, iter->second.tcInfo.ackFlags,
-					iter->tcInfo.tcPacketId,
+					iter->second.tcInfo.tcPacketId,
-					iter->tcInfo.tcSequenceControl, sendResult,
+					iter->second.tcInfo.tcSequenceControl, sendResult,
-					++iter->step, failureParameter1, failureParameter2);
+					++iter->second.step, failureParameter1, failureParameter2);
 		} else {
 			nextCommand->clearCommandMessage();
 			verificationReporter.sendFailureReport(
 					TC_VERIFY::COMPLETION_FAILURE,
-					iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
+					iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
-					iter->tcInfo.tcSequenceControl, sendResult, 0,
+					iter->second.tcInfo.tcSequenceControl, sendResult, 0,
 					failureParameter1, failureParameter2);
 		}
 		failureParameter1 = 0;
@@ -248,7 +248,7 @@ void CommandingServiceBase::handleRequestQueue() {
 		iter = commandMap.find(queue);
 
 		if (iter != commandMap.end()) {
-			result = iter->fifo.insert(address);
+			result = iter->second.fifo.insert(address);
 			if (result != RETURN_OK) {
 				rejectPacket(TC_VERIFY::START_FAILURE, &packet, OBJECT_BUSY);
 			}
@@ -316,11 +316,11 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
         CommandMapIter iter) {
     ReturnValue_t result = RETURN_OK;
     CommandMessage command;
-    iter->subservice = storedPacket->getSubService();
+    iter->second.subservice = storedPacket->getSubService();
-    result = prepareCommand(&command, iter->subservice,
+    result = prepareCommand(&command, iter->second.subservice,
             storedPacket->getApplicationData(),
-            storedPacket->getApplicationDataSize(), &iter->state,
+            storedPacket->getApplicationDataSize(), &iter->second.state,
-            iter->objectId);
+            iter->second.objectId);
 
     ReturnValue_t sendResult = RETURN_OK;
 	switch (result) {
@@ -330,13 +330,13 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
 					&command);
 		}
 		if (sendResult == RETURN_OK) {
-			Clock::getUptime(&iter->uptimeOfStart);
+			Clock::getUptime(&iter->second.uptimeOfStart);
-			iter->step = 0;
+			iter->second.step = 0;
-			iter->subservice = storedPacket->getSubService();
+			iter->second.subservice = storedPacket->getSubService();
-			iter->command = command.getCommand();
+			iter->second.command = command.getCommand();
-			iter->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
+			iter->second.tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
-			iter->tcInfo.tcPacketId = storedPacket->getPacketId();
+			iter->second.tcInfo.tcPacketId = storedPacket->getPacketId();
-			iter->tcInfo.tcSequenceControl =
+			iter->second.tcInfo.tcSequenceControl =
 					storedPacket->getPacketSequenceControl();
 			acceptPacket(TC_VERIFY::START_SUCCESS, storedPacket);
 		} else {
@@ -386,7 +386,7 @@ void CommandingServiceBase::acceptPacket(uint8_t reportId,
 
 void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter iter) {
 	store_address_t address;
-	if (iter->fifo.retrieve(&address) != RETURN_OK) {
+	if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
 		commandMap.erase(&iter);
 	} else {
 		TcPacketStored newPacket(address);
@@ -412,10 +412,10 @@ void CommandingServiceBase::checkTimeout() {
 	Clock::getUptime(&uptime);
 	CommandMapIter iter;
 	for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) {
-		if ((iter->uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
+		if ((iter->second.uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
 			verificationReporter.sendFailureReport(
-					TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
+					TC_VERIFY::COMPLETION_FAILURE, iter->second.tcInfo.ackFlags,
-					iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
+					iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
 					TIMEOUT);
 			checkAndExecuteFifo(iter);
 		}

tmtcservices/CommandingServiceBase.h

@@ -211,8 +211,7 @@ protected:
 
 	virtual void doPeriodicOperation();
 
-
+	struct CommandInfo: public SerializeIF{
-	struct CommandInfo {
 		struct tcInfo {
 			uint8_t ackFlags;
 			uint16_t tcPacketId;
@@ -225,6 +224,20 @@ protected:
 		Command_t command;
 		object_id_t objectId;
 		FIFO<store_address_t, 3> fifo;
+
+		virtual ReturnValue_t serialize(uint8_t **buffer, size_t *size,
+					size_t maxSize, Endianness streamEndianness) const override{
+			return HasReturnvaluesIF::RETURN_FAILED;
+		};
+
+		virtual size_t getSerializedSize() const override {
+			return 0;
+		};
+
+		virtual ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size,
+				Endianness streamEndianness) override{
+			return HasReturnvaluesIF::RETURN_FAILED;
+		};
 	};
 
 	using CommandMapIter = FixedMap<MessageQueueId_t,

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
+	        (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++;
 		}
-	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);
 	    }
+	    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;
 };