Merge pull request 'Watchdog' (#19) from watchdog-integration into main

Reviewed-on: https://git.intern.spaceteamaachen.de/ALPAKA/TACOS/pulls/19
Reviewed-by: carlwachter <carlwachter@noreply.git.intern.spaceteamaachen.de>

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
@@ -9,4 +12,4 @@
 // Per default, we assume state 0 to be the initial state.
 #define STA_TACOS_INITIAL_STATE             0
 
-#endif // STA_TACOS_CONFIGS_DEFAULT_HPP
+#endif // STA_TACOS_CONFIGS_DEFAULT_HPP

include/sta/tacos/debug.hpp

@@ -0,0 +1,28 @@
+#ifndef STA_TACOS_DEBUG_HPP
+#define STA_TACOS_DEBUG_HPP
+
+#include <sta/tacos/watchdog.hpp>
+
+/**
+ * @defgroup tacos_debugging TACOS Debugging
+ * @brief Functions and classes that are used to debugging TACOS.
+ * 
+ * @details This module contains all functions and classes that are used for debugging TACOS. This is mostly used for writing automated tests.
+ */
+
+namespace sta
+{
+    namespace tacos
+    {
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+		/**
+		 * @return uint16_t Returns the number thread restarts performed by the watchdog. Can be used for debugging and testing. 
+		 * 
+		 * @ingroup tacos_debugging
+		 */
+		uint16_t getNumThreadRestarts();
+#endif // STA_TACOS_WATCHDOG_ENABLED
+    } // namespace tacos
+} // namespace sta
+
+#endif // STA_TACOS_DEBUG_HPP

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

@@ -10,6 +10,7 @@
 
 #include <cmsis_os2.h>
 
+#include <sta/config.hpp>
 #include <sta/rtos/thread.hpp>
 
 /**
@@ -22,6 +23,48 @@ namespace sta
 {
 	namespace tacos
 	{
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+		/**
+		 * @brief A status flags for the watchdog.
+		 * 
+		 * @ingroup tacos_thread
+		 */
+		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
+		};
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
 		/**
 		 * @brief Abstract class for thread implementations in Tacos.
 		 *
@@ -42,31 +85,11 @@ namespace sta
 
 			virtual ~TacosThread();
 
-			/**
-			 * @brief Start the execution of this thread.
-			 */
-			void start();
-
 			/**
 			 * @brief Checks if this thread is currently running.
 			 */
 			bool isRunning();
 
-			/**
-			 * @brief Get the currently running instance.
-			 * 
-			 * @return The currently running instance id.
-			 */
-			osThreadId_t getInstance();
-
-			/**
-			 * @brief Get the name of this thread.
-			 */
-			const char* getName() const;
-
-			/**
-			 * @brief Compare two threads by their names.
-			 */
 			bool operator==(const TacosThread& other) const;
 
 			/**
@@ -74,11 +97,9 @@ namespace sta
 			 */
 			bool operator<(const TacosThread& other) const;
 
-			/**
-			 * @brief A function that wraps this task's functionality in a loop. This loop will run until
-			 * termination is requested.
-			 */
-			void loop();
+			void loop() override;
+
+			void start() override;
 
 			/**
 			 * @brief This function is executed first when this thread is started.
@@ -97,17 +118,81 @@ namespace sta
 			 */
 			virtual void cleanup();
 
-		private:
+			/**
+			 * @brief Sleep for a given number of ticks. Sets itself to WAITING if the watchdog is enabled, preventing
+			 * the watchdog from restarting this thread.
+			 * 
+			 * @param ticks 
+			 */
+			void sleep(uint32_t ticks);
+
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+			/**
+			 * @brief This macro wraps a given statement into waiting() and heartbeat() to make the code more readable.
+			 * 
+			 */
+			#define blocking(...) \
+				waiting(); \
+				__VA_ARGS__ \
+				heartbeat(); \
+
+		protected:
+			/**
+			 * @brief Sends a heartbeat signal by setting the thread status to RUNNING
+			 * 
+			 */
+			void heartbeat();
 
 			/**
-			 * @brief Static function to pass to RTOS to run as a thread. Calls the loop function implemented here.
+			 * @brief Set the thread's status to waiting. This can called before executing code which is very likely
+			 * to exceed a watchdog cycle. This stops the watchdog from restarting the thread.
+			 * 
 			 */
-			static void entry_point(void* arg);
+			void waiting();
+		public:
+			/**
+			 * @brief Get the current status of the thread.
+			 * 
+			 * @return ThreadStatus The current status of the thread.
+			 */
+			ThreadStatus getStatus();
 
+			/**
+			 * @brief Reset the thread's status to UNKNOWN. Should only be called by the Watchdog.
+			 * 
+			 */
+			void resetStatus();
+
+			/**
+			 * @brief This thread status tells the watchdog to ignore this thread.
+			 */
+			void watchdogIgnore();
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
+			/**
+			 * @brief Send termination request to thread.
+			 */
+			void requestTermination();
+
+			/**
+			 * @brief Clear the termination request flag for this thread.
+			 */
+			void deleteTerminationRequest();
+
+			/**
+			 * @brief Resets the terminate bool to false.
+			 *
+			 * @return Returns the previous value of this variable.
+			 */
+			bool isTerminationRequested();
 		private:
 			osThreadId_t instance_;
 			osThreadAttr_t attribs_;
 			bool running_;
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+			ThreadStatus status_;
+#endif // STA_TACOS_WATCHDOG_ENABLED
+			bool terminate_;
 		};
 	}
 }

include/sta/tacos/watchdog.hpp

@@ -0,0 +1,92 @@
+#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>
+
+
+/**
+ * @defgroup tacos_watchdog Watchdog Task
+ * @ingroup tacos
+ * @brief Watchdog class for TACOS.
+ */
+
+namespace sta
+{
+    namespace tacos
+    {
+        /**
+         * @brief Watchdog class for TACOS using singleton pattern.
+         * 
+		 * @ingroup tacos_watchdog
+         */
+        class Watchdog: public TacosThread
+		{
+		public:
+			/**
+			 * @brief Getter for the singleton instance. Constructs the instance if no exists.
+			 * 
+			 * @ingroup tacos_watchdog
+			 */
+			static Watchdog* instance()
+			{
+				static CGuard g;
+
+				if (!_instance)
+				{
+					// Create the watchdog singleton instance.
+					Watchdog::_instance = new Watchdog();
+				}
+
+				return _instance;
+			}
+
+			void func() override;
+
+			/**
+			 * @brief Get the number of thread restarts during the program's runtime.
+			 * 
+			 * @return uint16_t The number of thread restarts. 
+			 */
+			uint16_t getNumRestarts();
+		private:
+            static Watchdog* _instance;
+
+            class CGuard
+			{
+			public:
+			   ~CGuard()
+			   {
+				  if( NULL != Watchdog::_instance )
+				  {
+					 delete Watchdog::_instance;
+					 Watchdog::_instance = NULL;
+				  }
+			   }
+			};
+
+            Watchdog();
+
+			Watchdog(const Watchdog&);
+
+			~Watchdog() {}
+		private:
+			uint16_t restarts_;
+		};
+    } // namespace tacos
+} // namespace sta
+
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
+#endif // STA_TACOS_WATCHDOG_HPP

src/debug.cpp

@@ -0,0 +1,15 @@
+#include <sta/tacos/debug.hpp>
+
+
+namespace sta
+{
+    namespace tacos
+    {
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+        uint16_t getNumThreadRestarts()
+        {
+            return Watchdog::instance()->getNumRestarts();
+        }
+#endif // STA_TACOS_WATCHDOG_ENABLED
+    } // namespace tacos
+} // namespace sta

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/startup.cpp

@@ -28,6 +28,7 @@
 // Tacos-specific includes.
 #include <sta/tacos/manager.hpp>
 #include <sta/tacos/statemachine.hpp>
+#include <sta/tacos/watchdog.hpp>
 
 
 // The UART mutex defined in freertos.c
@@ -72,8 +73,6 @@ namespace sta
 			UARTSettings settings = { .mode =  UARTMode::RX_TX };
 			STM32UART * intf_ptr = new STM32UART(getUARThandle(), settings, mutex);
 			Debug = new PrintableUART(intf_ptr);
-
-			STA_DEBUG_PRINTLN("UART SUCCESSFULLY INITIALIZED");
 		}
 	}
 #endif // STA_DEBUGGING_ENABLED
@@ -113,6 +112,19 @@ namespace sta
 
 			Manager::instance()->start();
 		}
+
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+		STA_WEAK
+		void onWatchdogInit()
+		{}
+
+		void initWatchdog()
+		{
+			onWatchdogInit();
+
+			Watchdog::instance()->start();
+		}
+#endif // STA_TACOS_WATCHDOG_ENABLED
 	}  // namespace tacos
 
 
@@ -128,6 +140,10 @@ namespace sta
 			tacos::initStatemachine();
 
 			tacos::initManager();
+
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+			tacos::initWatchdog();
+#endif // STA_TACOS_WATCHDOG_ENABLED
 		}
 	} // namespace rtos
 } // namespace sta

src/thread.cpp

@@ -20,54 +20,34 @@ namespace sta
 	namespace tacos
 	{
 		TacosThread::TacosThread(const char* name, osPriority_t prio, uint32_t stack_size /* = 0 */, uint32_t cb_size /* = 0 */)
-				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
+				: RtosThread(name, prio, stack_size, cb_size),
 				  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
+				  terminate_{false}
 		{
 			STA_ASSERT(stack_size >= 0);
 			STA_ASSERT(cb_size >= 0);
 		}
 
-		void TacosThread::entry_point(void* arg)
-		{
-			STA_ASSERT(arg != nullptr);
-
-			TacosThread* instance = reinterpret_cast<TacosThread*>(arg) ;
-			instance->loop();
-		}
-
-		void TacosThread::start()
-		{
-			STA_ASSERT(!isRunning());
-
-			// If this is the first time starting the thread, it has to be started via rtos first.
-			if (instance_ == NULL)
-			{
-				instance_ = osThreadNew(entry_point, this, &attribs_);
-
-				STA_ASSERT(instance_ != NULL);
-			}
-
-			// Send a thread start signal.
-			sysNotify(STA_RTOS_THREAD_FLAG_START);
-		}
-
 		bool TacosThread::isRunning()
 		{
 			return running_;
 		}
 
-		osThreadId_t TacosThread::getInstance()
+		void TacosThread::start()
 		{
-			STA_ASSERT(isRunning());
-
-			return instance_;
-		}
-
-		const char* TacosThread::getName() const
-		{
-			return attribs_.name;
+			if (getInstance() == NULL)
+			{
+				RtosThread::start();
+			}
+			else
+			{
+				sysNotify(STA_RTOS_THREAD_FLAG_START);
+			}
 		}
 
 		void TacosThread::init() {}
@@ -90,8 +70,20 @@ namespace sta
 				// Run the thread until the termination flag is set. Reset this
 				while (!isTerminationRequested())
 				{
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+					if (status_ == ThreadStatus::UNKNOWN)
+					{
+						// 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 +98,61 @@ namespace sta
 
 		void TacosThread::cleanup() {}
 
+		void TacosThread::sleep(uint32_t ticks)
+		{
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+			waiting();
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
+			osDelay(ticks);
+
+#ifdef STA_TACOS_WATCHDOG_ENABLED
+			heartbeat();
+#endif // STA_TACOS_WATCHDOG_ENABLED
+		}
+
+#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;
+		}
+
+		void TacosThread::watchdogIgnore()
+		{
+			status_ = ThreadStatus::IGNORED;
+		}
+#endif // STA_TACOS_WATCHDOG_ENABLED
+
+		void TacosThread::requestTermination()
+		{
+			terminate_ = true;
+		}
+
+		void TacosThread::deleteTerminationRequest()
+		{
+			terminate_ = false;
+		}
+
+		bool TacosThread::isTerminationRequested()
+		{
+			return terminate_;
+		}
+
 		bool TacosThread::operator==(const TacosThread& other) const
 		{
 			return std::strcmp(this->getName(), other.getName()) == 0;

src/watchdog.cpp

@@ -0,0 +1,49 @@
+#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:
+					// Restart the thread.
+					thread->kill();
+					thread->start();
+					
+					restarts_++;
+					break;
+				case ThreadStatus::RUNNING:
+					// Set the thread's status back to UNKNOWN.
+					thread->resetStatus();
+					break;
+				default:
+					break;
+				}
+			}
+			sleep(STA_TACOS_WATCHDOG_FREQUENCY);
+		}
+
+		uint16_t Watchdog::getNumRestarts()
+		{
+			return restarts_;
+		}
+
+		Watchdog::Watchdog()
+			: TacosThread{"Watchdog", STA_TACOS_WATCHDOG_PRIORITY},
+			  restarts_{ 0 }
+		{}
+
+		Watchdog* Watchdog::_instance = nullptr;
+    } // namespace tacos
+} // namespace sta
+
+#endif // STA_TACOS_WATCHDOG_ENABLED