From cca5689a18425627559308e87dfde34f22cc28d0 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 4 Jan 2021 15:17:11 +0100
Subject: [PATCH 1/2] clock improvements for rtems

---
 osal/rtems/Clock.cpp | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/osal/rtems/Clock.cpp b/osal/rtems/Clock.cpp
index dda15464..8bd7ac37 100644
--- a/osal/rtems/Clock.cpp
+++ b/osal/rtems/Clock.cpp
@@ -37,15 +37,24 @@ ReturnValue_t Clock::setClock(const TimeOfDay_t* time) {
 }
 
 ReturnValue_t Clock::setClock(const timeval* time) {
-	//TODO This routine uses _TOD_Set which is not
 	timespec newTime;
 	newTime.tv_sec = time->tv_sec;
+	if(time->tv_usec < 0) {
+		// better returnvalue.
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
 	newTime.tv_nsec = time->tv_usec * TOD_NANOSECONDS_PER_MICROSECOND;
-	//SHOULDDO: Not sure if we need to protect this call somehow (by thread lock or something).
-	//Uli: rtems docu says you can call this from an ISR, not sure if this means no protetion needed
-	//TODO Second parameter is ISR_lock_Context
-	_TOD_Set(&newTime,nullptr);
-	return HasReturnvaluesIF::RETURN_OK;
+
+	ISR_lock_Context context;
+	_TOD_Lock();
+	_TOD_Acquire(&context);
+	Status_Control status = _TOD_Set(&newTime, &context);
+	_TOD_Unlock();
+	if(status == STATUS_SUCCESSFUL) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	// better returnvalue
+	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
 ReturnValue_t Clock::getClock_timeval(timeval* time) {
@@ -95,6 +104,7 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
 }
 
 ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
+	// TIsn't this a bug? Are RTEMS ticks always microseconds?
 	rtems_time_of_day* timeRtems = reinterpret_cast<rtems_time_of_day*>(time);
 	rtems_status_code status = rtems_clock_get_tod(timeRtems);
 	switch (status) {

From 3c041388d6705bccda5790ceeb5545ed9e48613d Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 4 Jan 2021 15:31:06 +0100
Subject: [PATCH 2/2] bugfix

---
 serviceinterface/ServiceInterfaceBuffer.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/serviceinterface/ServiceInterfaceBuffer.cpp b/serviceinterface/ServiceInterfaceBuffer.cpp
index 20ea61dd..5846d233 100644
--- a/serviceinterface/ServiceInterfaceBuffer.cpp
+++ b/serviceinterface/ServiceInterfaceBuffer.cpp
@@ -28,18 +28,21 @@ ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string setMessage,
 	}
 
 #if FSFW_COLORED_OUTPUT == 1
-	if(setMessage.find("DEBUG")) {
+	if(setMessage.find("DEBUG") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_MAGENTA;
 	}
-	else if(setMessage.find("INFO")) {
+	else if(setMessage.find("INFO") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_GREEN;
 	}
-	else if(setMessage.find("WARNING")) {
+	else if(setMessage.find("WARNING") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_YELLOW;
 	}
-	else if(setMessage.find("ERROR")) {
+	else if(setMessage.find("ERROR") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_RED;
 	}
+	else {
+		colorPrefix = fsfw::ANSI_COLOR_RESET;
+	}
 
 #ifdef WIN32
 	HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);