implemented fixed timeslot task

osal/host/FixedTimeslotTask.cpp

@@ -0,0 +1,191 @@
+#include <framework/osal/host/FixedTimeslotTask.h>
+
+#include <framework/ipc/MutexFactory.h>
+#include <framework/osal/host/Mutex.h>
+#include <framework/osal/host/FixedTimeslotTask.h>
+
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+#include <framework/tasks/ExecutableObjectIF.h>
+
+#include <thread>
+#include <chrono>
+
+#if defined(WIN32)
+#include <windows.h>
+#elif defined(LINUX)
+#include <pthread.h>
+#endif
+
+FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority,
+        TaskStackSize setStack, TaskPeriod setPeriod,
+        void (*setDeadlineMissedFunc)()) :
+        started(false), pollingSeqTable(setPeriod*1000), taskName(name),
+        period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) {
+    // It is propably possible to set task priorities by using the native
+    // task handles for Windows / Linux
+    mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this);
+#if defined(WIN32)
+    /* List of possible priority classes:
+     * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/
+     * nf-processthreadsapi-setpriorityclass
+     * And respective thread priority numbers:
+     * https://docs.microsoft.com/en-us/windows/
+     * win32/procthread/scheduling-priorities */
+    int result = SetPriorityClass(
+            reinterpret_cast<HANDLE>(mainThread.native_handle()),
+            ABOVE_NORMAL_PRIORITY_CLASS);
+    if(result != 0) {
+        sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code "
+                << GetLastError() << std::endl;
+    }
+    result = SetThreadPriority(
+            reinterpret_cast<HANDLE>(mainThread.native_handle()),
+            THREAD_PRIORITY_NORMAL);
+    if(result != 0) {
+        sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code "
+                << GetLastError() << std::endl;
+    }
+#elif defined(LINUX)
+    // we can just copy and paste the code from linux here.
+#endif
+}
+
+FixedTimeslotTask::~FixedTimeslotTask(void) {
+    //Do not delete objects, we were responsible for ptrs only.
+    terminateThread = true;
+    if(mainThread.joinable()) {
+        mainThread.join();
+    }
+    delete this;
+}
+
+void FixedTimeslotTask::taskEntryPoint(void* argument) {
+    FixedTimeslotTask *originalTask(reinterpret_cast<FixedTimeslotTask*>(argument));
+
+    if (not originalTask->started) {
+        // we have to suspend/block here until the task is started.
+        // if semaphores are implemented, use them here.
+        std::unique_lock<std::mutex> lock(initMutex);
+        initCondition.wait(lock);
+    }
+
+    this->taskFunctionality();
+    sif::debug << "FixedTimeslotTask::taskEntryPoint: "
+            "Returned from taskFunctionality." << std::endl;
+}
+
+ReturnValue_t FixedTimeslotTask::startTask() {
+    started = true;
+
+    // Notify task to start.
+    std::lock_guard<std::mutex> lock(initMutex);
+    initCondition.notify_one();
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void FixedTimeslotTask::taskFunctionality() {
+    // A local iterator for the Polling Sequence Table is created to
+    // find the start time for the first entry.
+    SlotListIter slotListIter = pollingSeqTable.current;
+    // Get start time for first entry.
+    chron_ms interval(slotListIter->pollingTimeMs);
+    auto currentStartTime {
+            std::chrono::duration_cast<chron_ms>(
+            std::chrono::system_clock::now().time_since_epoch())
+    };
+    if(interval.count() > 0) {
+        delayForInterval(&currentStartTime, interval);
+    }
+
+    /* Enter the loop that defines the task behavior. */
+    for (;;) {
+        if(terminateThread.load()) {
+            break;
+        }
+        //The component for this slot is executed and the next one is chosen.
+        this->pollingSeqTable.executeAndAdvance();
+        if (not pollingSeqTable.slotFollowsImmediately()) {
+            // we need to wait before executing the current slot
+            //this gives us the time to wait:
+            interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs());
+            delayForInterval(&currentStartTime, interval);
+            //TODO deadline missed check
+        }
+    }
+}
+
+ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
+        uint32_t slotTimeMs, int8_t executionStep) {
+    if (objectManager->get<ExecutableObjectIF>(componentId) != nullptr) {
+        pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, this);
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+
+    sif::error << "Component " << std::hex << componentId <<
+            " not found, not adding it to pst" << std::endl;
+    return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+ReturnValue_t FixedTimeslotTask::checkSequence() const {
+    return pollingSeqTable.checkSequence();
+}
+
+uint32_t FixedTimeslotTask::getPeriodMs() const {
+    return period * 1000;
+}
+
+bool FixedTimeslotTask::delayForInterval(chron_ms * previousWakeTimeMs,
+        const chron_ms interval) {
+    bool shouldDelay = false;
+    //Get current wakeup time
+    auto currentStartTime =
+            std::chrono::duration_cast<chron_ms>(
+            std::chrono::system_clock::now().time_since_epoch());
+    /* Generate the tick time at which the task wants to wake. */
+    auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval;
+
+    if (currentStartTime < *previousWakeTimeMs) {
+        /* The tick count has overflowed since this function was
+         lasted called.  In this case the only time we should ever
+         actually delay is if the wake time has also    overflowed,
+         and the wake time is greater than the tick time.  When this
+         is the case it is as if neither time had overflowed. */
+        if ((nextTimeToWake_ms < *previousWakeTimeMs)
+                && (nextTimeToWake_ms > currentStartTime)) {
+            shouldDelay = true;
+        }
+    } else {
+        /* The tick time has not overflowed.  In this case we will
+         delay if either the wake time has overflowed, and/or the
+         tick time is less than the wake time. */
+        if ((nextTimeToWake_ms < *previousWakeTimeMs)
+                || (nextTimeToWake_ms >  currentStartTime)) {
+            shouldDelay = true;
+        }
+    }
+
+    /* Update the wake time ready for the next call. */
+
+    (*previousWakeTimeMs) =  nextTimeToWake_ms;
+
+    if (shouldDelay) {
+        auto sleepTime = std::chrono::duration_cast<chron_ms>(
+                nextTimeToWake_ms - currentStartTime);
+        std::this_thread::sleep_for(sleepTime);
+        return true;
+    }
+    //We are shifting the time in case the deadline was missed like rtems
+    (*previousWakeTimeMs) = currentStartTime;
+    return false;
+
+}
+
+
+
+

osal/host/FixedTimeslotTask.h

@@ -0,0 +1,130 @@
+#ifndef FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_
+#define FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_
+
+#include <framework/objectmanager/ObjectManagerIF.h>
+#include <framework/devicehandlers/FixedSlotSequence.h>
+#include <framework/tasks/FixedTimeslotTaskIF.h>
+#include <framework/tasks/Typedef.h>
+
+#include <vector>
+#include <thread>
+#include <condition_variable>
+#include <atomic>
+
+class ExecutableObjectIF;
+
+/**
+ * @brief   This class represents a task for periodic activities with multiple
+ *          steps and strict timeslot requirements for these steps.
+ * @details
+ * @ingroup task_handling
+ */
+class FixedTimeslotTask: public FixedTimeslotTaskIF {
+public:
+    /**
+     * @brief   Standard constructor of the class.
+     * @details
+     * The class is initialized without allocated objects. These need to be
+     * added with #addComponent.
+     * @param priority
+     * @param stack_size
+     * @param setPeriod
+     * @param setDeadlineMissedFunc
+     * The function pointer to the deadline missed function that shall be
+     * assigned.
+     */
+    FixedTimeslotTask(const char *name, TaskPriority setPriority,
+            TaskStackSize setStack, TaskPeriod setPeriod,
+            void (*setDeadlineMissedFunc)());
+    /**
+     * @brief   Currently, the executed object's lifetime is not coupled with
+     *          the task object's lifetime, so the destructor is empty.
+     */
+    virtual ~FixedTimeslotTask(void);
+
+    /**
+     * @brief   The method to start the task.
+     * @details The method starts the task with the respective system call.
+     *          Entry point is the taskEntryPoint method described below.
+     *          The address of the task object is passed as an argument
+     *          to the system call.
+     */
+    ReturnValue_t startTask(void);
+
+    /**
+     * Add timeslot to the polling sequence table.
+     * @param componentId
+     * @param slotTimeMs
+     * @param executionStep
+     * @return
+     */
+    ReturnValue_t addSlot(object_id_t componentId,
+                   uint32_t slotTimeMs, int8_t executionStep);
+
+    ReturnValue_t checkSequence() const;
+
+    uint32_t getPeriodMs() const;
+
+    ReturnValue_t sleepFor(uint32_t ms);
+
+protected:
+    using chron_ms = std::chrono::milliseconds;
+
+    bool started;
+    //!< Typedef for the List of objects.
+    typedef std::vector<ExecutableObjectIF*> ObjectList;
+    std::thread mainThread;
+    std::atomic<bool> terminateThread = false;
+
+    //! Polling sequence table which contains the object to execute
+    //! and information like the timeslots and the passed execution step.
+    FixedSlotSequence pollingSeqTable;
+
+    std::condition_variable initCondition;
+    std::mutex initMutex;
+    std::string taskName;
+    /**
+     * @brief   The period of the task.
+     * @details
+     * The period determines the frequency of the task's execution.
+     * It is expressed in clock ticks.
+     */
+    TaskPeriod period;
+
+    /**
+     * @brief   The pointer to the deadline-missed function.
+     * @details
+     * This pointer stores the function that is executed if the task's deadline
+     * is missed. So, each may react individually on a timing failure.
+     * The pointer may be NULL, then nothing happens on missing the deadline.
+     * The deadline is equal to the next execution of the periodic task.
+     */
+    void (*deadlineMissedFunc)(void);
+    /**
+     * @brief   This is the function executed in the new task's context.
+     * @details
+     * It converts the argument back to the thread object type and copies the
+     * class instance to the task context.
+     * The taskFunctionality method is called afterwards.
+     * @param   A pointer to the task object itself is passed as argument.
+     */
+
+    void taskEntryPoint(void* argument);
+    /**
+     * @brief   The function containing the actual functionality of the task.
+     * @details
+     * The method sets and starts the task's period, then enters a loop that is
+     * repeated as long as the isRunning attribute is true. Within the loop,
+     * all performOperation methods of the added objects are called. Afterwards
+     * the checkAndRestartPeriod system call blocks the task until the next
+     *  period. On missing the deadline, the deadlineMissedFunction is executed.
+     */
+    void taskFunctionality(void);
+
+    bool delayForInterval(chron_ms * previousWakeTimeMs,
+            const chron_ms interval);
+};
+
+
+
+#endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */

osal/host/PeriodicTask.cpp

@@ -105,7 +105,7 @@ void PeriodicTask::taskFunctionality() {
 				it != objectList.end(); ++it) {
 			(*it)->performOperation();
 		}
-		if(not delayUntil(&currentStartTime, periodChrono)) {
+		if(not delayForInterval(&currentStartTime, periodChrono)) {
 			sif::warning << "PeriodicTask: " << taskName <<
 					" missed deadline!\n" << std::flush;
 			if(deadlineMissedFunc != nullptr) {
@@ -129,8 +129,8 @@ uint32_t PeriodicTask::getPeriodMs() const {
 	return period * 1000;
 }
 
-bool PeriodicTask::delayUntil(std::chrono::milliseconds * previousWakeTimeMs,
+bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs,
-        const std::chrono::milliseconds interval) {
+        const chron_ms interval) {
     bool shouldDelay = false;
     //Get current wakeup time
     auto currentStartTime =

osal/host/PeriodicTask.h

@@ -64,6 +64,7 @@ public:
 	ReturnValue_t sleepFor(uint32_t ms);
 
 protected:
+	using chron_ms = std::chrono::milliseconds;
 	bool started;
 	//!< Typedef for the List of objects.
 	typedef std::vector<ExecutableObjectIF*> ObjectList;
@@ -115,8 +116,8 @@ protected:
 	 */
 	void taskFunctionality(void);
 
-	bool delayUntil(std::chrono::milliseconds * previousWakeTimeMs,
+	bool delayForInterval(chron_ms * previousWakeTimeMs,
-	        const std::chrono::milliseconds interval);
+	        const chron_ms interval);
 };
 
 #endif /* PERIODICTASK_H_ */

osal/host/TaskFactory.cpp

@@ -1,3 +1,4 @@
+#include <framework/osal/host/FixedTimeslotTask.h>
 #include <framework/osal/host/PeriodicTask.h>
 #include <framework/tasks/TaskFactory.h>
 #include <framework/returnvalues/HasReturnvaluesIF.h>
@@ -36,9 +37,8 @@ FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_,
 		TaskDeadlineMissedFunction deadLineMissedFunction_) {
 	// This is going to be interesting. Time now learn the C++ threading library
 	// :-)
-	sif::warning << "TaskFactory::createFixedTimeslotTask: Not implemented "
+	return new FixedTimeslotTask(name_, taskPriority_, stackSize_,
-			"yet" << std::endl;
+	        periodInSeconds_, deadLineMissedFunction_);
-	return nullptr;
 }
 
 ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) {