added gy80 dcm
Dependencies: mbed DCM_AHRS_GY80 PID MMA8451Q
Fork of quadCommand by
Diff: quadCommand/com/com.cpp
- Revision:
- 64:2b6399fe00f6
diff -r f4cb482ac5d4 -r 2b6399fe00f6 quadCommand/com/com.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/quadCommand/com/com.cpp Sun Jul 28 21:46:54 2013 +0000 @@ -0,0 +1,142 @@ +/****************************** 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( short command, short value ) +{ + xbee.putc( (char)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() ********************************/ +/* */ +/*************************************************************************/ + +short * com::read() +{ + if( !rxBuffer->isEmpty()) + return rxBuffer->pop(); + + return NULL; +} + +/************************ void callback() ********************************/ +/* */ +/*************************************************************************/ + +void com::callback() +{ + while( xbee.readable() ) + { + char data = xbee.getc(); +// xbee.printf("data: %d\n\r", data); + if( bLength++ < BUFFERSIZE ) + 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. Max value of +/- 8063. */ +/*************************************************************************/ + +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 7 bits. + commandData[1] = buffer[--bLength]; // Fisrt 7 bits. + commandData[0] = buffer[--bLength]; // Command. + + if( commandData[0] + commandData[2] == commandData[3] ) // Validate checksum. + { + short * array = new short[2]; + + array[0] = (short)commandData[0]; + + short value = (short)(commandData[1] * 128 + commandData[2]); + + if( value > 8062 ) + value = (short)value + 57344; + + array[1] = value; +// xbee.printf("Cmd: %d,\tVal: %d\n\r,",array[0],array[1]); + rxBuffer->add( array ); // Add to read buffer. + write( 0, (short)commandData[4]); // Ack the packet with sequence nuber. + } + delete[] commandData; + bLength = 0; // Reset the buffer length. +} + + + + + + + + + + +