Statistics
| Revision:

root / branches / wireless / code / projects / libwireless / wireless_receive.c @ 1801

History | View | Annotate | Download (8.41 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_receive.c
28
 * @brief Wireless library receive functions
29
 *
30
 * Implementation of high level wireless communication.
31
 * This is the receive functions portion of the library.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 **/
35

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

    
41
/**
42
 * Definition for wireless library receive packet structure
43
 *
44
 * Basic Buffer Packet:
45
 *   byte 1: length of source+data
46
 *   bytes 2-3: source
47
 *   bytes 4-n: data
48
 *
49
 * Other Buffer Packet:
50
 *   byte 1: length of group+source+data
51
 *   byte 2: group number
52
 *   bytes 3-4: source
53
 *   bytes 5-n: data
54
 *
55
 **/
56

    
57
extern uint8_t xbee_basic_buf[PACKET_BUFFER_SIZE];  // buffer for basic-group packets
58
extern uint8_t xbee_other_buf[PACKET_BUFFER_SIZE];  // buffer for non-basic-group packets
59
extern uint8_t basic_buf_first;        // beginning of first packet in basic buffer
60
extern uint8_t basic_buf_last;        // end of last packet in basic buffer
61
extern uint8_t other_buf_first; // beginning of first packet in other buffer
62
extern uint8_t other_buf_last;  // end of last packet in other buffer
63
extern uint8_t xbee_basic_buf_get(uint8_t *ptr); // get byte from basic buf
64
extern uint8_t xbee_other_buf_get(uint8_t *ptr); // get byte from other buf
65

    
66
extern PacketGroupHandler wl_packet_handlers[MAX_PACKET_GROUPS];
67

    
68
/**
69
 * @addtogroup wireless Wireless
70
 * @{
71
 **/
72

    
73
/**
74
 * the main receive function (similar to wl_do)
75
 * 
76
 * when called, this function will receive the next packet on the default packet group
77
 * it will also dispatch registered packet handler functions for any other packet groups it has received
78
 *
79
 * @param data an already-initialized array to store the default group packet data in
80
 * @param length the length of the initialized data array
81
 *
82
 * @return the length of the used portion of data array or error (<0)
83
 **/
84
int8_t wl_get(char *data, uint8_t length) {
85
  int8_t return_code = wl_dispatch(); // run dispatch of other packet groups
86
  if (return_code != 0)
87
    return return_code; // error, so end early
88
  return wl_get_basic(data, length); // get a basic packet
89
}
90

    
91
/**
92
 * function to receive only packets on the default group
93
 * this function is only meant to receive packets sent using wl_send_basic()
94
 * 
95
 * @param data an already-initialized array to store the default group packet data in
96
 * @param length the length of the initialized data array
97
 * 
98
 * @return the length of the used portion of data array or error (<0)
99
 **/
100
int8_t wl_get_basic(char *data, uint8_t length) {
101
  uint8_t buf_pos = basic_buf_first;        // start at beginning of first (oldest) basic packet
102
  uint8_t data_length = 0xFF;
103
  while((data_length = xbee_basic_buf_get(&buf_pos)) == 0xFF) {  // get packet length
104
    if (buf_pos == basic_buf_last)
105
      break;
106
  }
107
  if (buf_pos == basic_buf_last)
108
      return WL_SUCCESS;
109
  if (data_length - 3 > length)   // not enough room for data in destination
110
    return WL_ERROR_TOO_SMALL;
111
  
112
  WL_DEBUG_PRINT_P("entered wl_get_basic\r\n");
113

    
114
  // packet has not fully arrived yet
115
  if (buf_pos + data_length >= PACKET_BUFFER_SIZE) {
116
    if ((buf_pos + data_length - PACKET_BUFFER_SIZE) > basic_buf_last)
117
      return WL_SUCCESS;
118
  } else {
119
    if ((buf_pos + data_length) > basic_buf_last)
120
      return WL_SUCCESS;
121
  }  
122
  WL_DEBUG_PRINT_P("wl_get_basic() - packet has arrived\r\n");
123
  
124
  // ignore the source ID
125
  xbee_basic_buf_get(&buf_pos);
126
  xbee_basic_buf_get(&buf_pos);
127
  
128
  // copy data to destination, byte by byte to account for circular buffer
129
  data_length-=2;
130
  uint8_t i;
131
  for(i=0;i<data_length;i++) {
132
    data[i] = xbee_basic_buf_get(&buf_pos);
133
  }
134
  WL_DEBUG_PRINT_P("we advanced ");
135
  WL_DEBUG_PRINT_INT(i+3);
136
  WL_DEBUG_PRINT_P(" bytes"); 
137
  WL_DEBUG_PRINT_P("|first=");
138
  WL_DEBUG_PRINT_INT(basic_buf_first);
139
  WL_DEBUG_PRINT_P("|last=");
140
  WL_DEBUG_PRINT_INT(basic_buf_last);
141
  
142
  // "free up" this packet's buffer space
143
  if (buf_pos == 0)
144
    basic_buf_first = PACKET_BUFFER_SIZE-1;
145
  else
146
    basic_buf_first = buf_pos-1;
147
    
148
  xbee_basic_buf[basic_buf_first] = 0xFF;
149
    
150
  WL_DEBUG_PRINT_P("|first=");
151
  WL_DEBUG_PRINT_INT(basic_buf_first);
152
  WL_DEBUG_PRINT_P("|last=");
153
  WL_DEBUG_PRINT_INT(basic_buf_last);
154
  WL_DEBUG_PRINT_P("\r\n");
155

    
156
  return data_length;
157
}
158

    
159
/*
160
 * function to dispatch all registered packet handlers for received packets on non-default groups
161
 *
162
 * @return error codes
163
 */
164
int8_t wl_dispatch(void) {
165
  uint8_t buf_pos = other_buf_first;        // start at beginning of first (oldest) basic packet
166
  uint8_t data_length = 0xFF;  
167
  uint8_t group;  // group of packet under consideration
168
  FNPTR; // pointer to handler
169
  uint8_t source=0; // robot ID of sender of packet under consideration
170
  
171
  WL_DEBUG_PRINT_P("entered wl_dispatch\r\n");
172
  WL_DEBUG_PRINT_P("other_buf_first=");
173
  WL_DEBUG_PRINT_INT(other_buf_first);
174
  WL_DEBUG_PRINT_P(" and other_buf_last=");
175
  WL_DEBUG_PRINT_INT(other_buf_last);
176
  
177
  while(1) {
178
    while((data_length = xbee_other_buf_get(&buf_pos)) == 0xFF) {  // get packet length
179
      WL_DEBUG_PRINT_P("|searching - data_length=");
180
      WL_DEBUG_PRINT_HEX(data_length);
181
      WL_DEBUG_PRINT_P(" on pos=");
182
      WL_DEBUG_PRINT_INT(buf_pos);
183
      if (buf_pos == other_buf_last)
184
        break;
185
    }
186
    if (buf_pos == other_buf_last) {
187
      WL_DEBUG_PRINT_P("|buf_pos=other_buf_last, buf_pos=");
188
      WL_DEBUG_PRINT_INT(buf_pos);
189
      WL_DEBUG_PRINT_P(" and other_buf_last=");
190
      WL_DEBUG_PRINT_INT(other_buf_last);
191
      WL_DEBUG_PRINT_P("\r\n");
192
      break;
193
    }       
194
    
195
    // packet has not fully arrived yet
196
    if (buf_pos + data_length >= PACKET_BUFFER_SIZE) {
197
      if ((buf_pos + data_length - PACKET_BUFFER_SIZE) > other_buf_last)
198
        break;
199
    } else {
200
      if ((buf_pos + data_length) > other_buf_last)
201
        break;
202
    }  
203
    WL_DEBUG_PRINT_P("wl_dispatch() - packet has arrived\r\n");
204
    
205
    // get group number
206
    group = xbee_other_buf_get(&buf_pos);
207
    WL_DEBUG_PRINT_P("group = ");
208
    WL_DEBUG_PRINT_HEX(group);
209
    WL_DEBUG_PRINT_P("|datalength = ");
210
    WL_DEBUG_PRINT_HEX(data_length);
211
    if (wl_packet_handlers[group].FUNC == NULL)
212
      WL_DEBUG_PRINT_P("|FUNC == NULL\r\n");
213
    if (group >= MAX_PACKET_GROUPS || wl_packet_handlers[group].FUNC == NULL) {
214
      // this is an invalid group, so go to next packet
215
      if (buf_pos + data_length - 2 >= PACKET_BUFFER_SIZE)
216
        buf_pos += data_length - 2 - PACKET_BUFFER_SIZE;
217
      else
218
        buf_pos += data_length - 2;
219
    } else {
220
      // this is a valid group number, so get handler
221
      FUNC = wl_packet_handlers[group].FUNC;
222
      xbee_other_buf_get(&buf_pos); // ignore first byte of source id
223
      source = xbee_other_buf_get(&buf_pos); // get low byte of source id
224
      
225
      // call handler
226
      (*FUNC)(xbee_other_buf+buf_pos,data_length-3,source);
227
      
228
      // go to next packet
229
      if (buf_pos + data_length - 4 >= PACKET_BUFFER_SIZE)
230
        buf_pos += data_length - 4 - PACKET_BUFFER_SIZE;
231
      else
232
        buf_pos += data_length - 4;
233
    }
234
  }
235
  
236
  WL_DEBUG_PRINT_P("other_buf_first=");
237
  WL_DEBUG_PRINT_INT(other_buf_first);
238
    
239
  
240
  // mark packets as read
241
  other_buf_first = (buf_pos==0)?(PACKET_BUFFER_SIZE-1):(buf_pos-1);
242
  xbee_other_buf[other_buf_first] = 0xFF;
243

    
244
  WL_DEBUG_PRINT_P("Exit wl dispatch\r\n");  
245
  WL_DEBUG_PRINT_P("other_buf_first=");
246
  WL_DEBUG_PRINT_INT(other_buf_first);
247
  WL_DEBUG_PRINT_P("\r\n");
248

    
249
  return WL_SUCCESS;
250
}
251

    
252

    
253
/**@} **/ //end defgroup
254

    
255

    
256
// Define all private functions down here
257

    
258

    
259