From 0f02168b25ef3b0b7459c8c31e80155b2379a7e1 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@rcor.de>
Date: Sat, 23 Sep 2023 22:42:47 +0200
Subject: [PATCH] Added modifiable callback for timers

---
 include/sta/rtos/timer.hpp | 48 +++++++++++++++++++++++++++++++++++---
 src/timer.cpp              | 34 ++++++++++++++++++++-------
 2 files changed, 70 insertions(+), 12 deletions(-)

diff --git a/include/sta/rtos/timer.hpp b/include/sta/rtos/timer.hpp
index b576078..2d0cf3d 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,16 +22,55 @@ 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
 
-#endif // STA_RTOS_TIMER_HPP
\ No newline at end of file
+#endif // STA_RTOS_TIMER_HPP
diff --git a/src/timer.cpp b/src/timer.cpp
index 398cdd3..dbaab72 100644
--- a/src/timer.cpp
+++ b/src/timer.cpp
@@ -1,28 +1,44 @@
 #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");
     }
-} // namespace sta
\ No newline at end of file
+} // namespace sta