Combined watchdog code with latest changes.

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