diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp index 43928f7c2..fa49150ec 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 a580f519f..09aa6bc7d 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_ */