Prosper Van / xbeeCom

xbee communication for UWB quadcopter project Originally by Greg Abdo Forking to reduce impact of interrupt by moving packetbuilder out of the interrupt and letting be handled in the main loop

Fork of com by Prosper Van

com.cpp@14:d2acb373d81c, 2014-05-25 (annotated)

Committer:
oprospero
Date:
Sun May 25 22:54:45 2014 +0000
Revision:
14:d2acb373d81c
Parent:
13:2fb7b19dcd70
Child:
15:3f742edaa359
changed buffer size to power of 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oprospero 0:26a151d2c6db 1 /****************************** com.cpp **********************************/
oprospero 0:26a151d2c6db 2 /* Version: 1.0 */
oprospero 0:26a151d2c6db 3 /* Last Updated: June 1, 2013 */
oprospero 0:26a151d2c6db 4 /* */
oprospero 0:26a151d2c6db 5 /* The com class implements reliable data transfer between two nodes */
oprospero 0:26a151d2c6db 6 /*using a checksum and a sequence number for guaranteed message delivery */
oprospero 0:26a151d2c6db 7 /*over an xbee modem connected to the passed in tx and rx pins. Messages */
oprospero 0:26a151d2c6db 8 /*are received and placed in the rxBuffer to be read when convenient. */
oprospero 0:26a151d2c6db 9 /*Messages are encoded by sending a byte with the value of the command */
oprospero 0:26a151d2c6db 10 /*then and int of the command. */
oprospero 0:26a151d2c6db 11 /* */
oprospero 0:26a151d2c6db 12 /* Commands: 0 -> Ack, does not get placed in rxQueue. */
oprospero 0:26a151d2c6db 13 /* 1 -> Throttle */
oprospero 0:26a151d2c6db 14 /* 2 -> Pitch */
oprospero 0:26a151d2c6db 15 /* 3 -> Roll */
oprospero 0:26a151d2c6db 16 /* 4 -> Yaw */
oprospero 0:26a151d2c6db 17 /*************************************************************************/
oprospero 0:26a151d2c6db 18
oprospero 0:26a151d2c6db 19 #include "com.h"
oprospero 13:2fb7b19dcd70 20 #ifdef DEBUG_COM
oprospero 13:2fb7b19dcd70 21 Timer timerCom;
oprospero 13:2fb7b19dcd70 22 #define NL "\n\r"
oprospero 13:2fb7b19dcd70 23 #define PRINT(x) xbeeTx.printf(x) //Serial.print(x)
oprospero 13:2fb7b19dcd70 24 #define PRINTF(x, y) xbeeTx.printf( (x) ,(y) ) //Serial.print(x, y)
oprospero 13:2fb7b19dcd70 25 #define PRINTLN(x) PRINT(x);PRINT(NL)
oprospero 13:2fb7b19dcd70 26 #define PRINTLNF(x, y) PRINTF((x) ,(y));PRINT(NL)
oprospero 13:2fb7b19dcd70 27 #define START timerCom.start();
oprospero 13:2fb7b19dcd70 28 #define STOP timerCom.stop()
oprospero 13:2fb7b19dcd70 29 #define RESET timerCom.reset()
oprospero 13:2fb7b19dcd70 30 #define READ timerCom.read_us()
oprospero 13:2fb7b19dcd70 31 #define GET(x) x = READ
oprospero 13:2fb7b19dcd70 32 #else
oprospero 13:2fb7b19dcd70 33 #define PRINT(x)
oprospero 13:2fb7b19dcd70 34 #define PRINTF(x, y)
oprospero 13:2fb7b19dcd70 35 #define PRINTLN(x)
oprospero 13:2fb7b19dcd70 36 #define PRINTLNF(x, y)
oprospero 13:2fb7b19dcd70 37 #define START
oprospero 13:2fb7b19dcd70 38 #define STOP
oprospero 13:2fb7b19dcd70 39 #define RESET
oprospero 13:2fb7b19dcd70 40 #define READ
oprospero 13:2fb7b19dcd70 41 #define GET(x)
oprospero 13:2fb7b19dcd70 42 #endif
oprospero 0:26a151d2c6db 43
oprospero 1:4f53de75bc96 44
oprospero 0:26a151d2c6db 45 /*********************** com( PinName, PinName ) *************************/
oprospero 0:26a151d2c6db 46 /* */
oprospero 0:26a151d2c6db 47 /*************************************************************************/
oprospero 0:26a151d2c6db 48
oprospero 2:bd1621102cad 49 com::com( PinName tx, PinName rx , PinName rssipin) : xbeeTx( tx, NC), xbeeRx( NC, rx), rssi(rssipin)
oprospero 0:26a151d2c6db 50 {
oprospero 13:2fb7b19dcd70 51 index1 = 0; // How many bytes are in the buffer.
oprospero 13:2fb7b19dcd70 52 index2 = 0; // How many bytes are in the buffer.
oprospero 13:2fb7b19dcd70 53 pindex = 0; // How many bytes are in the buffer.
oprospero 13:2fb7b19dcd70 54 rdy2build = false; // Xbee is in transparent mode.
oprospero 13:2fb7b19dcd70 55 xbeeTx.baud(BAUDRATE); // Setup the serial baud rate.
oprospero 13:2fb7b19dcd70 56 xbeeRx.baud(BAUDRATE); // Setup the serial baud rate.
oprospero 5:7cdf299ea7a4 57 txBuffer = new queue();
oprospero 0:26a151d2c6db 58 signalStrength = 0;
oprospero 0:26a151d2c6db 59 xbeeRx.attach( this, &com::callback ); // Set callback as the interrupt handler.
oprospero 13:2fb7b19dcd70 60 #ifdef DEBUG_COM
oprospero 0:26a151d2c6db 61 xbeeTx.printf("Communication.....Done\n\r");
oprospero 1:4f53de75bc96 62 #endif
oprospero 13:2fb7b19dcd70 63 START;
oprospero 0:26a151d2c6db 64 }
oprospero 0:26a151d2c6db 65
oprospero 0:26a151d2c6db 66 /************************* bool isData() ********************************/
oprospero 0:26a151d2c6db 67 /* */
oprospero 0:26a151d2c6db 68 /*************************************************************************/
oprospero 0:26a151d2c6db 69
oprospero 0:26a151d2c6db 70 bool com::isData()
oprospero 0:26a151d2c6db 71 {
oprospero 12:bf4642578682 72 return rdy2build;
oprospero 0:26a151d2c6db 73 }
oprospero 0:26a151d2c6db 74
oprospero 0:26a151d2c6db 75 /************************ void write( char ) *****************************/
oprospero 0:26a151d2c6db 76 /* Write a packet out the xbee com port. */
oprospero 0:26a151d2c6db 77 /* */
oprospero 0:26a151d2c6db 78 /* Data format byte[] */
oprospero 0:26a151d2c6db 79 /* byte[0] = command. */
oprospero 0:26a151d2c6db 80 /* byte[1] = upper 8 bits of value. */
oprospero 0:26a151d2c6db 81 /* byte[2] = lower 8 bits of value. */
oprospero 0:26a151d2c6db 82 /* byte[3] = Checksum byte[0] + byte[2]. */
oprospero 0:26a151d2c6db 83 /* byte[4] = Sequence Number. */
oprospero 0:26a151d2c6db 84 /* byte[5] = 255 End of message. */
oprospero 0:26a151d2c6db 85 /*************************************************************************/
oprospero 0:26a151d2c6db 86
oprospero 0:26a151d2c6db 87 void com::write( short command, short value )
oprospero 12:bf4642578682 88 {
oprospero 12:bf4642578682 89 short lvalue = (value % 128);
oprospero 12:bf4642578682 90 xbeeTx.putc( (char)command ); // Command
oprospero 12:bf4642578682 91 xbeeTx.putc( (char) value / 128 ); // First 8 bits in array.
oprospero 12:bf4642578682 92 xbeeTx.putc( (char) lvalue); // Second 8 bits in array.
oprospero 12:bf4642578682 93 xbeeTx.putc( command + lvalue ); // Checksum array[0] + array[1].
oprospero 12:bf4642578682 94 xbeeTx.putc( (char)value ); // Sequence number.
oprospero 12:bf4642578682 95 xbeeTx.putc( 255 ); // End of message.
oprospero 0:26a151d2c6db 96 }
oprospero 0:26a151d2c6db 97
oprospero 5:7cdf299ea7a4 98 /*************************** char ackCheck() ********************************/
oprospero 0:26a151d2c6db 99 /* */
oprospero 0:26a151d2c6db 100 /*************************************************************************/
oprospero 0:26a151d2c6db 101
oprospero 14:d2acb373d81c 102 void com::sendACK()
oprospero 4:f8e953201251 103 {
oprospero 13:2fb7b19dcd70 104 if( !txBuffer->isEmpty())
oprospero 4:f8e953201251 105 {
oprospero 11:d1f488302d06 106 __disable_irq();
oprospero 5:7cdf299ea7a4 107 short * pkt = txBuffer->pop();
oprospero 13:2fb7b19dcd70 108 __enable_irq();
oprospero 5:7cdf299ea7a4 109 write(pkt[0],pkt[1]); //may need to disable interrupt
oprospero 14:d2acb373d81c 110 // #ifdef DEBUG_COM
oprospero 13:2fb7b19dcd70 111 if(pkt[1] % 5 == 0)
oprospero 13:2fb7b19dcd70 112 {
oprospero 13:2fb7b19dcd70 113 PRINTLNF("len: %d",txBuffer->queueLength());
oprospero 14:d2acb373d81c 114 xbeeTx.printf("len: %d\n\r",txBuffer->queueLength());
oprospero 13:2fb7b19dcd70 115 }
oprospero 14:d2acb373d81c 116 // #endif
oprospero 5:7cdf299ea7a4 117 delete[] pkt;
oprospero 13:2fb7b19dcd70 118
oprospero 4:f8e953201251 119 }
oprospero 4:f8e953201251 120 }
oprospero 4:f8e953201251 121
oprospero 5:7cdf299ea7a4 122 bool com::rdy2ack()
oprospero 5:7cdf299ea7a4 123 {
oprospero 5:7cdf299ea7a4 124 return !txBuffer->isEmpty();
oprospero 5:7cdf299ea7a4 125 }
oprospero 4:f8e953201251 126
oprospero 4:f8e953201251 127
oprospero 0:26a151d2c6db 128
oprospero 0:26a151d2c6db 129 /************************ void callback() ********************************/
oprospero 0:26a151d2c6db 130 /* */
oprospero 0:26a151d2c6db 131 /*************************************************************************/
oprospero 0:26a151d2c6db 132
oprospero 0:26a151d2c6db 133 void com::callback()
oprospero 0:26a151d2c6db 134 {
oprospero 4:f8e953201251 135 __disable_irq();
oprospero 0:26a151d2c6db 136 while( xbeeRx.readable() )
oprospero 0:26a151d2c6db 137 {
oprospero 0:26a151d2c6db 138 char data = xbeeRx.getc();
oprospero 13:2fb7b19dcd70 139 // xbeeRx.putc(data);
oprospero 12:bf4642578682 140 if (isA1)
oprospero 12:bf4642578682 141 {
oprospero 12:bf4642578682 142 if ( data == 255 && index1 > 4 )
oprospero 12:bf4642578682 143 {
oprospero 12:bf4642578682 144 rdy2build = true;
oprospero 12:bf4642578682 145 pindex = index1;
oprospero 12:bf4642578682 146 index1 = 0;
oprospero 12:bf4642578682 147 isA1 = false;
oprospero 12:bf4642578682 148 }
oprospero 12:bf4642578682 149 else if ( index1 < BUFFERSIZE )
oprospero 12:bf4642578682 150 {
oprospero 13:2fb7b19dcd70 151
oprospero 12:bf4642578682 152 buffer1[index1++] = data;
oprospero 12:bf4642578682 153 }
oprospero 12:bf4642578682 154 }
oprospero 12:bf4642578682 155 else
oprospero 12:bf4642578682 156 {
oprospero 12:bf4642578682 157 if ( data == 255 && index2 > 4 )
oprospero 12:bf4642578682 158 {
oprospero 12:bf4642578682 159 rdy2build = true;
oprospero 12:bf4642578682 160 pindex = index2;
oprospero 12:bf4642578682 161 index2 = 0;
oprospero 12:bf4642578682 162 isA1 = true;
oprospero 12:bf4642578682 163 }
oprospero 12:bf4642578682 164 else if ( index2 < BUFFERSIZE )
oprospero 12:bf4642578682 165 {
oprospero 12:bf4642578682 166 buffer2[index2++] = data;
oprospero 12:bf4642578682 167 }
oprospero 12:bf4642578682 168 }
oprospero 0:26a151d2c6db 169 }
oprospero 4:f8e953201251 170 __enable_irq();
oprospero 0:26a151d2c6db 171 }
oprospero 0:26a151d2c6db 172
oprospero 2:bd1621102cad 173
oprospero 0:26a151d2c6db 174 /********************** void packetBuilder() *****************************/
oprospero 0:26a151d2c6db 175 /* Creates a packet from the buffered data and places it in the rxBuffer */
oprospero 0:26a151d2c6db 176 /* queue to be read whenever convenient. Max value of +/- 8063. */
oprospero 0:26a151d2c6db 177 /*************************************************************************/
oprospero 0:26a151d2c6db 178
oprospero 12:bf4642578682 179 short * com::read()
oprospero 0:26a151d2c6db 180 {
oprospero 12:bf4642578682 181 if (rdy2build)
oprospero 0:26a151d2c6db 182 {
oprospero 13:2fb7b19dcd70 183 rdy2build = false;
oprospero 13:2fb7b19dcd70 184 char * commandData = new char[5];
oprospero 12:bf4642578682 185 if (!isA1)
oprospero 12:bf4642578682 186 {
oprospero 12:bf4642578682 187 commandData[4] = buffer1[--pindex]; // Sequence Number.
oprospero 12:bf4642578682 188 commandData[3] = buffer1[--pindex]; // CheckSum value.
oprospero 12:bf4642578682 189 commandData[2] = buffer1[--pindex]; // Second 7 bits.
oprospero 12:bf4642578682 190 commandData[1] = buffer1[--pindex]; // Fisrt 7 bits.
oprospero 12:bf4642578682 191 commandData[0] = buffer1[--pindex]; // Command.
oprospero 12:bf4642578682 192 }
oprospero 12:bf4642578682 193 else
oprospero 12:bf4642578682 194 {
oprospero 12:bf4642578682 195 commandData[4] = buffer2[--pindex]; // Sequence Number.
oprospero 12:bf4642578682 196 commandData[3] = buffer2[--pindex]; // CheckSum value.
oprospero 12:bf4642578682 197 commandData[2] = buffer2[--pindex]; // Second 7 bits.
oprospero 12:bf4642578682 198 commandData[1] = buffer2[--pindex]; // Fisrt 7 bits.
oprospero 12:bf4642578682 199 commandData[0] = buffer2[--pindex]; // Command.
oprospero 12:bf4642578682 200 }
oprospero 0:26a151d2c6db 201
oprospero 13:2fb7b19dcd70 202 // xbeeTx.printf("Copied: %d %d %d %d %d\n\r",commandData[0],commandData[1],commandData[2],commandData[3],commandData[4]);
oprospero 12:bf4642578682 203
oprospero 12:bf4642578682 204 if( commandData[0] + commandData[2] == commandData[3] ) // Validate checksum.
oprospero 4:f8e953201251 205 {
oprospero 12:bf4642578682 206 short * array = new short[2];
oprospero 12:bf4642578682 207 array[0] = (short)commandData[0];
oprospero 12:bf4642578682 208
oprospero 12:bf4642578682 209 short value = (short)(commandData[1] * 128 + commandData[2]);
oprospero 12:bf4642578682 210
oprospero 12:bf4642578682 211 if( value > 8062 )
oprospero 12:bf4642578682 212 value = (short)value + 57344;
oprospero 12:bf4642578682 213
oprospero 12:bf4642578682 214 array[1] = value;
oprospero 12:bf4642578682 215 if ( commandData[0] != 0)
oprospero 12:bf4642578682 216 {
oprospero 12:bf4642578682 217 short * ackPacket = new short[2];
oprospero 12:bf4642578682 218 ackPacket[0] = commandData[0];
oprospero 12:bf4642578682 219 ackPacket[1] = commandData[4];
oprospero 12:bf4642578682 220 txBuffer->add( ackPacket ); // Ack the packet with sequence nuber.
oprospero 12:bf4642578682 221 }
oprospero 12:bf4642578682 222
oprospero 12:bf4642578682 223 delete[] commandData;
oprospero 12:bf4642578682 224 return array;
oprospero 12:bf4642578682 225 }
oprospero 12:bf4642578682 226 delete[] commandData;
oprospero 12:bf4642578682 227 }
oprospero 12:bf4642578682 228 return NULL;
oprospero 0:26a151d2c6db 229 }
oprospero 12:bf4642578682 230
oprospero 0:26a151d2c6db 231
oprospero 0:26a151d2c6db 232
oprospero 0:26a151d2c6db 233 /********************** bool isSignalGood() ******************************/
oprospero 0:26a151d2c6db 234 /* For future use */
oprospero 0:26a151d2c6db 235 /*************************************************************************/
oprospero 0:26a151d2c6db 236 bool com::isSignalGood()
oprospero 0:26a151d2c6db 237 {
oprospero 0:26a151d2c6db 238 signalStrength = rssi.dutycycle();
oprospero 0:26a151d2c6db 239 if (signalStrength > RSSI_THRES) return true;
oprospero 0:26a151d2c6db 240 else return false;
oprospero 0:26a151d2c6db 241 }