Combined watchdog code with latest changes.

include/sta/tacos/configs/default.hpp

@@ -1,6 +1,9 @@
 #ifndef STA_TACOS_CONFIGS_DEFAULT_HPP
 #define STA_TACOS_CONFIGS_DEFAULT_HPP
 
+// Enable the watchdog provided by TACOS.
+#define STA_TACOS_WATCHDOG_ENABLED
+
 // Generally, we assume the TACOS threads to have the highest priorties.
 #define STA_TACOS_MANAGER_PRIORITY          osPriorityHigh
 #define STA_TACOS_STATEMACHINE_PRIORITY     osPriorityHigh

include/sta/tacos/manager.hpp

@@ -17,6 +17,7 @@
 
 #include <set>
 #include <list>
+#include <vector>
 #include <memory>
 #include <sta/tacos/thread.hpp>
 
@@ -61,10 +62,16 @@ namespace sta
 			 */
 			void registerThread(std::shared_ptr<TacosThread> thread, std::set<uint16_t> states);
 
+			/**
+			 * @brief Get the Active Threads object
+			 * 
+			 * @return std::vector<std::shared_ptr<TacosThread>> 
+			 */
+			std::vector<std::shared_ptr<TacosThread>> getActiveThreads();
+
 			void init() override;
 
 			void func() override;
-
 		private:
 			static Manager* _instance;
 
@@ -107,7 +114,7 @@ namespace sta
 			 * 
 			 * @ingroup tacos_manager
 			 */
-			std::set<std::shared_ptr<TacosThread>> threads_[STA_TACOS_NUM_STATES];
+			std::vector<std::shared_ptr<TacosThread>> threads_[STA_TACOS_NUM_STATES];
 		};
 	}  // namespace tacos
 }  // namespace sta

include/sta/tacos/thread.hpp

@@ -22,6 +22,45 @@ namespace sta
 {
 	namespace tacos
 	{
+		/**
+		 * @brief A status flags for the watchdog.
+		 * 
+		 */
+		enum class ThreadStatus
+		{
+			/**
+			 * @brief This thread wants to be ignored by the watchdog.
+			 * 
+			 */
+			IGNORED,
+
+			/**
+			 * @brief The thread terminated regularly. The watchdog will not try to restart this thread.
+			 * 
+			 */
+			STOPPED,
+
+			/**
+			 * @brief The thread's status is unknown. If the watchdog encounters this status, it will try to
+			 * restart the affected thread.
+			 * 
+			 */
+			UNKNOWN,
+
+			/**
+			 * @brief The thread is running as intended. The watchdog will set this flag back to UNKNOWN.
+			 * 
+			 */
+			RUNNING,
+
+			/**
+			 * @brief The thread is waiting and might not send a heartbeat signal in a while. The watchdog will
+			 * not do anything in this case.
+			 * 
+			 */
+			WAITING
+		};
+
 		/**
 		 * @brief Abstract class for thread implementations in Tacos.
 		 *
@@ -97,8 +136,37 @@ namespace sta
 			 */
 			virtual void cleanup();
 
-		private:
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+		#define BLOCKING(stmt) waiting(); stmt heartbeat()
 
+		protected:
+			/**
+			 * @brief 
+			 * 
+			 */
+			void heartbeat();
+
+			/**
+			 * @brief Set the thread's status to waiting. Should be called before executing
+			 * 
+			 */
+			void waiting();
+		public:
+			/**
+			 * @brief Get the current status of the thread.
+			 * 
+			 * @return ThreadStatus 
+			 */
+			ThreadStatus getStatus();
+
+			/**
+			 * @brief Reset the thread's status to UNKNOWN. Should only be called by the Watchdog.
+			 * 
+			 */
+			void resetStatus();
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
+		private:
 			/**
 			 * @brief Static function to pass to RTOS to run as a thread. Calls the loop function implemented here.
 			 */
@@ -108,6 +176,9 @@ namespace sta
 			osThreadId_t instance_;
 			osThreadAttr_t attribs_;
 			bool running_;
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+			ThreadStatus status_;
+#endif // STA_TACOS_WATCHDOG_ENABLED
 		};
 	}
 }

include/sta/tacos/watchdog.hpp

@@ -0,0 +1,74 @@
+#ifndef STA_TACOS_WATCHDOG_HPP
+#define STA_TACOS_WATCHDOG_HPP
+
+#include <sta/config.hpp>
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+
+#ifndef STA_TACOS_WATCHDOG_PRIORITY
+#	error "TACOS watchdog priority was not specified!"
+#endif // STA_TACOS_WATCHDOG_PRIORITY
+
+#ifndef STA_TACOS_WATCHDOG_FREQUENCY
+#	error "TACOS watchdog frequency was not specified!"
+#endif // STA_TACOS_WATCHDOG_FREQUENCY
+
+
+#include <sta/tacos/thread.hpp>
+
+
+namespace sta
+{
+    namespace tacos
+    {
+        /**
+         * @brief Watchdog class for TACOS using singleton pattern.
+         * 
+         */
+        class Watchdog: public TacosThread
+		{
+		public:
+			/**
+			 * @brief Getter for the singleton instance. Constructs the instance if no exists.
+			 * 
+			 * @return Watchdog* 
+			 */
+			static Watchdog* instance()
+			{
+				static CGuard g;
+
+				if (!_instance)
+				{
+					// Create the watchdog singleton instance.
+					Watchdog::_instance = new Watchdog();
+				}
+
+				return _instance;
+			}
+
+			void func() override;
+		private:
+            static Watchdog* _instance;
+
+            class CGuard
+			{
+			public:
+			   ~CGuard()
+			   {
+				  if( NULL != Watchdog::_instance )
+				  {
+					 delete Watchdog::_instance;
+					 Watchdog::_instance = NULL;
+				  }
+			   }
+			};
+
+            Watchdog(const Watchdog&);
+
+			Watchdog();
+		};
+    } // namespace tacos
+} // namespace sta
+
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
+#endif // STA_TACOS_WATCHDOG_HPP

src/manager.cpp

@@ -12,6 +12,7 @@
 #include <algorithm>
 
 #include <FreeRTOS.h>
+#include <sta/tacos.hpp>
 
 
 namespace sta
@@ -24,10 +25,15 @@ namespace sta
 			{
 				STA_ASSERT(state < STA_TACOS_NUM_STATES);
 
-				threads_[state].emplace(thread);
+				threads_[state].push_back(thread);
 			}
 		}
 
+		std::vector<std::shared_ptr<TacosThread>> Manager::getActiveThreads()
+		{
+			return threads_[tacos::getState()];
+		}
+
 		void Manager::startThreads(uint16_t state)
 		{
 			STA_ASSERT(state < STA_TACOS_NUM_STATES);
@@ -55,7 +61,7 @@ namespace sta
 				for (std::shared_ptr<TacosThread> thread : threads_[other])
 				{
 					// If the thread is currently running but not part of the set of threads that should be running...
-					if (thread->isRunning() && terminated.count(thread) == 0 && threads_[state].count(thread) == 0)
+					if (thread->isRunning() && terminated.count(thread) == 0 && std::count(threads_[state].begin(), threads_[state].end(), thread) == 0)
 					{
 						// ...politely request termination.
 						thread->requestTermination();
@@ -90,13 +96,12 @@ namespace sta
 		}
 
 		Manager::Manager()
-			: TacosThread{"Manager", STA_TACOS_MANAGER_PRIORITY}
+			: TacosThread{"Manager", STA_TACOS_MANAGER_PRIORITY},
+			  threads_{}
 		{
 
 		}
 
-		//Manager::~Manager(){}
-
 		Manager* Manager::_instance = nullptr;
 
 	}  // namespace tacos

src/thread.cpp

@@ -23,12 +23,25 @@ namespace sta
 				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
 				  instance_{ NULL },
 				  attribs_{ .name=name, .cb_size=cb_size, .stack_size=stack_size, .priority=prio },
-				  running_{false}
+				  running_{false},
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+				  status_{ThreadStatus::STOPPED}
+#endif // STA_TACOS_WATCHDOG_ENABLED
 		{
 			STA_ASSERT(stack_size >= 0);
 			STA_ASSERT(cb_size >= 0);
 		}
 
+		TacosThread::TacosThread()
+				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
+				  instance_{ NULL },
+				  attribs_{ },
+				  running_{false},
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+				  status_{ThreadStatus::STOPPED}
+#endif // STA_TACOS_WATCHDOG_ENABLED
+		{}
+
 		void TacosThread::entry_point(void* arg)
 		{
 			STA_ASSERT(arg != nullptr);
@@ -90,8 +103,17 @@ namespace sta
 				// Run the thread until the termination flag is set. Reset this
 				while (!isTerminationRequested())
 				{
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+					// Send a fresh heartbeat signal.
+					heartbeat();
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
+					// Execute user-space implementation.
 					func();
 				}
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+				status_ = ThreadStatus::STOPPED;
+#endif // STA_TACOS_WATCHDOG_ENABLED
 
 				// Clear the termination request flag for this thread.
 				deleteTerminationRequest();
@@ -106,6 +128,28 @@ namespace sta
 
 		void TacosThread::cleanup() {}
 
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+		void TacosThread::heartbeat()
+		{
+			status_ = ThreadStatus::RUNNING;
+		}
+
+		void TacosThread::waiting()
+		{
+			status_ = ThreadStatus::WAITING;
+		}
+
+		ThreadStatus TacosThread::getStatus()
+		{
+			return status_;
+		}
+
+		void TacosThread::resetStatus()
+		{
+			status_ = ThreadStatus::UNKNOWN;
+		}
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
 		bool TacosThread::operator==(const TacosThread& other) const
 		{
 			return std::strcmp(this->getName(), other.getName()) == 0;

src/watchdog.cpp

@@ -0,0 +1,43 @@
+#include <sta/tacos/watchdog.hpp>
+
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+
+#include <sta/tacos/manager.hpp>
+
+namespace sta
+{
+    namespace tacos
+    {
+		void Watchdog::func()
+		{
+			for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads())
+			{
+				switch (thread->getStatus())
+				{
+				case ThreadStatus::UNKNOWN:
+					// TODO: Try to restart the thread.
+					break;
+				case ThreadStatus::RUNNING:
+					// Set the thread's status back to UNKNOWN.
+					thread->resetStatus();
+					break;
+
+				default:
+					break;
+				}
+
+				sleep(STA_TACOS_WATCHDOG_FREQUENCY);
+			}
+		}
+
+		Watchdog::Watchdog()
+			: TacosThread{"Watchdog", STA_TACOS_WATCHDOG_PRIORITY}
+		{
+
+		}
+
+		Watchdog* Watchdog::_instance = nullptr;
+    } // namespace tacos
+} // namespace sta
+
+#endif // STA_TACOS_WATCHDOG_ENABLED