Diff: Node/Observer/Observer.cpp

Revision:

0:99928431bb44

diff -r 000000000000 -r 99928431bb44 Node/Observer/Observer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/Observer.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,64 @@
+#include "Observer.h"
+
+Observer::Observer( MM2WayRanging& newRanging, DW1000& newDw, Serial& output, Serial& mavlinkIn ) : Node( newRanging, newDw ), bufferedOutput( output ), mavlinkPassthrough( mavlinkIn, bufferedOutput )
+{
+    debugprintf("Observer Initialized\r\n") ;
+    // Registering the callback for observer functionality
+    dw.setCallbacks(this, &Observer::observerCallbackRX, &Observer::observerCallbackTX);   
+    //dw.setCallbacks(this, &Observer::observerCallbackRX, NULL);   
+}
+
+void Observer::execute()
+{
+    // Executing the passthrough background task
+    mavlinkPassthrough.executeBackgroundTask() ;
+    // Executing the frames output background task
+    bufferedOutput.executeBackgroundTask() ;
+}
+
+void Observer::observerCallbackRX()
+{   
+    // Creating a distance frame to hold received message
+    DistancesFrame distFrame ;
+    // Reading received message into distance frame
+    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&distFrame, dw.getFramelength()) ;
+       
+    // If message directed at this node
+    if ( distFrame.destination == ranging.address )
+    {
+        // Disabling while accessing shared output buffer
+        __disable_irq() ;
+        
+        // A float for debug
+        //volatile float debugFloat = 0.00 ;
+        
+        // Printing the heading code
+        bufferedOutput.addChar( HEADERCODE ) ;
+        // Printing the message length
+        bufferedOutput.addChar( 4*sizeof(float) ) ;
+        // Looping through and printing received distance
+        for( int nodeIndex=1; nodeIndex<=4; nodeIndex++ )
+        {
+            // Retreving address of distance as char address
+            char *pdist = (char *) &distFrame.dist[nodeIndex-1] ;
+            //debugFloat ++ ;
+            //char *pdist = (char *) &debugFloat ;
+            // Looping over the bytes in the float and sending
+            for( char index = 0 ; index < sizeof(float) ; index++ )
+            {
+                 bufferedOutput.addChar( pdist[ index ] ) ;
+            }
+        }
+        
+        // Re-enabling interrupts
+        __enable_irq() ;
+    }
+    // Starting receive again
+    dw.startRX();
+}
+
+void Observer::observerCallbackTX()
+{
+    // Debug output
+    //debugprintf("Message sent\r\n") ;
+}
\ No newline at end of file