File content as of revision 0:99928431bb44:

#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 ;
    }
}