From b79279dcbca63d517e1a0ec8fe5064b1ce702b7c Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Sat, 5 Oct 2024 23:18:24 +0200
Subject: [PATCH] Added SPATZ tick feature

---
 include/sta/debug/spatz.hpp |  9 ++++++++-
 src/debug/spatz.cpp         | 27 +++++++++++++++++++--------
 2 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/include/sta/debug/spatz.hpp b/include/sta/debug/spatz.hpp
index 0dce873..1d71f4a 100644
--- a/include/sta/debug/spatz.hpp
+++ b/include/sta/debug/spatz.hpp
@@ -24,6 +24,11 @@ namespace sta
          */
         void init(Mutex * mutex, Event * signal);
         
+        /**
+         * @brief Notify SPATZ that a new tick has happened.
+         */
+        void tick();
+
         /**
          * @brief Request bytes for a specific sensor id via the printable.
          * 
@@ -59,13 +64,15 @@ namespace sta
 
 #define STA_SPATZ_INIT(mutex, signal)	sta::spatz::init(mutex, signal)
 
+#define STA_SPATZ_TICK()				sta::spatz::tick();
+
 #else
 
 namespace sta
 {
     namespace spatz
     {
-        void init(Mutex * mutex);
+        void init(Mutex * mutex, Event * signal);
     } // namespace spatz
 } // namespace sta
 
diff --git a/src/debug/spatz.cpp b/src/debug/spatz.cpp
index ccff7e7..11318e3 100644
--- a/src/debug/spatz.cpp
+++ b/src/debug/spatz.cpp
@@ -7,6 +7,7 @@
 #endif // STA_INIT_SPATZ_ID
 
 #include <usart.h>
+#include <sta/time.hpp>
 
 namespace sta
 {
@@ -29,29 +30,42 @@ namespace sta
             }
         }
 
+        void tick()
+        {
+        	mutex_->acquire();
+        	Debug->println('~');
+        	mutex_->release();
+        }
+
         void request(uint8_t id, uint8_t * buffer, size_t length)
         {
-            sta::lock_guard<sta::Mutex> lock(*mutex_);
+            mutex_->acquire();
 
 			Debug->printf("%d", id);
 			Debug->read(buffer, length);
+
+			mutex_->release();
         }
 
         void request(uint8_t id, float * buffer, size_t length)
         {
-        	sta::lock_guard<sta::Mutex> lock(*mutex_);
+        	mutex_->acquire();
 
             HAL_UART_Receive_IT(&huart1, reinterpret_cast<uint8_t*>(buffer), length * sizeof(float));
             Debug->printf("%d", id);
-            signal_->wait(0x01);
+            signal_->wait(0x01, 100);
+
+            mutex_->release();
         }
 
         void request(uint8_t id, double * buffer, size_t length)
         {
-            sta::lock_guard<sta::Mutex> lock(*mutex_);
+        	mutex_->acquire();
 
 			Debug->printf("%d", id);
 			Debug->read(buffer, length);
+
+			mutex_->release();
         }
 
         void notify(uint8_t id)
@@ -69,22 +83,19 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
 		sta::spatz::signal_->set(0x01);
 		sta::spatz::signal_->clear(0x01);
 	}
-
 }
 
-
 #else
 
 namespace sta
 {
 	namespace spatz
 	{
-		void init(sta::Mutex * mutex)
+		void init(sta::Mutex * mutex, Event * signal)
 	    {
 	        // Nothing to do here.
 	    }
 	}  // namespace spatz
 } // namespace sta
 
-
 #endif // STA_SPATZ_ENABLED