Statistics
| Revision:

root / branches / wireless / code / projects / libwireless / wireless_send.c @ 1789

History | View | Annotate | Download (18.1 KB)

1
/**
2
 * Copyright (c) 2009 Colony Project
3
 *
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 *
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 *
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

    
26
/**
27
 * @file wireless_send.c
28
 * @brief Wireless library send functions
29
 *
30
 * Implementation of high level wireless communication.
31
 * This is the send functions portion of the library.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 **/
35

    
36
#include "wl_defs.h"
37
#include "wireless.h"
38
#include "xbee.h"
39
#include <string.h>
40

    
41

    
42
/**
43
 * Definition for wireless library send packet structure
44
 * byte 1: length of packet (from frame number to end of data)
45
 * byte 2: frame number
46
 * bytes 3-n: data (dest, options, frame number, group code, packet)
47
 * byte n+1: num retries
48
 *
49
 * Definition for ack buffer
50
 * 2 bit system: 0=still sending
51
 *               1=OK
52
 *               2=ack failure
53
 *               3=CCA failure
54
 **/
55

    
56

    
57
/* global variables */
58
uint8_t nextframe = 1; // number of next frame
59
uint8_t ack_buf[64]; // ack buffer (holds 255 packet return codes)
60
uint8_t send_buf[PACKET_BUFFER_SIZE]; // sending buffer for retries
61
uint8_t send_buf_first = 0; // first byte of data on buffer
62
uint8_t send_buf_last = 0; // next free byte on buffer
63
uint8_t send_buf_num_packets = 0; // number of packets in sending buffer
64

    
65
/* private function prototypes */
66
void setack(uint8_t num,uint8_t val);
67
void ackhandle(uint8_t num,uint8_t val);
68
int8_t send_buf_add(uint8_t *ptr, uint8_t byte);
69
uint8_t send_buf_get(uint8_t *ptr);
70

    
71
// the send functions
72

    
73
/**
74
 * @addtogroup wireless Wireless
75
 * @{
76
 **/
77

    
78

    
79
/**
80
 * The core send function. This will take all possible arguments and send all types of packets.
81
 * 
82
 * @param data pointer to the byte array of data to be included in the packet
83
 * @param length the length of the data array
84
 * @param group the packet group of the packet
85
 * @param scope flag for sending global packet or your current PAN
86
 * @param dest robot ID (for robot to robot packet)
87
 * @param mode flag for using FAST or RELIABLE sending
88
 *
89
 * @return positive packet number for tracking acks, or error code
90
 **/
91
int16_t wl_send(uint8_t *data, uint8_t length, uint8_t group, uint8_t scope, uint16_t dest, uint8_t mode) {
92
  uint8_t packet[6];
93
  int16_t ret = WL_SUCCESS;
94
  
95
  // build packet header
96
  packet[0] = nextframe;
97
  packet[1] = (dest&0xFF00)>>8;
98
  packet[2] = dest&0x00FF;
99
  packet[3] = XBEE_OPTIONS_NONE;
100
  packet[4] = nextframe;
101
  packet[5] = group;
102
  
103
  // set scope
104
  if (scope == GLOBAL)
105
    packet[3] &= XBEE_OPTIONS_BROADCAST_ALL_PANS;
106
  else if (scope != PAN) {
107
    WL_DEBUG_PRINT_P("Error - bad scope in core send function\r\n");
108
    return WL_ERROR_SCOPE;
109
  }
110
    
111
  // set mode
112
  if (mode == FAST) {
113
    packet[3] &= XBEE_OPTIONS_DISABLE_RESPONSE;
114
  } else if (mode != RELIABLE) {
115
    WL_DEBUG_PRINT_P("Error - bad mode in core send function\r\n");
116
    return WL_ERROR_MODE;
117
  }
118
  
119
  // do checksum
120
  group = XBEE_FRAME_TX_REQUEST_16;
121
  xbee_checksum_add(packet,6,&group);
122
  xbee_checksum_add(data,length,&group); 
123
  group = 0xFF-group;
124

    
125
  // send the packet
126
  if (xbee_send_header(length+7) != WL_SUCCESS) {
127
    WL_DEBUG_PRINT_P("1Error sending packet from core send function\r\n");
128
    return WL_ERROR_SEND;
129
  }
130
  if (xbee_sendc(XBEE_FRAME_TX_REQUEST_16) != WL_SUCCESS) {
131
    WL_DEBUG_PRINT_P("2Error sending packet from core send function\r\n");
132
    return WL_ERROR_SEND;
133
  }
134
  if (xbee_send(packet,6) != WL_SUCCESS) {
135
    WL_DEBUG_PRINT_P("3Error sending packet from core send function\r\n");
136
    return WL_ERROR_SEND;
137
  }
138
  if (xbee_send(data,length) != WL_SUCCESS) {
139
    WL_DEBUG_PRINT_P("4Error sending packet from core send function\r\n");
140
    return WL_ERROR_SEND;
141
  }
142
  if (xbee_sendc(group) != WL_SUCCESS) {
143
    WL_DEBUG_PRINT_P("5Error sending packet from core send function\r\n");
144
    return WL_ERROR_SEND;
145
  }
146
  
147
  // save in ack system
148
  if (mode == FAST) {
149
    setack(nextframe,ACK_OK); // assume the send was successful
150
    nextframe = (nextframe == 0xFF)?1:nextframe+1; // increment frame number
151
    return ret; // no frame number
152
  } else if (mode == RELIABLE) {
153
    setack(nextframe,SENDING); // set status to SENDING
154
    // save packet on sending buffer
155
    scope = send_buf_last; // use as ptr to send buffer    
156
    if (send_buf_add(&scope,length+6) != WL_SUCCESS) { // add length
157
      WL_DEBUG_PRINT_P("Error adding length to sending buffer\r\n");
158
      ret = WL_ERROR_SENDING_BUFFER_FULL;
159
    }    
160
    for(mode=0;mode<6;mode++) { // add header
161
      if (send_buf_add(&scope,packet[mode]) != WL_SUCCESS) {
162
        WL_DEBUG_PRINT_P("Error adding header to sending buffer\r\n");
163
        ret = WL_ERROR_SENDING_BUFFER_FULL;
164
      }
165
    }
166
    for(mode=0;mode<length;mode++) { // add data
167
      if (send_buf_add(&scope,data[mode]) != WL_SUCCESS) {
168
        WL_DEBUG_PRINT_P("Error adding data to sendig buffer\r\n");
169
        ret = WL_ERROR_SENDING_BUFFER_FULL;
170
      }
171
    }
172
    if (send_buf_add(&scope,0) != WL_SUCCESS) { // add num retries=0
173
      WL_DEBUG_PRINT_P("Error adding num_retries to sending buffer\r\n");
174
      ret = WL_ERROR_SENDING_BUFFER_FULL;
175
    }
176
    send_buf_last = scope;
177
    send_buf_num_packets++;
178
    WL_DEBUG_PRINT_P("send_buf_last:");
179
    WL_DEBUG_PRINT_INT(send_buf_last);
180
    WL_DEBUG_PRINT_P("pos:");
181
    WL_DEBUG_PRINT_INT(scope);
182
    WL_DEBUG_PRINT_P("send_buf_num_packets:");
183
    WL_DEBUG_PRINT_INT(send_buf_num_packets);
184
    
185
    nextframe = (nextframe == 0xFF)?1:nextframe+1; // increment frame number
186
    return (ret == WL_SUCCESS)?packet[0]:ret; // return frame number for ack tracking
187
  }
188
  return WL_ERROR_SEND; // shouldn't get here, but if it does there's a problem
189
}
190

    
191
/**
192
 * Wrapper for core send function that will send a global packet across the current channel.
193
 *
194
 * @param data pointer to the byte array of data to be included in the packet
195
 * @param length the length of the data array
196
 * @param group the packet group of the packet
197
 *
198
 * @return positive packet number for tracking acks, or error code
199
 **/
200
int16_t wl_send_global(uint8_t *data, uint8_t length, uint8_t group) {
201
  return wl_send(data, length, group, GLOBAL, BROADCAST, RELIABLE);
202
}
203

    
204
/**
205
 * Wrapper for core send function that will send a packet across the current channel on the current pan.
206
 *
207
 * @param data pointer to the byte array of data to be included in the packet
208
 * @param length the length of the data array
209
 * @param group the packet group of the packet
210
 *
211
 * @return positive packet number for tracking acks, or error code (TBD)
212
 **/
213
int16_t wl_send_pan(uint8_t *data, uint8_t length, uint8_t group) {
214
  return wl_send(data, length, group, PAN, BROADCAST, RELIABLE);
215
}
216

    
217
/**
218
 * Wrapper for core send function that will send a packet across the current channel to a specific robot. 
219
 *
220
 * @param data pointer to the byte array of data to be included in the packet
221
 * @param length the length of the data array
222
 * @param group the packet group of the packet 
223
 * @param dest robot ID (for robot to robot packet)
224
 * @param mode flag for using FAST or RELIABLE sending
225
 *
226
 * @return positive packet number for tracking acks, or error code (TBD)
227
 **/
228
int16_t wl_send_robot(uint8_t *data, uint8_t length, uint8_t group, uint16_t dest, uint8_t mode) {
229
  return wl_send(data, length, group, GLOBAL, dest, mode);
230
}
231

    
232
/**
233
 * Default (i.e. basic) send wrapper.
234
 *
235
 * @param data pointer to the byte array of data to be included in the packet
236
 * @param length the length of the data array
237
 *
238
 * @return positive packet number for tracking acks, or error code (TBD)
239
 **/
240
int16_t wl_send_basic(uint8_t *data, uint8_t length) {
241
/** Check if it needs to adjust according to data type. **/
242
  return wl_send(data, length, BASIC, GLOBAL, BROADCAST, RELIABLE);
243
}
244

    
245

    
246
/**
247
 * acknowledgment error
248
 * check if any of the packets you have sent have been lost.
249
 *
250
 * note that all other error checking will be handled by library, 
251
 * so your user behavior won't have to worry about it
252
 *
253
 * @return the # of packets lost (up to 255)
254
 **/
255
uint8_t wl_ack_error(void) {
256
  WL_DEBUG_PRINT_P("entering wl_ack_error function");
257
  uint8_t val=0,i=1;
258
  
259
  while(1) {
260
    if (((ack_buf[i/4])&(0x2<<(i%4))) != 0) // if the ack is in an error state
261
      val++;
262
    if (i==255)
263
      break;
264
    i++;
265
  }  
266
  WL_DEBUG_PRINT_P("|num_errors:");
267
  WL_DEBUG_PRINT_INT(val);
268
  WL_DEBUG_PRINT_P("\r\n");
269
  
270
  return val;
271
}
272

    
273
/**
274
 * acknowledgement error check
275
 * check if a specific packet has been lost
276
 * note: buffer will overflow every 255 packets
277
 * 
278
 * @param packet packet number
279
 *
280
 * @return {SENDING,ACK_OK,ACK_FAILURE,CCA_FAILURE}
281
 **/
282
int8_t wl_ack_check(uint8_t packet) {
283
  WL_DEBUG_PRINT_P("entering wl_ack_check function|ack=");
284
  if (packet == 0) {
285
    // no ack information here
286
    WL_DEBUG_PRINT_P("packet number cannot be 0\r\n");
287
    return WL_ERROR_ARGUMENT;
288
  }
289
  
290
  WL_DEBUG_PRINT_HEX(ack_buf[packet/4]&(0x3<<(packet%4)));
291
  WL_DEBUG_PRINT_P("\r\n");
292
  
293
  // check ack
294
  return ack_buf[packet/4]&(0x3<<(packet%4));
295
}
296

    
297
/**
298
 * acknowledgement reset
299
 * reset the acknowledgement buffer
300
 **/
301
void wl_ack_reset(void) {
302
  WL_DEBUG_PRINT_P("entering wl_ack_reset function\r\n");
303
  memset(ack_buf,0,64);
304
}
305

    
306

    
307
/**@} **/ //end defgroup
308

    
309

    
310
// Define all private functions down here
311

    
312
/*
313
 * sets 2 bits in the ack_buf
314
 *
315
 * @param num the ack number
316
 * @param val {SENDING,ACK_OK,ACK_FAILURE,CCA_FAILURE}
317
 */
318
void setack(uint8_t num,uint8_t val) {
319
  WL_DEBUG_PRINT_P("entering setack function|num=");
320
  WL_DEBUG_PRINT_INT(num);
321
  WL_DEBUG_PRINT_P("|val=");
322
  WL_DEBUG_PRINT_HEX(val);
323
  WL_DEBUG_PRINT_P("\r\n");
324
  switch(num%4) {
325
  case 0:
326
    ack_buf[num/4] &= (0xFC|val);
327
    break;
328
  case 1:
329
    ack_buf[num/4] &= (0xF3|(val<<2));
330
    break;
331
  case 2:
332
    ack_buf[num/4] &= (0xCF|(val<<4));
333
    break;
334
  case 3:
335
    ack_buf[num/4] &= (0x3F|(val<<6));
336
    break;
337
  }
338
}
339

    
340

    
341
/* ack handler */
342
void ackhandle(uint8_t num,uint8_t val) {
343
  uint8_t len;
344
  uint8_t packets = 0;
345
  uint8_t num_packet = 0;
346
  WL_DEBUG_PRINT_P("entering ackhandle function: num:");
347
  WL_DEBUG_PRINT_HEX(num);
348
  WL_DEBUG_PRINT_P("\r\n");
349
  switch(val) {
350
  case 0:
351
    WL_DEBUG_PRINT_P("ack success\r\n");
352
    // success
353
    setack(num,ACK_OK); // set status code
354
    // remove from buffer
355
    val = send_buf_first;
356
    if (send_buf_num_packets == 0)
357
      return;
358
    WL_DEBUG_PRINT_P("|num_send_packets:");
359
    WL_DEBUG_PRINT_INT(send_buf_num_packets);
360
    while (1) {
361
      len = send_buf_get(&val);
362
      WL_DEBUG_PRINT_P("|send_packet_buf_pos:");
363
      WL_DEBUG_PRINT_INT(val);
364
      WL_DEBUG_PRINT_P("|send_packet_len:");
365
      WL_DEBUG_PRINT_INT(len);
366
      WL_DEBUG_PRINT_P("|send_packet_first:");
367
      WL_DEBUG_PRINT_INT(send_buf_first);
368
      WL_DEBUG_PRINT_P("|send_packet_last:");
369
      WL_DEBUG_PRINT_INT(send_buf_last);
370
      WL_DEBUG_PRINT_P("|send_buf:");
371
      for(int i=send_buf_first;i<send_buf_last;i++)
372
        WL_DEBUG_PRINT_HEX(send_buf[i]);
373
      
374
      num_packet = send_buf_get(&val);
375
      if (num_packet != num) {
376
        // not the correct packet
377
        
378
        if ((num_packet < num && num > 9 && num_packet < num-10)
379
            || (num_packet > num && num > 9)) {
380
          ackhandle(num_packet,1); // resend
381
          ackhandle(num,0); // redo this packet
382
          return;
383
        }
384
        
385
        WL_DEBUG_PRINT_P("|not correct packet");
386
        val += len;
387
        WL_DEBUG_PRINT_P("|send_packet_buf_pos:");
388
        WL_DEBUG_PRINT_INT(val);
389
        if (val >= PACKET_BUFFER_SIZE)
390
          val -= PACKET_BUFFER_SIZE;
391
        if (val == send_buf_last || ++packets >= send_buf_num_packets) {
392
          // reached end of bufer, so assume not present
393
          return;
394
        }
395
      } else {
396
        // remove packet
397
        WL_DEBUG_PRINT_P("|remove packet");
398
        num = val+len;
399
        val = (val<2)?PACKET_BUFFER_SIZE+val-2:val-2;
400
        if (num >= PACKET_BUFFER_SIZE)
401
          num -= PACKET_BUFFER_SIZE;
402
        while(num != send_buf_last) {
403
          if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)
404
            return; // error
405
        }
406
        send_buf_last = val; // set new end of buffer
407
        send_buf_num_packets--; // remove packet from count
408
        
409
        WL_DEBUG_PRINT_P("|send_packet_first:");
410
        WL_DEBUG_PRINT_INT(send_buf_first);
411
        WL_DEBUG_PRINT_P("|send_packet_last:");
412
        WL_DEBUG_PRINT_INT(send_buf_last);
413
        WL_DEBUG_PRINT_P("|num_send_packets:");
414
        WL_DEBUG_PRINT_INT(send_buf_num_packets);
415
        return;
416
      }
417
    }
418
    break;
419
  case 1:
420
    WL_DEBUG_PRINT_P("no ack\r\n");
421
    // no ack
422
    // check resend attempts
423
    val = send_buf_first;
424
    if (send_buf_num_packets == 0)
425
      return;
426
    while (1) {
427
      len = send_buf_get(&val);
428
      if (send_buf_get(&val) != num) {
429
        // not the correct packet, so continue
430
        val += len;
431
        if (val >= PACKET_BUFFER_SIZE)
432
          val -= PACKET_BUFFER_SIZE;
433
        if (val == send_buf_last) {
434
          // reached end of bufer, so assume not present
435
          setack(num,ACK_FAILURE); // mark as failed send
436
          return;
437
        }
438
      } else {
439
        // check attempts on packet
440
        num = val+len-1; // set to end of packet
441
        if (num >= PACKET_BUFFER_SIZE)
442
          num -= PACKET_BUFFER_SIZE;
443
        if (send_buf[num] < NUM_RETRIES) {
444
          val = (val<2)?PACKET_BUFFER_SIZE+val-2:val-2; // set to start of this packet
445
          // retry sending
446
          send_buf[num]++; // increment retries
447
          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // go back one byte
448
          if (xbee_send_header(len+1) != WL_SUCCESS) {
449
            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
450
            return;
451
          }
452
          len = XBEE_FRAME_TX_REQUEST_16;
453
          while(val!=num) {
454
            // compute checksum
455
            len += send_buf[val];
456
            if (xbee_putc(send_buf_get(&val)) != WL_SUCCESS) { // send  byte
457
              WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
458
              return;
459
            }
460
          }
461
          len = 0xFF - len;
462
          if (xbee_putc(len) != WL_SUCCESS) { // send
463
            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
464
            return;
465
          }
466
          return;
467
        } else {
468
          // done sending, mark as failed send
469
          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to frame num
470
          setack(send_buf[val],ACK_FAILURE);
471
          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to start of this packet
472
          // remove packet
473
          while(num != send_buf_last) {
474
            if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)
475
              return; // error
476
          }
477
          return;
478
        }
479
      }
480
    }    
481
    break; // shouldn't get here, but just in case
482
  case 2:
483
    WL_DEBUG_PRINT_P("CCA failure\r\n");
484
    // CCA failure
485
    // check resend attempts
486
    val = send_buf_first;
487
    if (send_buf_num_packets == 0)
488
      return;
489
    while (1) {
490
      len = send_buf_get(&val);
491
      if (send_buf_get(&val) != num) {
492
        // not the correct packet, so continue
493
        val += len;
494
        if (val >= PACKET_BUFFER_SIZE)
495
          val -= PACKET_BUFFER_SIZE;
496
        if (val == send_buf_last) {
497
          // reached end of bufer, so assume not present
498
          setack(num,CCA_FAILURE); // mark as failed send
499
          return;
500
        }
501
      } else {
502
        // check attempts on packet
503
        num = val+len-1; // set to end of packet
504
        if (num >= PACKET_BUFFER_SIZE)
505
          num -= PACKET_BUFFER_SIZE;
506
        if (send_buf[num] < NUM_RETRIES) {
507
          val = (val<2)?PACKET_BUFFER_SIZE+val-2:val-2; // set to start of this packet
508
          // retry sending
509
          send_buf[num]++; // increment retries
510
          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // go back one byte
511
          if (xbee_send_header(len+1) != WL_SUCCESS) {
512
            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
513
            return;
514
          }
515
          len = XBEE_FRAME_TX_REQUEST_16;
516
          while(val!=num) {
517
            // compute checksum
518
            len += send_buf[val];
519
            if (xbee_putc(send_buf_get(&val)) != WL_SUCCESS) { // send  byte
520
              WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
521
              return;
522
            }
523
          }
524
          len = 0xFF - len;
525
          if (xbee_putc(len) != WL_SUCCESS) { // send
526
            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
527
            return;
528
          }
529
          return;
530
        } else {
531
          // done sending, mark as failed send
532
          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to frame num
533
          setack(send_buf[val],CCA_FAILURE);
534
          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to start of this packet
535
          // remove packet
536
          while(num != send_buf_last) {
537
            if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)
538
              return; // error
539
          }
540
          return;
541
        }
542
      }
543
    }    
544
    break; // shouldn't get here, but just in case
545
  }
546
}
547

    
548
/* adds a byte to the send buffer */
549
int8_t send_buf_add(uint8_t *ptr, uint8_t byte) {
550
  if (*ptr == send_buf_first && send_buf_num_packets > 0) {
551
    // buffer full
552
    WL_DEBUG_PRINT_P("send buffer full\r\n");
553
    return WL_ERROR_SENDING_BUFFER_FULL;
554
  }
555
  send_buf[(*ptr)++] = byte;
556
  if (*ptr == PACKET_BUFFER_SIZE)
557
    *ptr = 0;
558
  return WL_SUCCESS;
559
}
560
/* gets a byte from the send buffer */
561
uint8_t send_buf_get(uint8_t *ptr) {
562
  uint8_t byte = send_buf[(*ptr)++];
563
  if (*ptr == PACKET_BUFFER_SIZE)
564
    *ptr = 0;
565
  return byte;
566
}