From 87f64d99cd1b7eae59b601140d13a5b92565f384 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 5 Jun 2020 23:18:00 +0200 Subject: [PATCH] implemented fixed timeslot task --- osal/host/FixedTimeslotTask.cpp | 191 ++++++++++++++++++++++++++++++++ osal/host/FixedTimeslotTask.h | 130 ++++++++++++++++++++++ osal/host/PeriodicTask.cpp | 6 +- osal/host/PeriodicTask.h | 5 +- osal/host/TaskFactory.cpp | 6 +- 5 files changed, 330 insertions(+), 8 deletions(-) create mode 100644 osal/host/FixedTimeslotTask.cpp create mode 100644 osal/host/FixedTimeslotTask.h diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp new file mode 100644 index 000000000..48d11843f --- /dev/null +++ b/osal/host/FixedTimeslotTask.cpp @@ -0,0 +1,191 @@ +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#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(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(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(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 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 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( + std::chrono::system_clock::now().time_since_epoch()) + }; + if(interval.count() > 0) { + delayForInterval(¤tStartTime, 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(¤tStartTime, interval); + //TODO deadline missed check + } + } +} + +ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, + uint32_t slotTimeMs, int8_t executionStep) { + if (objectManager->get(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( + 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( + 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; + +} + + + + diff --git a/osal/host/FixedTimeslotTask.h b/osal/host/FixedTimeslotTask.h new file mode 100644 index 000000000..bb2f757aa --- /dev/null +++ b/osal/host/FixedTimeslotTask.h @@ -0,0 +1,130 @@ +#ifndef FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ +#define FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +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 ObjectList; + std::thread mainThread; + std::atomic 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_ */ diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp index 57898ac2c..1a5024abf 100644 --- a/osal/host/PeriodicTask.cpp +++ b/osal/host/PeriodicTask.cpp @@ -105,7 +105,7 @@ void PeriodicTask::taskFunctionality() { it != objectList.end(); ++it) { (*it)->performOperation(); } - if(not delayUntil(¤tStartTime, periodChrono)) { + if(not delayForInterval(¤tStartTime, 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, - const std::chrono::milliseconds interval) { +bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, + const chron_ms interval) { bool shouldDelay = false; //Get current wakeup time auto currentStartTime = diff --git a/osal/host/PeriodicTask.h b/osal/host/PeriodicTask.h index 19c06b52c..d97bf0890 100644 --- a/osal/host/PeriodicTask.h +++ b/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 ObjectList; @@ -115,8 +116,8 @@ protected: */ void taskFunctionality(void); - bool delayUntil(std::chrono::milliseconds * previousWakeTimeMs, - const std::chrono::milliseconds interval); + bool delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval); }; #endif /* PERIODICTASK_H_ */ diff --git a/osal/host/TaskFactory.cpp b/osal/host/TaskFactory.cpp index a13844c4a..9db8ac4d3 100644 --- a/osal/host/TaskFactory.cpp +++ b/osal/host/TaskFactory.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -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 " - "yet" << std::endl; - return nullptr; + return new FixedTimeslotTask(name_, taskPriority_, stackSize_, + periodInSeconds_, deadLineMissedFunction_); } ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) {