From 64f97fc3baa8c3c240d0f1f7cfafd7530dcd0dc4 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meier-jakob@gmx.de>
Date: Mon, 23 Dec 2024 12:02:29 +0100
Subject: [PATCH] implemented freertos monotonic clock

---
 src/fsfw/osal/freertos/Clock.cpp      |  5 ++---
 src/fsfw/osal/freertos/Timekeeper.cpp | 12 +++++++++++-
 src/fsfw/osal/freertos/Timekeeper.h   |  6 ++++++
 src/fsfw/timemanager/Clock.h          |  2 ++
 4 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/src/fsfw/osal/freertos/Clock.cpp b/src/fsfw/osal/freertos/Clock.cpp
index 4c8dde54..43ff7b72 100644
--- a/src/fsfw/osal/freertos/Clock.cpp
+++ b/src/fsfw/osal/freertos/Clock.cpp
@@ -48,8 +48,7 @@ ReturnValue_t Clock::getClock(timeval* time) {
 }
 
 ReturnValue_t Clock::getClockMonotonic(timeval* time) {
-	TickType_t ticks = Timekeeper::getTicks();
-	*time = Timekeeper::ticksToTimeval(ticks);
+	*time = Timekeeper::instance()->getMonotonicClockOffset() + getUptime();
   return returnvalue::OK;
 }
 
@@ -60,7 +59,7 @@ ReturnValue_t Clock::getUptime(timeval* uptime) {
 }
 
 timeval Clock::getUptime() {
-  TickType_t ticksSinceStart = xTaskGetTickCount();
+  TickType_t ticksSinceStart = Timekeeper::instance()->getTicks();
   return Timekeeper::ticksToTimeval(ticksSinceStart);
 }
 
diff --git a/src/fsfw/osal/freertos/Timekeeper.cpp b/src/fsfw/osal/freertos/Timekeeper.cpp
index 6649e099..ca0fe5d0 100644
--- a/src/fsfw/osal/freertos/Timekeeper.cpp
+++ b/src/fsfw/osal/freertos/Timekeeper.cpp
@@ -17,7 +17,13 @@ Timekeeper* Timekeeper::instance() {
   return myinstance;
 }
 
-void Timekeeper::setOffset(const timeval& offset) { this->offset = offset; }
+void Timekeeper::setOffset(const timeval& offset) {
+	if (not monotonicClockInitialized) {
+		this->monotonicClockOffset = offset;
+		monotonicClockInitialized = true;
+	}
+	this->offset = offset;
+}
 
 timeval Timekeeper::ticksToTimeval(TickType_t ticks) {
   timeval uptime;
@@ -33,3 +39,7 @@ timeval Timekeeper::ticksToTimeval(TickType_t ticks) {
 }
 
 TickType_t Timekeeper::getTicks() { return xTaskGetTickCount(); }
+
+const timeval Timekeeper::getMonotonicClockOffset() const {
+	return monotonicClockOffset;
+}
diff --git a/src/fsfw/osal/freertos/Timekeeper.h b/src/fsfw/osal/freertos/Timekeeper.h
index 9068b902..f7970bc8 100644
--- a/src/fsfw/osal/freertos/Timekeeper.h
+++ b/src/fsfw/osal/freertos/Timekeeper.h
@@ -18,9 +18,14 @@ class Timekeeper {
   Timekeeper();
 
   timeval offset;
+  // Set when offset is initialized the first time
+  timeval monotonicClockOffset;
+  bool monotonicClockInitialized = false;
 
   static Timekeeper* myinstance;
 
+  void setMonotonicClockOffset(const timeval& monotonicClockOffset);
+
  public:
   static Timekeeper* instance();
   virtual ~Timekeeper();
@@ -34,6 +39,7 @@ class Timekeeper {
 
   const timeval& getOffset() const;
   void setOffset(const timeval& offset);
+  const timeval getMonotonicClockOffset() const;
 };
 
 #endif /* FRAMEWORK_OSAL_FREERTOS_TIMEKEEPER_H_ */
diff --git a/src/fsfw/timemanager/Clock.h b/src/fsfw/timemanager/Clock.h
index 6fe6486c..e9c1dd05 100644
--- a/src/fsfw/timemanager/Clock.h
+++ b/src/fsfw/timemanager/Clock.h
@@ -192,6 +192,8 @@ class Clock {
   static MutexIF *timeMutex;
   static uint16_t leapSeconds;
   static bool leapSecondsSet;
+  static bool monotonicClockInitialized;
+  static timeval monotonicClockOffset;
 };
 
 #endif /* FSFW_TIMEMANAGER_CLOCK_H_ */