Added modifiable callback for timers

include/sta/rtos/timer.hpp

@@ -9,6 +9,9 @@
 #include <cmsis_os2.h>
 #include <sta/timer.hpp>
 
+#include <functional>
+
+
 namespace sta
 {
 	/**
@@ -19,15 +22,54 @@ namespace sta
 	class RtosTimer : public Timer
 	{
 	public:
+		/**
+		 * @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;
 
+	private:
+		static void timeoutHandler(void* arg)
+		{
+			RtosTimer* timer = static_cast<RtosTimer*>(arg);
+			timer->callback_(timer->callbackArg_);
+		}
+
 	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. */
 	};
 } // namespace sta
 

src/timer.cpp

@@ -1,26 +1,42 @@
 #include <sta/rtos/timer.hpp>
 #include <sta/debug/debug.hpp>
+#include <sta/debug/assert.hpp>
+
 
 namespace sta {
-    RtosTimer::RtosTimer(void (*callback)(void *arg), void *arg) {
-        timer_attr_.name = "Timer";
-        timer_attr_.attr_bits = osTimerOnce;
-        timer_attr_.cb_size = sizeof(osTimerAttr_t);
+    RtosTimer::RtosTimer(void (*callback)(void *arg), void *arg)
+    	: timer_id_{NULL}, timer_attr_{.name="Timer", .attr_bits=osTimerOnce, .cb_size=sizeof(osTimerAttr_t)}, callback_{callback}, callbackArg_{arg}
+    {
+    	// Pass an anonymous function as the callback which will invoke the currently registered callback.
+        timer_id_ = osTimerNew(timeoutHandler, osTimerOnce, arg, &timer_attr_);
 
-        timer_id_ = osTimerNew(callback, osTimerOnce, arg, &timer_attr_);
+        STA_ASSERT_MSG(timer_id_ != NULL, "Failed to create timer!");
     }
 
-    RtosTimer::~RtosTimer() {
+    RtosTimer::RtosTimer()
+    	: RtosTimer([](void *){}, NULL)
+    {}
+
+    RtosTimer::~RtosTimer()
+    {
         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");
     }
 
-    void RtosTimer::stop() {
+    void RtosTimer::stop()
+    {
         osStatus_t status = osTimerStop(timer_id_);
         
         if (status != osOK) STA_DEBUG_PRINTLN("Timer stop failed");