Diff: Node/Observer/MavlinkPassthrough/MavlinkPassthrough.cpp

Revision:

0:99928431bb44

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/MavlinkPassthrough/MavlinkPassthrough.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,121 @@
+#include "MavlinkPassthrough.h"
+
+MavlinkPassthrough::MavlinkPassthrough( Serial& serial, BufferedOutput& bufferedOutput ) : mavlinkIn(serial), bufferedOutput(bufferedOutput)
+{
+    // Debug startup message
+    debugprintf("Initializing Mavlink Passthrough object\r\n") ;
+    
+    // Attaching the serial interrupt callbacks
+    mavlinkIn.attach(this, &MavlinkPassthrough::mavlinkInCallback, Serial::RxIrq ) ;
+    
+    // The state of the current mavlink message
+    mavlinkTransferState = START ;
+    currentMavlinkFrameLength = 0 ;
+    
+    // Initializing the buffer pointers
+    mavlinkBufferIn = 0 ;
+    mavlinkBufferOut = 0 ;
+    mavlinkBufferRead = 0 ;
+}
+
+void MavlinkPassthrough::executeBackgroundTask()
+{
+    // Blinking LED for debug
+    //led = !led ;
+    // Maintaing the buffer
+    maintainMavlinkBuffer() ;
+}
+
+void MavlinkPassthrough::mavlinkInCallback()
+{
+    // Reading out all chars in the USART buffer in the mavlink buffer
+    // while the mavlink buffer is not full
+    while( mavlinkIn.readable() && !((( mavlinkBufferIn + 1 ) % mavlinkBufferSize) == mavlinkBufferOut ) )
+    {
+        // Taking character from device and putting the buffer
+        mavlinkBuffer[ mavlinkBufferIn ] = mavlinkIn.getc() ;
+        // Incrementing the buffer pointer 
+        mavlinkBufferIn = ( mavlinkBufferIn + 1 ) % mavlinkBufferSize ;
+    }
+}
+
+void MavlinkPassthrough::maintainMavlinkBuffer()
+{
+    // Switching actions depending on the mavlink transfer case
+    switch( mavlinkTransferState )
+    {
+        case START :
+            // Searching for start character until end of mavlink buffer
+            while( !( mavlinkBufferRead == mavlinkBufferIn ) )
+            {
+                //debugprintf("Buffer not empty searching for start char\r\n") ;
+                // Detect the start character
+                if( mavlinkBuffer[mavlinkBufferRead] == MAVLINKSTARTCHAR )
+                {
+                    //debugprintf("Start character found. Switching state\r\n") ;
+                    // Changing state, advancing read pointer and stopping search
+                    mavlinkTransferState = LENGTH ;
+                    debugprintf("Mavlink state LENGTH\r\n") ;
+                    mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+                    break ;
+                }
+                // Else advance the read and output pointer and continue the search
+                else
+                {
+                    mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+                    mavlinkBufferOut = ( mavlinkBufferOut + 1 ) % mavlinkBufferSize ;
+                }   
+            }
+            break ;
+        case LENGTH :
+            // Checking if there's another byte in the mavlink buffer
+            if( !( mavlinkBufferRead == mavlinkBufferIn ) )
+            {
+                // Reading the character as the datalength
+                currentMavlinkFrameLength = mavlinkBuffer[mavlinkBufferRead] + MAVLINKADDITIONALDATA ;
+                //debugprintf("Length character found: %u\r\n", currentMavlinkFrameLength ) ;
+                // Advancing the read pointer and changing the state
+                mavlinkTransferState = DATA ;
+                debugprintf("Mavlink state DATA\r\n") ;
+                mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+            }
+            break ;
+        case DATA :
+            // Looping while there are still data bits to be read
+            while( !( mavlinkBufferRead == mavlinkBufferIn ) )
+            {
+                // Incrementing the read pointer and decrementing the data bits left counter
+                mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+                currentMavlinkFrameLength -= 1 ;
+                //debugprintf("Databits left: %u\r\n", currentMavlinkFrameLength) ;
+                // If no databits left move to the next state
+                if( currentMavlinkFrameLength == 0 )
+                {
+                    // Advancing the read pointer and changing the state
+                    mavlinkTransferState = TRANSFER ;
+                    debugprintf("Mavlink state TRANSFER\r\n") ;
+                    break ;
+                }
+            }
+            break ;
+        case TRANSFER :
+            
+            // Disabling while accessing shared output buffer
+            __disable_irq() ;
+            // Looping until all characters moved to the transferBuffer frame
+            while( !( mavlinkBufferOut == mavlinkBufferRead ) )
+            {
+                // Transfering a character to the frames buffer
+                bufferedOutput.addChar( mavlinkBuffer[mavlinkBufferOut] ) ;
+                // Incrementing the mavlinkOut pointer
+                mavlinkBufferOut = ( mavlinkBufferOut + 1 ) % mavlinkBufferSize ;
+            }
+            // Re-enabling interrupts
+            __enable_irq() ;
+            
+            // Changing back to start state
+            mavlinkTransferState = START ;
+            debugprintf("Mavlink state START\r\n") ;
+            break ;
+    }
+}
\ No newline at end of file