From 847292ea30e3c71678b0cf994e6436caad71a463 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 23 Jun 2020 01:10:19 +0200
Subject: [PATCH] added overflow checking for periodic task

---
 osal/FreeRTOS/PeriodicTask.cpp | 47 +++++++++++++++++++++++++---------
 osal/FreeRTOS/PeriodicTask.h   |  4 +++
 2 files changed, 39 insertions(+), 12 deletions(-)

diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp
index 43928f7c..fa49150e 100644
--- a/osal/FreeRTOS/PeriodicTask.cpp
+++ b/osal/FreeRTOS/PeriodicTask.cpp
@@ -74,18 +74,7 @@ void PeriodicTask::taskFunctionality() {
 			object->performOperation();
 		}
 
-		/* If all operations are finished and the difference of the
-		 * current time minus the last wake time is larger than the
-		 * wait period, a deadline was missed. */
-		if(xTaskGetTickCount() - xLastWakeTime >= xPeriod) {
-#ifdef DEBUG
-			sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) <<
-					" missed deadline!\n" << std::flush;
-#endif
-			if(deadlineMissedFunc != nullptr) {
-				this->deadlineMissedFunc();
-			}
-		}
+		checkMissedDeadline(xLastWakeTime, xPeriod);
 
 		vTaskDelayUntil(&xLastWakeTime, xPeriod);
 
@@ -110,3 +99,37 @@ ReturnValue_t PeriodicTask::addComponent(object_id_t object, bool setTaskIF) {
 uint32_t PeriodicTask::getPeriodMs() const {
 	return period * 1000;
 }
+
+void PeriodicTask::checkMissedDeadline(const TickType_t xLastWakeTime,
+        const TickType_t interval) {
+    /* Check whether deadline was missed while also taking overflows
+     * into account. Drawing this on paper with a timeline helps to understand
+     * it. */
+    TickType_t currentTickCount = xTaskGetTickCount();
+    TickType_t timeToWake = xLastWakeTime + interval;
+    // Tick count has overflown
+    if(currentTickCount < xLastWakeTime) {
+        // Time to wake has overflown as well. If the tick count
+        // is larger than the time to wake, a deadline was missed.
+        if(timeToWake < xLastWakeTime and
+                currentTickCount > timeToWake) {
+            handleMissedDeadline();
+        }
+    }
+    // No tick count overflow. If the timeToWake has not overflown
+    // and the current tick count is larger than the time to wake,
+    // a deadline was missed.
+    else if(timeToWake > xLastWakeTime and currentTickCount > timeToWake) {
+        handleMissedDeadline();
+    }
+}
+
+void PeriodicTask::handleMissedDeadline() {
+#ifdef DEBUG
+    sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) <<
+            " missed deadline!\n" << std::flush;
+#endif
+    if(deadlineMissedFunc != nullptr) {
+        this->deadlineMissedFunc();
+    }
+}
diff --git a/osal/FreeRTOS/PeriodicTask.h b/osal/FreeRTOS/PeriodicTask.h
index a580f519..09aa6bc7 100644
--- a/osal/FreeRTOS/PeriodicTask.h
+++ b/osal/FreeRTOS/PeriodicTask.h
@@ -116,6 +116,10 @@ protected:
 	 * On missing the deadline, the deadlineMissedFunction is executed.
 	 */
 	void taskFunctionality(void);
+
+	void checkMissedDeadline(const TickType_t xLastWakeTime,
+	        const TickType_t interval);
+	void handleMissedDeadline();
 };
 
 #endif /* PERIODICTASK_H_ */