diff --git a/osal/FreeRTOS/FixedTimeslotTask.cpp b/osal/FreeRTOS/FixedTimeslotTask.cpp index d92474c7..d72354ae 100644 --- a/osal/FreeRTOS/FixedTimeslotTask.cpp +++ b/osal/FreeRTOS/FixedTimeslotTask.cpp @@ -109,25 +109,8 @@ void FixedTimeslotTask::taskFunctionality() { intervalMs = this->pst.getIntervalToPreviousSlotMs(); interval = pdMS_TO_TICKS(intervalMs); - /* If all operations are finished and the difference of the - * current time minus the last wake time is larger than the - * expected wait period, a deadline was missed. - * We also check whether an overflow has occured first. - * In this special case, we will ignore missed deadlines for now. */ - const TickType_t constTickCount = xTaskGetTickCount(); - if(constTickCount < xLastWakeTime or - constTickCount + interval < xLastWakeTime) { - // don't do anything for now. - } - else if(xTaskGetTickCount() - xLastWakeTime >= interval) { -#ifdef DEBUG - sif::warning << "FixedTimeslotTask: " << pcTaskGetName(NULL) << - " missed deadline!\n" << std::flush; -#endif - if(deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); - } - } + checkMissedDeadline(xLastWakeTime, interval); + // Wait for the interval. This exits immediately if a deadline was // missed while also updating the last wake time. vTaskDelayUntil(&xLastWakeTime, interval); @@ -135,6 +118,40 @@ void FixedTimeslotTask::taskFunctionality() { } } +void FixedTimeslotTask::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 FixedTimeslotTask::handleMissedDeadline() { +#ifdef DEBUG + sif::warning << "FixedTimeslotTask: " << pcTaskGetName(NULL) << + " missed deadline!\n" << std::flush; +#endif + if(deadlineMissedFunc != nullptr) { + this->deadlineMissedFunc(); + } +} + ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { vTaskDelay(pdMS_TO_TICKS(ms)); return HasReturnvaluesIF::RETURN_OK; diff --git a/osal/FreeRTOS/FixedTimeslotTask.h b/osal/FreeRTOS/FixedTimeslotTask.h index 39db667c..cddd5ecd 100644 --- a/osal/FreeRTOS/FixedTimeslotTask.h +++ b/osal/FreeRTOS/FixedTimeslotTask.h @@ -89,6 +89,10 @@ protected: * OS's System Calls to keep the timing of the periods. */ void taskFunctionality(void); + + void checkMissedDeadline(const TickType_t xLastWakeTime, + const TickType_t interval); + void handleMissedDeadline(); }; #endif /* POLLINGTASK_H_ */