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@15:3f742edaa359, 2014-08-27 (annotated)

Committer:
oprospero
Date:
Wed Aug 27 06:20:23 2014 +0000
Revision:
15:3f742edaa359
Parent:
14:d2acb373d81c
Child:
16:89695823d407
Changed to new controls.; Off load code from RX callback to main loop

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 15:3f742edaa359 70 //bool com::isData()
oprospero 15:3f742edaa359 71 //{
oprospero 15:3f742edaa359 72 // return rdy2build;
oprospero 15:3f742edaa359 73 //}
oprospero 15:3f742edaa359 74
oprospero 0:26a151d2c6db 75 bool com::isData()
oprospero 0:26a151d2c6db 76 {
oprospero 15:3f742edaa359 77 static short packetIndex = 0;
oprospero 15:3f742edaa359 78 static short pack_cmd = 0;
oprospero 15:3f742edaa359 79 static short pack_value[2] = {0,0};
oprospero 15:3f742edaa359 80 static short pack_seq = 0;
oprospero 15:3f742edaa359 81 static short pack_checksum = 0;
oprospero 15:3f742edaa359 82 while ( !rxBuffer->isEmpty() )
oprospero 15:3f742edaa359 83 {
oprospero 15:3f742edaa359 84 short * data = rxBuffer->pop();
oprospero 15:3f742edaa359 85 switch (packetIndex)
oprospero 15:3f742edaa359 86 {
oprospero 15:3f742edaa359 87 case 0:
oprospero 15:3f742edaa359 88 {
oprospero 15:3f742edaa359 89 if ( *data == 255 )
oprospero 15:3f742edaa359 90 packetIndex++;
oprospero 15:3f742edaa359 91 break;
oprospero 15:3f742edaa359 92 }
oprospero 15:3f742edaa359 93 case 1:
oprospero 15:3f742edaa359 94 {
oprospero 15:3f742edaa359 95 if ( *data < 13 )
oprospero 15:3f742edaa359 96 {
oprospero 15:3f742edaa359 97 pack_cmd = *data;
oprospero 15:3f742edaa359 98 packetIndex++;
oprospero 15:3f742edaa359 99 }
oprospero 15:3f742edaa359 100 else
oprospero 15:3f742edaa359 101 packetIndex = 0;
oprospero 15:3f742edaa359 102 break;
oprospero 15:3f742edaa359 103 }
oprospero 15:3f742edaa359 104 case 2:
oprospero 15:3f742edaa359 105 {
oprospero 15:3f742edaa359 106 pack_value[1] = *data;
oprospero 15:3f742edaa359 107 packetIndex++;
oprospero 15:3f742edaa359 108 break;
oprospero 15:3f742edaa359 109 }
oprospero 15:3f742edaa359 110 case 3:
oprospero 15:3f742edaa359 111 {
oprospero 15:3f742edaa359 112 pack_value[0] = *data;
oprospero 15:3f742edaa359 113 packetIndex++;
oprospero 15:3f742edaa359 114 break;
oprospero 15:3f742edaa359 115 }
oprospero 15:3f742edaa359 116 case 4:
oprospero 15:3f742edaa359 117 {
oprospero 15:3f742edaa359 118 pack_seq = *data;
oprospero 15:3f742edaa359 119 packetIndex++;
oprospero 15:3f742edaa359 120 break;
oprospero 15:3f742edaa359 121 }
oprospero 15:3f742edaa359 122 case 5:
oprospero 15:3f742edaa359 123 {
oprospero 15:3f742edaa359 124 short temp = pack_value[0] + pack_cmd;
oprospero 15:3f742edaa359 125 pack_checksum = *data;
oprospero 15:3f742edaa359 126 if ( temp == pack_checksum )
oprospero 15:3f742edaa359 127 {
oprospero 15:3f742edaa359 128 short * ackPacket = new short[2];
oprospero 15:3f742edaa359 129 ackPacket[0] = pack_cmd;
oprospero 15:3f742edaa359 130 ackPacket[1] = pack_seq;
oprospero 15:3f742edaa359 131 txBuffer->add( ackPacket ); // Ack the packet with sequence nuber.
oprospero 15:3f742edaa359 132
oprospero 15:3f742edaa359 133 short * array = new short[2];
oprospero 15:3f742edaa359 134 array[0] = pack_cmd;
oprospero 15:3f742edaa359 135 array[1] = pack_value[1] * 128 + pack_value[0];
oprospero 15:3f742edaa359 136 cmdBuffer->add( array );
oprospero 15:3f742edaa359 137 }
oprospero 15:3f742edaa359 138
oprospero 15:3f742edaa359 139 packetIndex = 0;
oprospero 15:3f742edaa359 140 break;
oprospero 15:3f742edaa359 141 }
oprospero 15:3f742edaa359 142 default:
oprospero 15:3f742edaa359 143 packetIndex = 0;
oprospero 15:3f742edaa359 144 break;
oprospero 15:3f742edaa359 145 }
oprospero 15:3f742edaa359 146
oprospero 15:3f742edaa359 147 }
oprospero 15:3f742edaa359 148 return !cmdBuffer->isEmpty();
oprospero 0:26a151d2c6db 149 }
oprospero 0:26a151d2c6db 150
oprospero 0:26a151d2c6db 151 /************************ void write( char ) *****************************/
oprospero 0:26a151d2c6db 152 /* Write a packet out the xbee com port. */
oprospero 0:26a151d2c6db 153 /* */
oprospero 0:26a151d2c6db 154 /* Data format byte[] */
oprospero 0:26a151d2c6db 155 /* byte[0] = command. */
oprospero 0:26a151d2c6db 156 /* byte[1] = upper 8 bits of value. */
oprospero 0:26a151d2c6db 157 /* byte[2] = lower 8 bits of value. */
oprospero 0:26a151d2c6db 158 /* byte[3] = Checksum byte[0] + byte[2]. */
oprospero 0:26a151d2c6db 159 /* byte[4] = Sequence Number. */
oprospero 0:26a151d2c6db 160 /* byte[5] = 255 End of message. */
oprospero 0:26a151d2c6db 161 /*************************************************************************/
oprospero 0:26a151d2c6db 162
oprospero 0:26a151d2c6db 163 void com::write( short command, short value )
oprospero 12:bf4642578682 164 {
oprospero 15:3f742edaa359 165 xbeeTx.putc( 255 ); // End of message.
oprospero 12:bf4642578682 166 xbeeTx.putc( (char)command ); // Command
oprospero 15:3f742edaa359 167 xbeeTx.putc( (char) value); // Second 8 bits in array.
oprospero 15:3f742edaa359 168 xbeeTx.putc( command + value ); // Checksum array[0] + array[1].
oprospero 0:26a151d2c6db 169 }
oprospero 0:26a151d2c6db 170
oprospero 5:7cdf299ea7a4 171 /*************************** char ackCheck() ********************************/
oprospero 0:26a151d2c6db 172 /* */
oprospero 0:26a151d2c6db 173 /*************************************************************************/
oprospero 0:26a151d2c6db 174
oprospero 14:d2acb373d81c 175 void com::sendACK()
oprospero 4:f8e953201251 176 {
oprospero 13:2fb7b19dcd70 177 if( !txBuffer->isEmpty())
oprospero 4:f8e953201251 178 {
oprospero 11:d1f488302d06 179 __disable_irq();
oprospero 5:7cdf299ea7a4 180 short * pkt = txBuffer->pop();
oprospero 13:2fb7b19dcd70 181 __enable_irq();
oprospero 5:7cdf299ea7a4 182 write(pkt[0],pkt[1]); //may need to disable interrupt
oprospero 14:d2acb373d81c 183 // #ifdef DEBUG_COM
oprospero 13:2fb7b19dcd70 184 if(pkt[1] % 5 == 0)
oprospero 13:2fb7b19dcd70 185 {
oprospero 13:2fb7b19dcd70 186 PRINTLNF("len: %d",txBuffer->queueLength());
oprospero 14:d2acb373d81c 187 xbeeTx.printf("len: %d\n\r",txBuffer->queueLength());
oprospero 13:2fb7b19dcd70 188 }
oprospero 14:d2acb373d81c 189 // #endif
oprospero 5:7cdf299ea7a4 190 delete[] pkt;
oprospero 13:2fb7b19dcd70 191
oprospero 4:f8e953201251 192 }
oprospero 4:f8e953201251 193 }
oprospero 4:f8e953201251 194
oprospero 5:7cdf299ea7a4 195 bool com::rdy2ack()
oprospero 5:7cdf299ea7a4 196 {
oprospero 5:7cdf299ea7a4 197 return !txBuffer->isEmpty();
oprospero 5:7cdf299ea7a4 198 }
oprospero 4:f8e953201251 199
oprospero 4:f8e953201251 200
oprospero 0:26a151d2c6db 201
oprospero 0:26a151d2c6db 202 /************************ void callback() ********************************/
oprospero 0:26a151d2c6db 203 /* */
oprospero 0:26a151d2c6db 204 /*************************************************************************/
oprospero 0:26a151d2c6db 205
oprospero 15:3f742edaa359 206 //void com::callback()
oprospero 15:3f742edaa359 207 //{
oprospero 15:3f742edaa359 208 // __disable_irq();
oprospero 15:3f742edaa359 209 // while( xbeeRx.readable() )
oprospero 15:3f742edaa359 210 // {
oprospero 15:3f742edaa359 211 // char data = xbeeRx.getc();
oprospero 15:3f742edaa359 212 //// xbeeRx.putc(data);
oprospero 15:3f742edaa359 213 // if (isA1)
oprospero 15:3f742edaa359 214 // {
oprospero 15:3f742edaa359 215 // if ( data == 255 && index1 > 4 )
oprospero 15:3f742edaa359 216 // {
oprospero 15:3f742edaa359 217 // rdy2build = true;
oprospero 15:3f742edaa359 218 // pindex = index1;
oprospero 15:3f742edaa359 219 // index1 = 0;
oprospero 15:3f742edaa359 220 // isA1 = false;
oprospero 15:3f742edaa359 221 // }
oprospero 15:3f742edaa359 222 // else if ( index1 < BUFFERSIZE )
oprospero 15:3f742edaa359 223 // {
oprospero 15:3f742edaa359 224 //
oprospero 15:3f742edaa359 225 // buffer1[index1++] = data;
oprospero 15:3f742edaa359 226 // }
oprospero 15:3f742edaa359 227 // }
oprospero 15:3f742edaa359 228 // else
oprospero 15:3f742edaa359 229 // {
oprospero 15:3f742edaa359 230 // if ( data == 255 && index2 > 4 )
oprospero 15:3f742edaa359 231 // {
oprospero 15:3f742edaa359 232 // rdy2build = true;
oprospero 15:3f742edaa359 233 // pindex = index2;
oprospero 15:3f742edaa359 234 // index2 = 0;
oprospero 15:3f742edaa359 235 // isA1 = true;
oprospero 15:3f742edaa359 236 // }
oprospero 15:3f742edaa359 237 // else if ( index2 < BUFFERSIZE )
oprospero 15:3f742edaa359 238 // {
oprospero 15:3f742edaa359 239 // buffer2[index2++] = data;
oprospero 15:3f742edaa359 240 // }
oprospero 15:3f742edaa359 241 // }
oprospero 15:3f742edaa359 242 // }
oprospero 15:3f742edaa359 243 // __enable_irq();
oprospero 15:3f742edaa359 244 //}
oprospero 15:3f742edaa359 245
oprospero 0:26a151d2c6db 246 void com::callback()
oprospero 0:26a151d2c6db 247 {
oprospero 4:f8e953201251 248 __disable_irq();
oprospero 0:26a151d2c6db 249 while( xbeeRx.readable() )
oprospero 0:26a151d2c6db 250 {
oprospero 15:3f742edaa359 251 short *data = new short;
oprospero 15:3f742edaa359 252 *data = xbeeRx.getc();
oprospero 15:3f742edaa359 253
oprospero 15:3f742edaa359 254 rxBuffer->add( data );
oprospero 0:26a151d2c6db 255 }
oprospero 4:f8e953201251 256 __enable_irq();
oprospero 0:26a151d2c6db 257 }
oprospero 0:26a151d2c6db 258
oprospero 0:26a151d2c6db 259 /********************** void packetBuilder() *****************************/
oprospero 0:26a151d2c6db 260 /* Creates a packet from the buffered data and places it in the rxBuffer */
oprospero 0:26a151d2c6db 261 /* queue to be read whenever convenient. Max value of +/- 8063. */
oprospero 0:26a151d2c6db 262 /*************************************************************************/
oprospero 0:26a151d2c6db 263
oprospero 15:3f742edaa359 264 //short * com::read()
oprospero 15:3f742edaa359 265 //{
oprospero 15:3f742edaa359 266 // if (rdy2build)
oprospero 15:3f742edaa359 267 // {
oprospero 15:3f742edaa359 268 // rdy2build = false;
oprospero 15:3f742edaa359 269 // char * commandData = new char[5];
oprospero 15:3f742edaa359 270 // if (!isA1)
oprospero 15:3f742edaa359 271 // {
oprospero 15:3f742edaa359 272 // commandData[4] = buffer1[--pindex]; // Sequence Number.
oprospero 15:3f742edaa359 273 // commandData[3] = buffer1[--pindex]; // CheckSum value.
oprospero 15:3f742edaa359 274 // commandData[2] = buffer1[--pindex]; // Second 7 bits.
oprospero 15:3f742edaa359 275 // commandData[1] = buffer1[--pindex]; // Fisrt 7 bits.
oprospero 15:3f742edaa359 276 // commandData[0] = buffer1[--pindex]; // Command.
oprospero 15:3f742edaa359 277 // }
oprospero 15:3f742edaa359 278 // else
oprospero 15:3f742edaa359 279 // {
oprospero 15:3f742edaa359 280 // commandData[4] = buffer2[--pindex]; // Sequence Number.
oprospero 15:3f742edaa359 281 // commandData[3] = buffer2[--pindex]; // CheckSum value.
oprospero 15:3f742edaa359 282 // commandData[2] = buffer2[--pindex]; // Second 7 bits.
oprospero 15:3f742edaa359 283 // commandData[1] = buffer2[--pindex]; // Fisrt 7 bits.
oprospero 15:3f742edaa359 284 // commandData[0] = buffer2[--pindex]; // Command.
oprospero 15:3f742edaa359 285 // }
oprospero 15:3f742edaa359 286 //
oprospero 15:3f742edaa359 287 //// xbeeTx.printf("Copied: %d %d %d %d %d\n\r",commandData[0],commandData[1],commandData[2],commandData[3],commandData[4]);
oprospero 15:3f742edaa359 288 //
oprospero 15:3f742edaa359 289 // if( commandData[0] + commandData[2] == commandData[3] ) // Validate checksum.
oprospero 15:3f742edaa359 290 // {
oprospero 15:3f742edaa359 291 // short * array = new short[2];
oprospero 15:3f742edaa359 292 // array[0] = (short)commandData[0];
oprospero 15:3f742edaa359 293 //
oprospero 15:3f742edaa359 294 // short value = (short)(commandData[1] * 128 + commandData[2]);
oprospero 15:3f742edaa359 295 //
oprospero 15:3f742edaa359 296 // if( value > 8062 )
oprospero 15:3f742edaa359 297 // value = (short)value + 57344;
oprospero 15:3f742edaa359 298 //
oprospero 15:3f742edaa359 299 // array[1] = value;
oprospero 15:3f742edaa359 300 // if ( commandData[0] != 0)
oprospero 15:3f742edaa359 301 // {
oprospero 15:3f742edaa359 302 // short * ackPacket = new short[2];
oprospero 15:3f742edaa359 303 // ackPacket[0] = commandData[0];
oprospero 15:3f742edaa359 304 // ackPacket[1] = commandData[4];
oprospero 15:3f742edaa359 305 // txBuffer->add( ackPacket ); // Ack the packet with sequence nuber.
oprospero 15:3f742edaa359 306 // }
oprospero 15:3f742edaa359 307 //
oprospero 15:3f742edaa359 308 // delete[] commandData;
oprospero 15:3f742edaa359 309 // return array;
oprospero 15:3f742edaa359 310 // }
oprospero 15:3f742edaa359 311 // delete[] commandData;
oprospero 15:3f742edaa359 312 // }
oprospero 15:3f742edaa359 313 // return NULL;
oprospero 15:3f742edaa359 314 //}
oprospero 15:3f742edaa359 315
oprospero 12:bf4642578682 316 short * com::read()
oprospero 0:26a151d2c6db 317 {
oprospero 15:3f742edaa359 318 if ( !cmdBuffer->isEmpty() )
oprospero 15:3f742edaa359 319 return cmdBuffer->pop();
oprospero 15:3f742edaa359 320 else
oprospero 15:3f742edaa359 321 return NULL;
oprospero 0:26a151d2c6db 322 }
oprospero 12:bf4642578682 323
oprospero 0:26a151d2c6db 324
oprospero 0:26a151d2c6db 325 /********************** bool isSignalGood() ******************************/
oprospero 0:26a151d2c6db 326 /* For future use */
oprospero 0:26a151d2c6db 327 /*************************************************************************/
oprospero 0:26a151d2c6db 328 bool com::isSignalGood()
oprospero 0:26a151d2c6db 329 {
oprospero 0:26a151d2c6db 330 signalStrength = rssi.dutycycle();
oprospero 0:26a151d2c6db 331 if (signalStrength > RSSI_THRES) return true;
oprospero 0:26a151d2c6db 332 else return false;
oprospero 0:26a151d2c6db 333 }