added gy80 dcm
Dependencies: mbed DCM_AHRS_GY80 PID MMA8451Q
Fork of quadCommand by
quadCommand/com/com.cpp
- Committer:
- oprospero
- Date:
- 2013-07-28
- Revision:
- 64:2b6399fe00f6
File content as of revision 64:2b6399fe00f6:
/****************************** 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. }