diff --git a/src/statemachine.cpp b/src/statemachine.cpp
index 86077ad..8aa2d4e 100644
--- a/src/statemachine.cpp
+++ b/src/statemachine.cpp
@@ -10,6 +10,7 @@
 
 #include <sta/debug/debug.hpp>
 #include <FreeRTOS.h>
+#include <FreeRTOS.h>
 
 
 namespace sta
@@ -23,6 +24,8 @@ namespace sta
 			  failsafeTimer_{[](void *){}, nullptr},
 			  queue_{STA_TACOS_STATEMACHINE_QUEUE_LENGTH},
 			  threads_{}
+			  queue_{STA_TACOS_STATEMACHINE_QUEUE_LENGTH},
+			  threads_{}
 		{
 			STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES);
 		}
@@ -218,6 +221,66 @@ namespace sta
 			startThreads(state);
 		}
 
+		void Statemachine::registerThread(std::shared_ptr<TacosThread> thread, std::set<uint16_t> states)
+		{
+			for (uint16_t state : states)
+			{
+				STA_ASSERT(state < STA_TACOS_NUM_STATES);
+
+				threads_[state].push_back(thread);
+			}
+		}
+
+		std::vector<std::shared_ptr<TacosThread>> Statemachine::getActiveThreads()
+		{
+			return threads_[tacos::getState()];
+		}
+
+		void Statemachine::startThreads(uint16_t state)
+		{
+			STA_ASSERT(state < STA_TACOS_NUM_STATES);
+
+			for (std::shared_ptr<TacosThread> thread : threads_[state])
+			{
+				if (!thread->isRunning())
+				{
+					thread->start();
+				}
+			}
+		}
+
+		void Statemachine::stopThreads(uint16_t state)
+		{
+			std::set<std::shared_ptr<TacosThread>> terminated;
+
+			for (uint16_t other = 0; other < STA_TACOS_NUM_STATES; ++other)
+			{
+				if (other == state)
+				{
+					continue;
+				}
+
+				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 && std::count(threads_[state].begin(), threads_[state].end(), thread) == 0)
+					{
+						// ...politely request termination.
+						thread->requestTermination();
+						terminated.emplace(thread);
+					}
+				}
+			}
+		}
+
+		void Statemachine::updateThreads()
+		{
+			uint16_t state = getCurrentState();
+
+			stopThreads(state);
+			startThreads(state);
+		}
+
 		Statemachine* Statemachine::_instance = nullptr;
 
 		RtosEvent Statemachine::stateChangeEvent;