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@16:89695823d407, 2014-09-25 (annotated)

Committer:
oprospero
Date:
Thu Sep 25 02:30:25 2014 +0000
Revision:
16:89695823d407
Parent:
15:3f742edaa359
Child:
17:acef0fb07510
changed to started char instead of ending

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