Statistics
| Revision:

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

History | View | Annotate | Download (16.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
  nextframe = (nextframe == 0xFF)?1:nextframe+1; // increment frame number
103
  
104
  // set scope
105
  if (scope == GLOBAL)
106
    packet[3] &= XBEE_OPTIONS_BROADCAST_ALL_PANS;
107
  else if (scope != PAN) {
108
    WL_DEBUG_PRINT_P("Error - bad scope in core send function\r\n");
109
    return WL_ERROR_SCOPE;
110
  }
111
    
112
  // set mode
113
  if (mode == FAST) {
114
    packet[3] &= XBEE_OPTIONS_DISABLE_RESPONSE;
115
  } else if (mode != RELIABLE) {
116
    WL_DEBUG_PRINT_P("Error - bad mode in core send function\r\n");
117
    return WL_ERROR_MODE;
118
  }
119
  
120
  // do checksum
121
  group = XBEE_FRAME_TX_REQUEST_16;
122
  xbee_checksum_add(packet,6,&group);
123
  xbee_checksum_add(data,length,&group); 
124
  group = 0xFF-group;
125

    
126
  // send the packet
127
  if (xbee_send_header(length+7) != WL_SUCCESS) {
128
    WL_DEBUG_PRINT_P("1Error sending packet from core send function\r\n");
129
    return WL_ERROR_SEND;
130
  }
131
  if (xbee_sendc(XBEE_FRAME_TX_REQUEST_16) != WL_SUCCESS) {
132
    WL_DEBUG_PRINT_P("2Error sending packet from core send function\r\n");
133
    return WL_ERROR_SEND;
134
  }
135
  if (xbee_send(packet,6) != WL_SUCCESS) {
136
    WL_DEBUG_PRINT_P("3Error sending packet from core send function\r\n");
137
    return WL_ERROR_SEND;
138
  }
139
  if (xbee_send(data,length) != WL_SUCCESS) {
140
    WL_DEBUG_PRINT_P("4Error sending packet from core send function\r\n");
141
    return WL_ERROR_SEND;
142
  }
143
  if (xbee_sendc(group) != WL_SUCCESS) {
144
    WL_DEBUG_PRINT_P("5Error sending packet from core send function\r\n");
145
    return WL_ERROR_SEND;
146
  }
147
  
148
  // save in ack system
149
  if (mode == FAST) {
150
    setack(nextframe,ACK_OK); // assume the send was successful
151
    nextframe = (nextframe == 0xFF)?1:nextframe+1; // increment frame number
152
    return ret; // no frame number
153
  } else if (mode == RELIABLE) {
154
    setack(nextframe,SENDING); // set status to SENDING
155
    // save packet on sending buffer
156
    scope = send_buf_last; // use as ptr to send buffer    
157
    if (send_buf_add(&scope,length+6) != WL_SUCCESS) { // add length
158
      WL_DEBUG_PRINT_P("Error adding length to sending buffer\r\n");
159
      ret = WL_ERROR_SENDING_BUFFER_FULL;
160
    }    
161
    for(mode=0;mode<6;mode++) { // add header
162
      if (send_buf_add(&scope,packet[mode]) != WL_SUCCESS) {
163
        WL_DEBUG_PRINT_P("Error adding header to sending buffer\r\n");
164
        ret = WL_ERROR_SENDING_BUFFER_FULL;
165
      }
166
    }
167
    for(mode=0;mode<length;mode++) { // add data
168
      if (send_buf_add(&scope,data[mode]) != WL_SUCCESS) {
169
        WL_DEBUG_PRINT_P("Error adding data to sendig buffer\r\n");
170
        ret = WL_ERROR_SENDING_BUFFER_FULL;
171
      }
172
    }
173
    if (send_buf_add(&scope,0) != WL_SUCCESS) { // add num retries=0
174
      WL_DEBUG_PRINT_P("Error adding num_retries to sending buffer\r\n");
175
      ret = WL_ERROR_SENDING_BUFFER_FULL;
176
    }
177
    send_buf_last = scope;
178
    send_buf_num_packets++;
179
    WL_DEBUG_PRINT_P("send_buf_last:");
180
    WL_DEBUG_PRINT_INT(send_buf_last);
181
    WL_DEBUG_PRINT_P("pos:");
182
    WL_DEBUG_PRINT_INT(scope);
183
    WL_DEBUG_PRINT_P("send_buf_num_packets:");
184
    WL_DEBUG_PRINT_INT(send_buf_num_packets);
185
    
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("|num_send_packets:");
349
  WL_DEBUG_PRINT_INT(send_buf_num_packets);
350
  WL_DEBUG_PRINT_P("\r\n");
351
  if (send_buf_num_packets == 0)
352
    return;
353
  switch(val) {
354
  case 0:
355
    WL_DEBUG_PRINT_P("ack success\r\n");
356
    // success
357
    setack(num,ACK_OK); // set status code
358
    // remove from buffer
359
    val = send_buf_first;
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
        if (val+len >= PACKET_BUFFER_SIZE)
399
          num = val+len-PACKET_BUFFER_SIZE;
400
        else
401
          num = val+len;
402
        val = (val<2)?PACKET_BUFFER_SIZE+val-2:val-2;
403
        while(num != send_buf_last) {
404
          if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)
405
            return; // error
406
        }
407
        send_buf_last = val; // set new end of buffer
408
        send_buf_num_packets--; // remove packet from count
409
        
410
        WL_DEBUG_PRINT_P("|send_packet_first:");
411
        WL_DEBUG_PRINT_INT(send_buf_first);
412
        WL_DEBUG_PRINT_P("|send_packet_last:");
413
        WL_DEBUG_PRINT_INT(send_buf_last);
414
        WL_DEBUG_PRINT_P("|num_send_packets:");
415
        WL_DEBUG_PRINT_INT(send_buf_num_packets);
416
        return;
417
      }
418
    }
419
    break;  
420
  case 2:
421
    WL_DEBUG_PRINT_P("CCA failure\r\n");
422
  case 1:
423
    WL_DEBUG_PRINT_P("no ack\r\n");
424
    // no ack
425
    // check resend attempts
426
    val = send_buf_first;
427
    if (send_buf_num_packets == 0)
428
      return;
429
    while (1) {
430
      len = send_buf_get(&val);
431
      if (send_buf[val] != num) {
432
        // not the correct packet, so continue
433
        val += len;
434
        if (val >= PACKET_BUFFER_SIZE)
435
          val -= PACKET_BUFFER_SIZE;
436
        if (val == send_buf_last) {
437
          // reached end of bufer, so assume not present
438
          setack(num,ACK_FAILURE); // mark as failed send
439
          return;
440
        }
441
      } else {
442
        // check attempts on packet
443
        if (val+len+1 >= PACKET_BUFFER_SIZE)
444
          num = val+len-PACKET_BUFFER_SIZE;
445
        else
446
          num = val+len; // set to end of packet
447
        if (send_buf[num] < NUM_RETRIES) {
448
          // retry sending          
449
          send_buf[num]++; // increment retries
450
          
451
          setack(send_buf[val],ACK_OK);
452
          send_buf[val] = nextframe; // reset frame number
453
          setack(nextframe,SENDING);
454
          nextframe = (nextframe == 0xFF)?1:nextframe+1; // increment frame number
455
          
456
          val = (val>0)?PACKET_BUFFER_SIZE+val-1:val-1; // set to start of this packet
457
          if (xbee_send_header(len+1) != WL_SUCCESS) {
458
            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
459
            return;
460
          }
461
          len = XBEE_FRAME_TX_REQUEST_16;
462
          while(val!=num) {
463
            // compute checksum
464
            len += send_buf[val];
465
            if (xbee_sendc(send_buf_get(&val)) != WL_SUCCESS) { // send  byte
466
              WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
467
              return;
468
            }
469
          }
470
          len = 0xFF - len;
471
          if (xbee_sendc(len) != WL_SUCCESS) { // send
472
            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");
473
            return;
474
          }
475
          return;
476
        } else {
477
          // done sending, mark as failed send
478
          setack(send_buf[num],ACK_FAILURE);
479
          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to start of this packet
480
          num = (num+1==PACKET_BUFFER_SIZE)?0:num+1; // set to end of this packet
481
          // remove packet
482
          while(num != send_buf_last) {
483
            if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)
484
              return; // error
485
          }
486
          return;
487
        }
488
      }
489
    }    
490
    break; // shouldn't get here, but just in case
491
  }
492
}
493

    
494
/* adds a byte to the send buffer */
495
int8_t send_buf_add(uint8_t *ptr, uint8_t byte) {
496
  if (*ptr == send_buf_first && send_buf_num_packets > 0) {
497
    // buffer full
498
    WL_DEBUG_PRINT_P("send buffer full\r\n");
499
    return WL_ERROR_SENDING_BUFFER_FULL;
500
  }
501
  send_buf[(*ptr)++] = byte;
502
  if (*ptr == PACKET_BUFFER_SIZE)
503
    *ptr = 0;
504
  return WL_SUCCESS;
505
}
506
/* gets a byte from the send buffer */
507
uint8_t send_buf_get(uint8_t *ptr) {
508
  uint8_t byte = send_buf[(*ptr)++];
509
  if (*ptr == PACKET_BUFFER_SIZE)
510
    *ptr = 0;
511
  return byte;
512
}