Derek McLean / Mbed 2 deprecated uwb-quadcopter

Dependencies:   mbed ESC SR04 TSI

com/com.cpp

Committer:
gabdo
Date:
2013-06-09
Revision:
32:d2b973c8d196

File content as of revision 32:d2b973c8d196:

/****************************** com.cpp **********************************/
/* Version: 1.0                                                          */
/* Last Updated: June 1, 2013                                            */
/*                                                                       */
/* The com class implements reliable data transfer between two nodes     */
/*using a checksum and a sequence number for guaranteed message delivery */
/*over an xbee modem connected to the passed in tx and rx pins. Messages */
/*are received and placed in the rxBuffer to be read when convenient.    */
/*Messages are encoded by sending a byte with the value of the command   */
/*then and int of the command.                                           */
/*                                                                       */
/* Commands:    0 -> Ack, does not get placed in rxQueue.                */
/*              1 -> Throttle                                            */
/*              2 -> Pitch                                               */
/*              3 -> Roll                                                */
/*              4 -> Yaw                                                 */
/*************************************************************************/

#include "mbed.h"
#include "com.h"

/*********************** com( PinName, PinName ) *************************/
/*                                                                       */
/*************************************************************************/

com::com( PinName tx, PinName rx ) : xbee( tx, rx)
{
    bLength = 0;                            // How many bytes are in the buffer.
    xbee.attach( this, &com::callback );    // Set callback as the interrupt handler. 
    xbee.baud(BAUDRATE);                    // Setup the serial baud rate.
    rxBuffer = new queue();                     // Point to the rxQueue.

}

/************************* bool isData()  ********************************/
/*                                                                       */
/*************************************************************************/

bool com::isData()
{
    if( rxBuffer->isEmpty() )
        return false;
        
    return true;
}

/************************ void write( char ) *****************************/
/* Write a packet out the xbee com port.                                 */
/*                                                                       */
/* Data format byte[]                                                    */
/*                byte[0] = command.                                     */
/*                byte[1] = upper 8 bits of value.                       */
/*                byte[2] = lower 8 bits of value.                       */
/*                byte[3] = Checksum byte[0] + byte[2].                  */
/*                byte[4] = Sequence Number.                             */
/*                byte[5] = 255 End of message.                          */
/*************************************************************************/

void com::write( char command, int value  )
{
    xbee.putc( command );           // Command
    xbee.putc( 0 );                 // First 8 bits in array. 
    xbee.putc( (char)value );       // Second 8 bits in array.
    xbee.putc( command + value );   // Checksum array[0] + array[1].
    xbee.putc( 0 );                 // Sequence number.
    xbee.putc( 255 );               // End of message.
}

/*************************** char read()  ********************************/
/*                                                                       */
/*************************************************************************/

char * com::read()
{
    // Are there commands in the readBuffer queue?    
    if( !rxBuffer->isEmpty())
        return rxBuffer->pop();
        
    return NULL;
}

/************************ void callback() ********************************/
/*                                                                       */
/*************************************************************************/

void com::callback()
{   
    while( xbee.readable() )
    {
        char data = xbee.getc(); 
        if( bLength++ < 15 )
            buffer[bLength] = data;      
     
        if( data == 255 )
            packetBuilder();
    }
}

/********************** void packetBuilder() *****************************/
/* Creates a packet from the buffered data and places it in the rxBuffer */
/*queue to be read whenever convenient.                                  */
/*************************************************************************/

void com::packetBuilder()
{
    char * commandData = new char[bLength];
    commandData[4] = buffer[--bLength];     // Sequence Number.
    commandData[3] = buffer[--bLength];     // CheckSum value.
    commandData[2] = buffer[--bLength];     // Second 8 bits.
    commandData[1] = buffer[--bLength];     // Fisrt 8 bits.
    commandData[0] = buffer[--bLength];     // Command.
            
    if( commandData[0] + commandData[2] == commandData[3] ) // Validate checksum.
    {
        rxBuffer->add( commandData );   // Add to read buffer.
        write( 0, (int)commandData[4]); // Ack the packet with sequence nuber.
    } 
    bLength = 0;    // Reset the buffer length.                                           
}