diff --git a/include/sta/rtos/timer.hpp b/include/sta/rtos/timer.hpp
index 2fc06bd..d54cfeb 100644
--- a/include/sta/rtos/timer.hpp
+++ b/include/sta/rtos/timer.hpp
@@ -9,6 +9,9 @@
 #include <cmsis_os2.h>
 #include <sta/timer.hpp>
 
+#include <functional>
+
+
 namespace sta
 {
 	/**
@@ -19,17 +22,56 @@ namespace sta
 	class RtosTimer : public Timer
 	{
 	public:
-		RtosTimer();
+		/**
+		 * @brief
+		 *
+		 * @param callback 	The callback function to call upon timer timeout.
+		 * @param arg 		The argument to pass to the callback function.
+		 */
 		RtosTimer(void (*callback)(void *arg), void *arg);
-        ~RtosTimer();
 
+		/**
+		 * @brief Initializes the timer with a callback that has no effect.
+		 */
+		RtosTimer();
+
+        ~RtosTimer();
+	public:
+        /**
+         * @brief Change the timer's callback to a new function.
+         *
+         * @param callback 	The callback function to call upon timer timeout.
+		 * @param arg 		The argument to pass to the callback function.
+         */
+        void setCallback(void (*callback)(void *arg), void *arg);
+
+        /**
+         * @brief Run the timer for a given number of milliseconds.
+         *
+         * @param millis	The number of milliseconds to run the timer for.
+         */
 		void start(uint32_t millis) override;
+
+		/**
+		 * @brief Stop the timer.
+		 */
 		void stop() override;
+
+		/**
+		 * @returns True of the timmer is currently running.
+		 */
 		bool isRunning() override;
 
+	private:
+		static void timeoutHandler(void* arg);
+
 	private:
 		osTimerId_t timer_id_;                  /**< CMSIS RTOS2 Timer */
-        osTimerAttr_t timer_attr_;         /**< CMSIS RTOS2 Timer attributes */
+        osTimerAttr_t timer_attr_;         		/**< CMSIS RTOS2 Timer attributes */
+
+        std::function<void(void*)> callback_;	/**< The callback to call on timeout. */
+        void* callbackArg_;						/**< The argument to pass to the callback. */
+        bool running_;							/**< The current state of the timer. Running or not? */
 	};
 } // namespace sta
 
diff --git a/src/system/watchdog.cpp b/src/system/watchdog.cpp
index 373b349..1bf582d 100644
--- a/src/system/watchdog.cpp
+++ b/src/system/watchdog.cpp
@@ -2,6 +2,7 @@
 #ifdef STA_RTOS_WATCHDOG_ENABLE
 
 #include <sta/debug/assert.hpp>
+#include <sta/debug/debug.hpp>
 #include <sta/lang.hpp>
 #include <sta/rtos/defs.hpp>
 #include <sta/rtos/system/events.hpp>
diff --git a/src/timer.cpp b/src/timer.cpp
index 180286a..4f227b0 100644
--- a/src/timer.cpp
+++ b/src/timer.cpp
@@ -2,31 +2,74 @@
 #include <sta/debug/debug.hpp>
 #include <sta/debug/assert.hpp>
 
-namespace sta {
-	RtosTimer::RtosTimer(){}
 
-    RtosTimer::RtosTimer(void (*callback)(void *arg), void *arg) {
-        timer_id_ = osTimerNew(callback, osTimerOnce, arg, NULL);
-        STA_ASSERT_MSG(timer_id_ != NULL, "Failed to initialize timer");
+namespace sta {
+    RtosTimer::RtosTimer(void (*callback)(void *arg), void *arg)
+    	: timer_id_{NULL},
+		  timer_attr_{.name="Timer", .attr_bits=osTimerOnce},
+		  callback_{callback},
+		  callbackArg_{arg},
+		  running_{false}
+    {
+    	// Pass an anonymous function as the callback which will invoke the currently registered callback.
+        timer_id_ = osTimerNew(timeoutHandler, osTimerOnce, this, &timer_attr_);
+
+        STA_ASSERT_MSG(timer_id_ != NULL, "Failed to create timer!");
     }
 
-    RtosTimer::~RtosTimer() {
+    RtosTimer::RtosTimer()
+    	: RtosTimer([](void *){}, NULL)
+    {}
+
+    RtosTimer::~RtosTimer()
+    {
+    	if (isRunning()) stop();
+
         osTimerDelete(timer_id_);
     }
 
-    void RtosTimer::start(uint32_t millis) {
+    void RtosTimer::setCallback(void (*callback)(void *arg), void *arg)
+    {
+    	callback_ = callback;
+    	callbackArg_ = arg;
+    }
+
+    void RtosTimer::start(uint32_t millis)
+    {
         osStatus_t status = osTimerStart(timer_id_, millis);
 
-        if (status != osOK) STA_DEBUG_PRINTLN("Timer start failed");
+        if (status != osOK)
+        {
+        	STA_DEBUG_PRINTLN("Timer start failed");
+        	return;
+        }
+
+        running_ = true;
     }
 
-    void RtosTimer::stop() {
+    void RtosTimer::stop()
+    {
         osStatus_t status = osTimerStop(timer_id_);
         
-        if (status != osOK) STA_DEBUG_PRINTLN("Timer stop failed");
+        if (status != osOK)
+		{
+        	STA_DEBUG_PRINTLN("Timer stop failed");
+        	return;
+		}
+
+        running_ = false;
     }
 
-    bool RtosTimer::isRunning() {
-        return osTimerIsRunning(timer_id_);
-    }
+    bool RtosTimer::isRunning()
+    {
+		return osTimerIsRunning(timer_id_);
+	}
+
+    void RtosTimer::timeoutHandler(void* arg)
+    {
+		RtosTimer* timer = static_cast<RtosTimer*>(arg);
+		timer->running_ = false;
+		timer->callback_(timer->callbackArg_);
+	}
+
 } // namespace sta