Revision 1751
wireless: update of wl_dispatch to match changes in wl_get_basic. Need to test wl_dispatch.
branches/wireless/code/projects/libwireless/wireless_receive.c | ||
---|---|---|
161 | 161 |
* @return error codes |
162 | 162 |
*/ |
163 | 163 |
int8_t wl_dispatch(void) { |
164 |
uint8_t buf_pos = other_buf_first; // current position in buffer |
|
164 |
uint8_t buf_pos = other_buf_first; // start at beginning of first (oldest) basic packet |
|
165 |
uint8_t data_length = 0xFF; |
|
165 | 166 |
uint8_t group; // group of packet under consideration |
166 | 167 |
FNPTR; // pointer to handler |
167 |
uint8_t source; // robot ID of sender of packet under consideration |
|
168 |
uint8_t data_length = 0xFF; |
|
168 |
uint8_t source=0; // robot ID of sender of packet under consideration |
|
169 |
|
|
170 |
while(1) { |
|
171 |
while((data_length = xbee_other_buf_get(&buf_pos)) == 0xFF) { // get packet length |
|
172 |
if (buf_pos == other_buf_last) |
|
173 |
break; |
|
174 |
} |
|
175 |
if (buf_pos == other_buf_last) |
|
176 |
return WL_SUCCESS; |
|
177 |
|
|
178 |
WL_DEBUG_PRINT_P("entered wl_dispatch\r\n"); |
|
169 | 179 |
|
170 |
while (1) { |
|
171 |
while((data_length = xbee_other_buf_get(&buf_pos)) == 0xFF) { } // get packet length |
|
172 |
|
|
173 |
// check if packet has not fully arrived yet |
|
180 |
// packet has not fully arrived yet |
|
174 | 181 |
if (buf_pos + data_length >= PACKET_BUFFER_SIZE) { |
175 | 182 |
if ((buf_pos + data_length - PACKET_BUFFER_SIZE) > other_buf_last) |
176 | 183 |
break; |
177 | 184 |
} else { |
178 | 185 |
if ((buf_pos + data_length) > other_buf_last) |
179 | 186 |
break; |
180 |
} |
|
187 |
} |
|
188 |
WL_DEBUG_PRINT_P("wl_dispatch() - packet has arrived\r\n"); |
|
181 | 189 |
|
182 | 190 |
// get group number |
183 | 191 |
group = xbee_other_buf_get(&buf_pos); |
184 | 192 |
if (group >= MAX_PACKET_GROUPS || wl_packet_handlers[group].FUNC == NULL) { |
185 | 193 |
// this is an invalid group, so go to next packet |
186 |
buf_pos += data_length - 2; |
|
187 |
if (buf_pos >= PACKET_BUFFER_SIZE) |
|
188 |
buf_pos -= PACKET_BUFFER_SIZE; |
|
194 |
if (buf_pos + data_length - 2 >= PACKET_BUFFER_SIZE) |
|
195 |
buf_pos += data_length - 2 - PACKET_BUFFER_SIZE; |
|
196 |
else |
|
197 |
buf_pos += data_length - 2; |
|
189 | 198 |
} else { |
190 | 199 |
// this is a valid group number, so get handler |
191 | 200 |
FUNC = wl_packet_handlers[group].FUNC; |
... | ... | |
196 | 205 |
(*FUNC)(xbee_other_buf+buf_pos,data_length-4,source); |
197 | 206 |
|
198 | 207 |
// go to next packet |
199 |
buf_pos += data_length - 4; |
|
200 |
if (buf_pos >= PACKET_BUFFER_SIZE) |
|
201 |
buf_pos -= PACKET_BUFFER_SIZE; |
|
208 |
if (buf_pos + data_length - 4 >= PACKET_BUFFER_SIZE) |
|
209 |
buf_pos += data_length - 4 - PACKET_BUFFER_SIZE; |
|
210 |
else |
|
211 |
buf_pos += data_length - 4; |
|
202 | 212 |
} |
203 | 213 |
} |
204 | 214 |
|
205 | 215 |
// mark packets as read |
206 | 216 |
other_buf_first = (buf_pos==0)?(PACKET_BUFFER_SIZE-1):(buf_pos-1); |
217 |
xbee_other_buf[other_buf_first] = 0xFF; |
|
207 | 218 |
|
208 | 219 |
return WL_SUCCESS; |
209 | 220 |
} |
Also available in: Unified diff