Updated spatz implementation

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
 
-#endif // STA_SPATZ_ENABLED
+#else
+
+namespace sta
+{
+	namespace spatz
+	{
+		void init(sta::Mutex * mutex)
+	    {
+	        // Nothing to do here.
+	    }
+	}  // namespace spatz
+} // namespace sta
+
+
+#endif // STA_SPATZ_ENABLED