diff --git a/src/System/include/BipedalLocomotion/System/PeriodicThread.h b/src/System/include/BipedalLocomotion/System/PeriodicThread.h index 9b97c8e724..bcd92bc56f 100644 --- a/src/System/include/BipedalLocomotion/System/PeriodicThread.h +++ b/src/System/include/BipedalLocomotion/System/PeriodicThread.h @@ -109,6 +109,25 @@ class PeriodicThread */ bool setPolicy(int policy, int priority = 0); + /** + * @brief Set the maximum number of accepted deadline miss. + * @param maximumNumberOfAcceptedDeadlineMiss maximum number of accepted deadline miss. + */ + bool setMaximumNumberOfAcceptedDeadlineMiss(int maximumNumberOfAcceptedDeadlineMiss); + + /** + * @brief Get the number of deadline miss. + * @return number of deadline miss. + */ + int getNumberOfDeadlineMiss(); + + /** + * @brief Enable the early wake up. The thread will be awaken before and busy wait until the + * actual wake up time. + * @return true if the early wake up was correctly set, false otherwise. + */ + bool enableEarlyWakeUp(); + protected: /** * @brief This method is called at each iteration of the thread. @@ -153,12 +172,11 @@ class PeriodicThread * miss. */ - int m_deadlineMiss = 0; /**< Number of deadline miss. */ + std::atomic m_deadlineMiss = 0; /**< Number of deadline miss. */ std::chrono::nanoseconds m_wakeUpTime = std::chrono::nanoseconds(0); /**< Wake up time of the * thread. */ - int m_priority = 0; /**< Priority of the thread. */ int m_policy = SCHED_OTHER; /**< Policy of the thread. */ diff --git a/src/System/src/PeriodicThread.cpp b/src/System/src/PeriodicThread.cpp index 67246ea21c..d400dd2e36 100644 --- a/src/System/src/PeriodicThread.cpp +++ b/src/System/src/PeriodicThread.cpp @@ -115,6 +115,36 @@ bool PeriodicThread::setPeriod(std::chrono::nanoseconds period) return true; } +bool PeriodicThread::setMaximumNumberOfAcceptedDeadlineMiss(int maximumNumberOfAcceptedDeadlineMiss) +{ + if (m_state.load() != PeriodicThreadState::INACTIVE) + { + BipedalLocomotion::log()->error("[PeriodicThread::setMaximumNumberOfAcceptedDeadlineMiss] " + "The thread has already started. The maximum number of " + "accepted deadline miss cannot be changed."); + return false; + } + m_maximumNumberOfAcceptedDeadlineMiss = maximumNumberOfAcceptedDeadlineMiss; + return true; +} + +int PeriodicThread::getNumberOfDeadlineMiss() +{ + return m_deadlineMiss.load(); +} + +bool PeriodicThread::enableEarlyWakeUp() +{ + if (m_state.load() != PeriodicThreadState::INACTIVE) + { + BipedalLocomotion::log()->error("[PeriodicThread::enableEarlyWakeUp] The thread has " + "already started. The early wake up cannot be changed."); + return false; + } + m_earlyWakeUp = true; + return true; +} + bool PeriodicThread::threadInit() { return true; @@ -232,10 +262,10 @@ void PeriodicThread::advance() // check if the deadline is missed if (BipedalLocomotion::clock().now() > m_wakeUpTime) { - m_deadlineMiss++; + m_deadlineMiss.fetch_add(1); // increment the number of deadline miss if (m_maximumNumberOfAcceptedDeadlineMiss > 0) { - if (m_deadlineMiss > m_maximumNumberOfAcceptedDeadlineMiss) + if (m_deadlineMiss.load() > m_maximumNumberOfAcceptedDeadlineMiss) { // we have to close the runner m_state.store(PeriodicThreadState::STOPPED);