From eead2a8a49733cd7f0c79d273432d7d978506c24 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Mon, 14 Jun 2021 14:40:40 +0200
Subject: [PATCH 01/14] Moved leap second management

---
 osal/FreeRTOS/Clock.cpp | 68 -------------------------------
 osal/host/Clock.cpp     | 68 -------------------------------
 osal/linux/Clock.cpp    | 68 -------------------------------
 osal/rtems/Clock.cpp    | 62 ----------------------------
 timemanager/Clock.h     | 90 ++++++++++++++++++++++++++++++++---------
 5 files changed, 71 insertions(+), 285 deletions(-)

diff --git a/osal/FreeRTOS/Clock.cpp b/osal/FreeRTOS/Clock.cpp
index c15971fee..66207d75e 100644
--- a/osal/FreeRTOS/Clock.cpp
+++ b/osal/FreeRTOS/Clock.cpp
@@ -134,71 +134,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
             / 3600.;
     return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-    //SHOULDDO: works not for dates in the past (might have less leap seconds)
-    if (timeMutex == nullptr) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
-    uint16_t leapSeconds;
-    ReturnValue_t result = getLeapSeconds(&leapSeconds);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-    timeval leapSeconds_timeval = { 0, 0 };
-    leapSeconds_timeval.tv_sec = leapSeconds;
-
-    //initial offset between UTC and TAI
-    timeval UTCtoTAI1972 = { 10, 0 };
-
-    timeval TAItoTT = { 32, 184000 };
-
-    *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-    if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-
-    leapSeconds = leapSeconds_;
-
-    result = timeMutex->unlockMutex();
-    return result;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-    if (timeMutex == NULL) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-
-    *leapSeconds_ = leapSeconds;
-
-    result = timeMutex->unlockMutex();
-    return result;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex() {
-    if (timeMutex == NULL) {
-        MutexFactory* mutexFactory = MutexFactory::instance();
-        if (mutexFactory == NULL) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-        timeMutex = mutexFactory->createMutex();
-        if (timeMutex == NULL) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-    }
-    return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index c097f6199..219d3e969 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -170,71 +170,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
 			/ 3600.;
 	return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-	//SHOULDDO: works not for dates in the past (might have less leap seconds)
-	if (timeMutex == NULL) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	uint16_t leapSeconds;
-	ReturnValue_t result = getLeapSeconds(&leapSeconds);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-	timeval leapSeconds_timeval = { 0, 0 };
-	leapSeconds_timeval.tv_sec = leapSeconds;
-
-	//initial offset between UTC and TAI
-	timeval UTCtoTAI1972 = { 10, 0 };
-
-	timeval TAItoTT = { 32, 184000 };
-
-	*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-	if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex();
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	leapSeconds = leapSeconds_;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-	if(timeMutex == nullptr){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex();
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	*leapSeconds_ = leapSeconds;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex(){
-	if(timeMutex == nullptr){
-		MutexFactory* mutexFactory = MutexFactory::instance();
-		if (mutexFactory == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		timeMutex = mutexFactory->createMutex();
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/osal/linux/Clock.cpp b/osal/linux/Clock.cpp
index 35cbfae03..960d9c0d7 100644
--- a/osal/linux/Clock.cpp
+++ b/osal/linux/Clock.cpp
@@ -153,71 +153,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
 			/ 3600.;
 	return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-	//SHOULDDO: works not for dates in the past (might have less leap seconds)
-	if (timeMutex == NULL) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	uint16_t leapSeconds;
-	ReturnValue_t result = getLeapSeconds(&leapSeconds);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-	timeval leapSeconds_timeval = { 0, 0 };
-	leapSeconds_timeval.tv_sec = leapSeconds;
-
-	//initial offset between UTC and TAI
-	timeval UTCtoTAI1972 = { 10, 0 };
-
-	timeval TAItoTT = { 32, 184000 };
-
-	*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-	if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	leapSeconds = leapSeconds_;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-	if(timeMutex==NULL){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	*leapSeconds_ = leapSeconds;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex(){
-	if(timeMutex == nullptr){
-		MutexFactory* mutexFactory = MutexFactory::instance();
-		if (mutexFactory == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		timeMutex = mutexFactory->createMutex();
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/osal/rtems/Clock.cpp b/osal/rtems/Clock.cpp
index b80786f74..ae720c361 100644
--- a/osal/rtems/Clock.cpp
+++ b/osal/rtems/Clock.cpp
@@ -154,65 +154,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
             / 3600.;
     return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-    //SHOULDDO: works not for dates in the past (might have less leap seconds)
-    if (timeMutex == nullptr) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
-    uint16_t leapSeconds;
-    ReturnValue_t result = getLeapSeconds(&leapSeconds);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-    timeval leapSeconds_timeval = { 0, 0 };
-    leapSeconds_timeval.tv_sec = leapSeconds;
-
-    //initial offset between UTC and TAI
-    timeval UTCtoTAI1972 = { 10, 0 };
-
-    timeval TAItoTT = { 32, 184000 };
-
-    *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-    if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    MutexGuard helper(timeMutex);
-
-
-    leapSeconds = leapSeconds_;
-
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-    if(timeMutex==nullptr){
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    MutexGuard helper(timeMutex);
-
-    *leapSeconds_ = leapSeconds;
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex(){
-    if(timeMutex==nullptr){
-        MutexFactory* mutexFactory = MutexFactory::instance();
-        if (mutexFactory == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-        timeMutex = mutexFactory->createMutex();
-        if (timeMutex == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-    }
-    return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/timemanager/Clock.h b/timemanager/Clock.h
index 6400d284c..2ac483847 100644
--- a/timemanager/Clock.h
+++ b/timemanager/Clock.h
@@ -41,14 +41,14 @@ public:
 	 * @return	-@c RETURN_OK on success. Otherwise, the OS failure code
 	 * 				is returned.
 	 */
-	static ReturnValue_t setClock(const TimeOfDay_t* time);
+	static ReturnValue_t setClock(const TimeOfDay_t *time);
 	/**
 	 * This system call sets the system time.
 	 * To set the time, it uses a timeval struct.
 	 * @param time The struct with the time settings to set.
 	 * @return	-@c RETURN_OK on success. Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t setClock(const timeval* time);
+	static ReturnValue_t setClock(const timeval *time);
 	/**
 	 * This system call returns the current system clock in timeval format.
 	 * The timval format has the fields @c tv_sec with seconds and @c tv_usec with
@@ -56,7 +56,7 @@ public:
 	 * @param time	A pointer to a timeval struct where the current time is stored.
 	 * @return @c RETURN_OK on success. Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getClock_timeval(timeval* time);
+	static ReturnValue_t getClock_timeval(timeval *time);
 
 	/**
 	 * Get the time since boot in a timeval struct
@@ -66,7 +66,7 @@ public:
 	 *
 	 * @deprecated, I do not think this should be able to fail, use timeval getUptime()
 	 */
-	static ReturnValue_t getUptime(timeval* uptime);
+	static ReturnValue_t getUptime(timeval *uptime);
 
 	static timeval getUptime();
 
@@ -79,7 +79,7 @@ public:
 	 * @param ms uptime in ms
 	 * @return RETURN_OK on success. Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getUptime(uint32_t* uptimeMs);
+	static ReturnValue_t getUptime(uint32_t *uptimeMs);
 
 	/**
 	 * Returns the time in microseconds since an OS-defined epoch.
@@ -89,7 +89,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getClock_usecs(uint64_t* time);
+	static ReturnValue_t getClock_usecs(uint64_t *time);
 	/**
 	 * Returns the time in a TimeOfDay_t struct.
 	 * @param time A pointer to a TimeOfDay_t struct.
@@ -97,7 +97,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getDateAndTime(TimeOfDay_t* time);
+	static ReturnValue_t getDateAndTime(TimeOfDay_t *time);
 
 	/**
 	 * Converts a time of day struct to POSIX seconds.
@@ -107,8 +107,8 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t convertTimeOfDayToTimeval(const TimeOfDay_t* from,
-			timeval* to);
+	static ReturnValue_t convertTimeOfDayToTimeval(const TimeOfDay_t *from,
+			timeval *to);
 
 	/**
 	 * Converts a time represented as seconds and subseconds since unix
@@ -118,12 +118,14 @@ public:
 	 * @param[out] JD2000 days since J2000
 	 * @return @c RETURN_OK
 	 */
-	static ReturnValue_t convertTimevalToJD2000(timeval time, double* JD2000);
+	static ReturnValue_t convertTimevalToJD2000(timeval time, double *JD2000);
 
 	/**
 	 * Calculates and adds the offset between UTC and TT
 	 *
 	 * Depends on the leap seconds to be set correctly.
+	 * Therefore, it does not work for historic
+	 * dates as only the current leap seconds are known.
 	 *
 	 * @param utc timeval, corresponding to UTC time
 	 * @param[out] tt timeval, corresponding to Terrestial Time
@@ -131,7 +133,28 @@ public:
 	 *  - @c RETURN_OK on success
 	 *  - @c RETURN_FAILED if leapSeconds are not set
 	 */
-	static ReturnValue_t convertUTCToTT(timeval utc, timeval* tt);
+	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
+		if (timeMutex == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+
+		uint16_t leapSeconds;
+		ReturnValue_t result = getLeapSeconds(&leapSeconds);
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+		timeval leapSeconds_timeval = { 0, 0 };
+		leapSeconds_timeval.tv_sec = leapSeconds;
+
+		//initial offset between UTC and TAI
+		timeval UTCtoTAI1972 = { 10, 0 };
+
+		timeval TAItoTT = { 32, 184000 };
+
+		*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
+
+		return HasReturnvaluesIF::RETURN_OK;
+	}
 
 	/**
 	 * Set the Leap Seconds since 1972
@@ -139,34 +162,63 @@ public:
 	 * @param leapSeconds_
 	 * @return
 	 *  - @c RETURN_OK on success.
-	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t setLeapSeconds(const uint16_t leapSeconds_);
+	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
+		if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		MutexGuard helper(timeMutex);
+
+		leapSeconds = leapSeconds_;
+
+		return HasReturnvaluesIF::RETURN_OK;
+	}
 
 	/**
 	 * Get the Leap Seconds since 1972
 	 *
-	 * Must be set before!
+	 * Setter must be called before
 	 *
 	 * @param[out] leapSeconds_
 	 * @return
 	 *  - @c RETURN_OK on success.
-	 *  - Otherwise, the OS failure code is returned.
+	 *  - @c RETURN_FAILED on error
 	 */
 	static ReturnValue_t getLeapSeconds(uint16_t *leapSeconds_);
+	ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
+		if (timeMutex == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		MutexGuard helper(timeMutex);
 
+		*leapSeconds_ = leapSeconds;
+
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+
+private:
 	/**
 	 * Function to check and create the Mutex for the clock
 	 * @return
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise @c RETURN_FAILED if not able to create one
 	 */
-	static ReturnValue_t checkOrCreateClockMutex();
+	static ReturnValue_t Clock::checkOrCreateClockMutex() {
+		if (timeMutex == nullptr) {
+			MutexFactory *mutexFactory = MutexFactory::instance();
+			if (mutexFactory == nullptr) {
+				return HasReturnvaluesIF::RETURN_FAILED;
+			}
+			timeMutex = mutexFactory->createMutex();
+			if (timeMutex == nullptr) {
+				return HasReturnvaluesIF::RETURN_FAILED;
+			}
+		}
+		return HasReturnvaluesIF::RETURN_OK;
+	}
 
-private:
-	static MutexIF* timeMutex;
+	static MutexIF *timeMutex;
 	static uint16_t leapSeconds;
 };
 
-
 #endif /* FSFW_TIMEMANAGER_CLOCK_H_ */

From 4b248740f326b3329c417f3050e0f1cb9c472fd5 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 13:32:11 +0200
Subject: [PATCH 02/14] added HAL gpio class ID

---
 returnvalues/FwClassIds.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h
index 60cb33ac2..af32f9a76 100644
--- a/returnvalues/FwClassIds.h
+++ b/returnvalues/FwClassIds.h
@@ -75,6 +75,7 @@ enum: uint8_t {
     HAL_SPI, //HSPI
     HAL_UART, //HURT
     HAL_I2C, //HI2C
+    HAL_GPIO, //HGIO
     FW_CLASS_ID_COUNT // [EXPORT] : [END]
 
 };

From aa1bfcbb967465658ff50a0d73bb9500e792b78e Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 14:52:56 +0200
Subject: [PATCH 03/14] DHB update

---
 devicehandlers/DeviceHandlerBase.cpp | 27 +++++++++++++--------------
 devicehandlers/DeviceHandlerBase.h   |  2 +-
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 436606167..2a453a375 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -226,16 +226,15 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 }
 
 void DeviceHandlerBase::decrementDeviceReplyMap() {
-    for (std::map<DeviceCommandId_t, DeviceReplyInfo>::iterator iter =
-            deviceReplyMap.begin(); iter != deviceReplyMap.end(); iter++) {
-        if (iter->second.delayCycles != 0) {
-            iter->second.delayCycles--;
-            if (iter->second.delayCycles == 0) {
-                if (iter->second.periodic) {
-                    iter->second.delayCycles = iter->second.maxDelayCycles;
+    for (auto pair: deviceReplyMap) {
+        if (pair.second.delayCycles != 0) {
+            pair.second.delayCycles--;
+            if (pair.second.delayCycles == 0) {
+                if (pair.second.periodic) {
+                    pair.second.delayCycles = pair.second.maxDelayCycles;
                 }
-                replyToReply(iter, TIMEOUT);
-                missedReply(iter->first);
+                replyToReply(pair.first, pair.second, TIMEOUT);
+                missedReply(pair.first);
             }
         }
     }
@@ -584,16 +583,16 @@ void DeviceHandlerBase::replyToCommand(ReturnValue_t status,
     }
 }
 
-void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
+void DeviceHandlerBase::replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
         ReturnValue_t status) {
     // No need to check if iter exists, as this is checked by callers.
     // If someone else uses the method, add check.
-    if (iter->second.command == deviceCommandMap.end()) {
+    if (replyInfo.command == deviceCommandMap.end()) {
         //Is most likely periodic reply. Silent return.
         return;
     }
     // Check if more replies are expected. If so, do nothing.
-    DeviceCommandInfo* info = &(iter->second.command->second);
+    DeviceCommandInfo* info = &replyInfo.command->second;
     if (--info->expectedReplies == 0) {
         // Check if it was transition or internal command.
         // Don't send any replies in that case.
@@ -602,7 +601,7 @@ void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
             if(status == HasReturnvaluesIF::RETURN_OK) {
                 success = true;
             }
-            actionHelper.finish(success, info->sendReplyTo, iter->first, status);
+            actionHelper.finish(success, info->sendReplyTo, command, status);
         }
         info->isExecuting = false;
     }
@@ -801,7 +800,7 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData,
             replyRawReplyIfnotWiretapped(receivedData, foundLen);
             triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId);
         }
-        replyToReply(iter, result);
+        replyToReply(iter->first, iter->second, result);
     }
     else {
         /* Other completion failure messages are created by timeout.
diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index 496c08ffd..5141800f5 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -1195,7 +1195,7 @@ private:
      * @foundLen the length of the packet
      */
     void handleReply(const uint8_t *data, DeviceCommandId_t id, uint32_t foundLen);
-    void replyToReply(DeviceReplyMap::iterator iter, ReturnValue_t status);
+    void replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo, ReturnValue_t status);
 
     /**
      * Build and send a command to the device.

From 1c7c532ef6c237938aac18069e8595c2a8162fc4 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 15:01:02 +0200
Subject: [PATCH 04/14] DHB update

---
 devicehandlers/DeviceHandlerBase.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 2a453a375..dfa9f947b 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -226,15 +226,15 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 }
 
 void DeviceHandlerBase::decrementDeviceReplyMap() {
-    for (auto pair: deviceReplyMap) {
-        if (pair.second.delayCycles != 0) {
-            pair.second.delayCycles--;
-            if (pair.second.delayCycles == 0) {
-                if (pair.second.periodic) {
-                    pair.second.delayCycles = pair.second.maxDelayCycles;
+    for (auto replyPair: deviceReplyMap) {
+        if (replyPair.second.delayCycles != 0) {
+            replyPair.second.delayCycles--;
+            if (replyPair.second.delayCycles == 0) {
+                if (replyPair.second.periodic) {
+                    replyPair.second.delayCycles = replyPair.second.maxDelayCycles;
                 }
-                replyToReply(pair.first, pair.second, TIMEOUT);
-                missedReply(pair.first);
+                replyToReply(replyPair.first, replyPair.second, TIMEOUT);
+                missedReply(replyPair.first);
             }
         }
     }

From cc5c8ca698615bdb426d41d6eef11afff6450ac0 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 15:04:40 +0200
Subject: [PATCH 05/14] type

---
 devicehandlers/DeviceHandlerBase.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index dfa9f947b..e0062a81d 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -226,7 +226,7 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 }
 
 void DeviceHandlerBase::decrementDeviceReplyMap() {
-    for (auto replyPair: deviceReplyMap) {
+    for (std::pair<const DeviceCommandId_t, DeviceReplyInfo>& replyPair: deviceReplyMap) {
         if (replyPair.second.delayCycles != 0) {
             replyPair.second.delayCycles--;
             if (replyPair.second.delayCycles == 0) {

From 03e1a3e945c860f4a69d2e74aebff31228e8e5b4 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 15:08:18 +0200
Subject: [PATCH 06/14] const correctness

---
 devicehandlers/DeviceHandlerBase.cpp | 2 +-
 devicehandlers/DeviceHandlerBase.h   | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index e0062a81d..2a3f9d661 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -583,7 +583,7 @@ void DeviceHandlerBase::replyToCommand(ReturnValue_t status,
     }
 }
 
-void DeviceHandlerBase::replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
+void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
         ReturnValue_t status) {
     // No need to check if iter exists, as this is checked by callers.
     // If someone else uses the method, add check.
diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index 5141800f5..c278985e1 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -1195,7 +1195,8 @@ private:
      * @foundLen the length of the packet
      */
     void handleReply(const uint8_t *data, DeviceCommandId_t id, uint32_t foundLen);
-    void replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo, ReturnValue_t status);
+    void replyToReply(const DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
+            ReturnValue_t status);
 
     /**
      * Build and send a command to the device.

From 0cde65f5a1ccc7f18074cb8ede32f702b0256802 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 15:33:52 +0200
Subject: [PATCH 07/14] Added some precautions in DHB::replyToReply

---
 devicehandlers/DeviceHandlerBase.cpp | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 2a3f9d661..5665b101b 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -591,9 +591,20 @@ void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceRepl
         //Is most likely periodic reply. Silent return.
         return;
     }
-    // Check if more replies are expected. If so, do nothing.
     DeviceCommandInfo* info = &replyInfo.command->second;
-    if (--info->expectedReplies == 0) {
+    if (info == nullptr){
+    	printWarningOrError(sif::OutputTypes::OUT_ERROR,
+    	                        "replyToReply", HasReturnvaluesIF::RETURN_FAILED,
+    	                        "Command pointer not found");
+    	return;
+    }
+
+    if (info->expectedReplies > 0){
+    	// Check before to avoid underflow
+    	info->expectedReplies--;
+    }
+    // Check if more replies are expected. If so, do nothing.
+    if (info->expectedReplies == 0) {
         // Check if it was transition or internal command.
         // Don't send any replies in that case.
         if (info->sendReplyTo != NO_COMMANDER) {

From ecb03b8a6db9e8e7fcf673f424104055af5247d6 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 15:59:20 +0200
Subject: [PATCH 08/14] Moved Code to own cpp

---
 timemanager/CMakeLists.txt  |  1 +
 timemanager/Clock.h         | 60 +++-----------------------------
 timemanager/ClockCommon.cpp | 68 +++++++++++++++++++++++++++++++++++++
 3 files changed, 73 insertions(+), 56 deletions(-)
 create mode 100644 timemanager/ClockCommon.cpp

diff --git a/timemanager/CMakeLists.txt b/timemanager/CMakeLists.txt
index 3367775f4..70dd41fa6 100644
--- a/timemanager/CMakeLists.txt
+++ b/timemanager/CMakeLists.txt
@@ -5,4 +5,5 @@ target_sources(${LIB_FSFW_NAME}
 		Stopwatch.cpp
 		TimeMessage.cpp
 		TimeStamper.cpp
+		ClockCommon.cpp
 )
diff --git a/timemanager/Clock.h b/timemanager/Clock.h
index 2ac483847..5750e3d80 100644
--- a/timemanager/Clock.h
+++ b/timemanager/Clock.h
@@ -133,28 +133,7 @@ public:
 	 *  - @c RETURN_OK on success
 	 *  - @c RETURN_FAILED if leapSeconds are not set
 	 */
-	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-
-		uint16_t leapSeconds;
-		ReturnValue_t result = getLeapSeconds(&leapSeconds);
-		if (result != HasReturnvaluesIF::RETURN_OK) {
-			return result;
-		}
-		timeval leapSeconds_timeval = { 0, 0 };
-		leapSeconds_timeval.tv_sec = leapSeconds;
-
-		//initial offset between UTC and TAI
-		timeval UTCtoTAI1972 = { 10, 0 };
-
-		timeval TAItoTT = { 32, 184000 };
-
-		*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt);
 
 	/**
 	 * Set the Leap Seconds since 1972
@@ -163,16 +142,7 @@ public:
 	 * @return
 	 *  - @c RETURN_OK on success.
 	 */
-	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-		if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		MutexGuard helper(timeMutex);
-
-		leapSeconds = leapSeconds_;
-
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_);
 
 	/**
 	 * Get the Leap Seconds since 1972
@@ -184,17 +154,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - @c RETURN_FAILED on error
 	 */
-	static ReturnValue_t getLeapSeconds(uint16_t *leapSeconds_);
-	ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		MutexGuard helper(timeMutex);
-
-		*leapSeconds_ = leapSeconds;
-
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_);
 
 private:
 	/**
@@ -203,19 +163,7 @@ private:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise @c RETURN_FAILED if not able to create one
 	 */
-	static ReturnValue_t Clock::checkOrCreateClockMutex() {
-		if (timeMutex == nullptr) {
-			MutexFactory *mutexFactory = MutexFactory::instance();
-			if (mutexFactory == nullptr) {
-				return HasReturnvaluesIF::RETURN_FAILED;
-			}
-			timeMutex = mutexFactory->createMutex();
-			if (timeMutex == nullptr) {
-				return HasReturnvaluesIF::RETURN_FAILED;
-			}
-		}
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::checkOrCreateClockMutex();
 
 	static MutexIF *timeMutex;
 	static uint16_t leapSeconds;
diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
new file mode 100644
index 000000000..62be43751
--- /dev/null
+++ b/timemanager/ClockCommon.cpp
@@ -0,0 +1,68 @@
+/*
+ * ClockCommon.cpp
+ *
+ *  Created on: Jun 15, 2021
+ *      Author: steffen
+ */
+
+#include "Clock.h"
+#include "../ipc/MutexGuard.h"
+
+ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
+	if (timeMutex == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	uint16_t leapSeconds;
+	ReturnValue_t result = getLeapSeconds(&leapSeconds);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	timeval leapSeconds_timeval = { 0, 0 };
+	leapSeconds_timeval.tv_sec = leapSeconds;
+
+	//initial offset between UTC and TAI
+	timeval UTCtoTAI1972 = { 10, 0 };
+
+	timeval TAItoTT = { 32, 184000 };
+
+	*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
+	if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	MutexGuard helper(timeMutex);
+
+	leapSeconds = leapSeconds_;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
+	if (timeMutex == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	MutexGuard helper(timeMutex);
+
+	*leapSeconds_ = leapSeconds;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+static ReturnValue_t Clock::checkOrCreateClockMutex() {
+	if (timeMutex == nullptr) {
+		MutexFactory *mutexFactory = MutexFactory::instance();
+		if (mutexFactory == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		timeMutex = mutexFactory->createMutex();
+		if (timeMutex == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}

From 717971f69c67ece165f19ea4e2c11b2825e9c942 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:01:28 +0200
Subject: [PATCH 09/14] Fixed wrong namespace qualifier

---
 timemanager/Clock.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/timemanager/Clock.h b/timemanager/Clock.h
index 5750e3d80..6a76c86d9 100644
--- a/timemanager/Clock.h
+++ b/timemanager/Clock.h
@@ -133,7 +133,7 @@ public:
 	 *  - @c RETURN_OK on success
 	 *  - @c RETURN_FAILED if leapSeconds are not set
 	 */
-	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt);
+	static ReturnValue_t convertUTCToTT(timeval utc, timeval *tt);
 
 	/**
 	 * Set the Leap Seconds since 1972
@@ -142,7 +142,7 @@ public:
 	 * @return
 	 *  - @c RETURN_OK on success.
 	 */
-	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_);
+	static ReturnValue_t setLeapSeconds(const uint16_t leapSeconds_);
 
 	/**
 	 * Get the Leap Seconds since 1972
@@ -154,7 +154,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - @c RETURN_FAILED on error
 	 */
-	static ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_);
+	static ReturnValue_t getLeapSeconds(uint16_t *leapSeconds_);
 
 private:
 	/**
@@ -163,7 +163,7 @@ private:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise @c RETURN_FAILED if not able to create one
 	 */
-	static ReturnValue_t Clock::checkOrCreateClockMutex();
+	static ReturnValue_t checkOrCreateClockMutex();
 
 	static MutexIF *timeMutex;
 	static uint16_t leapSeconds;

From dbfcf8b271d8e37d5645288d00e8ec26e8d3eb81 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:03:11 +0200
Subject: [PATCH 10/14] Removed old check

---
 timemanager/ClockCommon.cpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
index 62be43751..efec0937e 100644
--- a/timemanager/ClockCommon.cpp
+++ b/timemanager/ClockCommon.cpp
@@ -9,10 +9,6 @@
 #include "../ipc/MutexGuard.h"
 
 ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
-	if (timeMutex == nullptr) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
 	uint16_t leapSeconds;
 	ReturnValue_t result = getLeapSeconds(&leapSeconds);
 	if (result != HasReturnvaluesIF::RETURN_OK) {

From ee89a2f00d8bf7c36f63ecea1a8a1a3305adea3d Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:05:12 +0200
Subject: [PATCH 11/14] Removed author tag

---
 timemanager/ClockCommon.cpp | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
index efec0937e..8b75f3e57 100644
--- a/timemanager/ClockCommon.cpp
+++ b/timemanager/ClockCommon.cpp
@@ -1,10 +1,3 @@
-/*
- * ClockCommon.cpp
- *
- *  Created on: Jun 15, 2021
- *      Author: steffen
- */
-
 #include "Clock.h"
 #include "../ipc/MutexGuard.h"
 

From 2ccc0dbb0075d3c90068a565ef55436affb3de71 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:12:25 +0200
Subject: [PATCH 12/14] Removed wrong static

---
 timemanager/ClockCommon.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
index 8b75f3e57..e56d4953a 100644
--- a/timemanager/ClockCommon.cpp
+++ b/timemanager/ClockCommon.cpp
@@ -42,7 +42,7 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
-static ReturnValue_t Clock::checkOrCreateClockMutex() {
+ReturnValue_t Clock::checkOrCreateClockMutex() {
 	if (timeMutex == nullptr) {
 		MutexFactory *mutexFactory = MutexFactory::instance();
 		if (mutexFactory == nullptr) {

From bd7c13ff7e187bb5590f597d87cd2a76c2650497 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:47:24 +0200
Subject: [PATCH 13/14] NO_COMMANDER was not using NO_QUEUE in DHB

---
 devicehandlers/DeviceHandlerBase.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index c278985e1..53bd1e653 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -667,7 +667,7 @@ protected:
     static const ReturnValue_t CHILD_TIMEOUT = MAKE_RETURN_CODE(0xE0);
     static const ReturnValue_t SWITCH_FAILED = MAKE_RETURN_CODE(0xE1);
 
-    static const MessageQueueId_t NO_COMMANDER = 0;
+    static const MessageQueueId_t NO_COMMANDER = MessageQueueIF::NO_QUEUE;
 
     //! Pointer to the raw packet that will be sent.
     uint8_t *rawPacket = nullptr;

From 47af5260a2341249bc00cdba095f96efa5a509dc Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Thu, 17 Jun 2021 12:04:39 +0200
Subject: [PATCH 14/14] added missing include

---
 tmtcpacket/pus/tc/TcPacketStoredPus.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/tmtcpacket/pus/tc/TcPacketStoredPus.cpp b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
index f098ce2b0..426aafdb3 100644
--- a/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
@@ -1,5 +1,7 @@
 #include "TcPacketStoredPus.h"
 
+#include "../../../serviceinterface/ServiceInterface.h"
+
 #include <cstring>
 
 TcPacketStoredPus::TcPacketStoredPus(uint16_t apid,  uint8_t service,