From 7d579889792144e9cf45fdcc332bb3636c69f488 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Sat, 16 May 2020 13:09:50 +0200
Subject: [PATCH] added deadline check

---
 osal/FreeRTOS/PeriodicTask.cpp | 18 +++++++++++++++---
 osal/FreeRTOS/PeriodicTask.h   |  6 ++++--
 2 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp
index 3075be79..a8e40561 100644
--- a/osal/FreeRTOS/PeriodicTask.cpp
+++ b/osal/FreeRTOS/PeriodicTask.cpp
@@ -1,6 +1,7 @@
+#include "PeriodicTask.h"
+
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
 #include <framework/tasks/ExecutableObjectIF.h>
-#include "PeriodicTask.h"
 
 PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
 		TaskStackSize setStack, TaskPeriod setPeriod,
@@ -31,7 +32,7 @@ void PeriodicTask::taskEntryPoint(void* argument) {
 	// if it is not set and we get here, the scheduler was started before #startTask() was called and we need to suspend
 	// if it is set, the scheduler was not running before #startTask() was called and we can continue
 
-	if (!originalTask->started) {
+	if (not originalTask->started) {
 		vTaskSuspend(NULL);
 	}
 
@@ -70,8 +71,19 @@ void PeriodicTask::taskFunctionality() {
 				it != objectList.end(); ++it) {
 			(*it)->performOperation();
 		}
-		//TODO deadline missed check
+
+		/* 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) {
+			sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) <<
+					" missed deadline!\n" << std::flush;
+			if(deadlineMissedFunc != nullptr) {
+				this->deadlineMissedFunc();
+			}
+		}
 		vTaskDelayUntil(&xLastWakeTime, xPeriod);
+
 	}
 }
 
diff --git a/osal/FreeRTOS/PeriodicTask.h b/osal/FreeRTOS/PeriodicTask.h
index 02ab7148..4ece20b3 100644
--- a/osal/FreeRTOS/PeriodicTask.h
+++ b/osal/FreeRTOS/PeriodicTask.h
@@ -5,8 +5,10 @@
 #include <framework/tasks/PeriodicTaskIF.h>
 #include <framework/tasks/Typedef.h>
 
-#include <FreeRTOS.h>
-#include "task.h"
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+}
 
 #include <vector>