Revision 1751
wireless: update of wl_dispatch to match changes in wl_get_basic. Need to test wl_dispatch.
| 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