Updated spatz implementation

include/sta/debug/printing/printable.hpp

@@ -201,15 +201,6 @@ namespace sta
          * @param length    Buffer length
          */
         void read(double * buffer, size_t length);
-    public:
-        virtual void request(uint8_t * txBuffer, size_t txLength, uint8_t * rxBuffer, size_t rxLength) = 0;
-
-        void request(uint8_t * txBuffer, size_t txLength, float * rxBuffer, size_t rxLength);
-
-        void request(uint8_t * txBuffer, size_t txLength, double * rxBuffer, size_t rxLenght);
-
-        
-
     private:
         /**
          * @brief Print unsigned integer in selected base.

include/sta/debug/spatz.hpp

@@ -1,6 +1,8 @@
 #ifndef STA_CORE_SPATZ_HPP
 #define STA_CORE_SPATZ_HPP
 
+#include <sta/mutex.hpp>
+
 #include <sta/config.hpp>
 #ifdef STA_SPATZ_ENABLED
 
@@ -9,8 +11,6 @@
 #   error "Cannot use SPATZ without debugging being enabled."
 #endif // STA_DEBUGGING_ENABLED
 
-#include <sta/mutex.hpp>
-
 namespace sta
 {
     namespace spatz
@@ -51,6 +51,16 @@ namespace sta
     } // namespace spatz
 } // namespace sta
 
+#else
+
+namespace sta
+{
+    namespace spatz
+    {
+        void init(Mutex * mutex);
+    } // namespace spatz
+} // namespace sta
+
 #endif // STA_SPATZ_ENABLED
 
 #endif // STA_CORE_SPATZ_HPP

src/debug/spatz.cpp

@@ -2,6 +2,10 @@
 
 #ifdef STA_SPATZ_ENABLED
 
+#ifndef STA_INIT_SPATZ_ID
+#	error "The ID used to communicate SPATZ initialization was not defined."
+#endif // STA_INIT_SPATZ_ID
+
 namespace sta
 {
     namespace spatz
@@ -11,32 +15,57 @@ namespace sta
         void init(sta::Mutex * mutex)
         {
             mutex_ = mutex;
+            uint8_t msg = 0xFF;
+
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
+
+            // Wait until SPATZ sends the init byte.
+            while (msg != STA_INIT_SPATZ_ID)
+            {
+            	Debug->read(&msg, 1);
+            }
         }
 
         void request(uint8_t id, uint8_t * buffer, size_t length)
         {
-            sta::lock_guard lock(*mutex_);
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
 
-            Debug->println(id);
+            Debug->println("%d", id);
             Debug->read(buffer, length);
         }
 
         void request(uint8_t id, float * buffer, size_t length)
         {
-            sta::lock_guard lock(*mutex_);
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
 
-            Debug->println(id);
+            // Debug->println(id, IntegerBase::DEC);
+
+            Debug->printf("%d", id);
             Debug->read(buffer, length);
         }
 
         void request(uint8_t id, double * buffer, size_t length)
         {
-            sta::lock_guard lock(*mutex_);
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
 
-            Debug->println(id);
+            Debug->println("%d", id);
             Debug->read(buffer, length);
         }
     } // namespace spatz    
 } // namespace sta
 
+#else
+
+namespace sta
+{
+	namespace spatz
+	{
+		void init(sta::Mutex * mutex)
+	    {
+	        // Nothing to do here.
+	    }
+	}  // namespace spatz
+} // namespace sta
+
+
 #endif // STA_SPATZ_ENABLED