Project

General

Profile

Revision 1345

Added by Rich Hong over 11 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

xbee.c
1
/**
2
 * Copyright (c) 2007 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 xbee.c
28
 * @brief XBee Interface
29
 *
30
 * Implementation of low level communication with the XBee in API mode.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

  
1 35
#include "xbee.h"
2 36
#include "wl_defs.h"
3 37

  
......
7 41
#include <unistd.h>
8 42
#include <pthread.h>
9 43
#include <errno.h>
44
#include <termios.h>
10 45

  
11 46
#else
12 47

  
......
19 54
#include <stdlib.h>
20 55
#include <string.h>
21 56

  
22
#include <queue.h>
23

  
24 57
#define XBEE_FRAME_START 0x7E
58
#define XBEE_GET_PACKET_TIMEOUT 1000
25 59

  
26 60
/*Frame Types*/
27 61
#define XBEE_FRAME_STATUS 0x8A
......
36 70
/*Internal Function Prototypes*/
37 71

  
38 72
/*I/O Functions*/
39
void xbee_send(char* buf, int size);
40
void xbee_send_string(char* c);
73
static int xbee_send(char* buf, int size);
74
static int xbee_send_string(char* c);
41 75

  
42 76
#ifndef ROBOT
43
void xbee_read(char* buf, int size);
77
static int xbee_read(char* buf, int size);
44 78
#endif
45 79

  
46 80
/*Command Mode Functions
47 81
 * Called during initialization.
48 82
 */
49
void xbee_enter_command_mode(void);
50
void xbee_exit_command_mode(void);
51
void xbee_enter_api_mode(void);
52
void xbee_wait_for_string(char* s, int len);
53
void xbee_wait_for_ok(void);
83
static int xbee_enter_command_mode(void);
84
static int xbee_exit_command_mode(void);
85
static int xbee_enter_api_mode(void);
86
static int xbee_wait_for_string(char* s, int len);
87
static int xbee_wait_for_ok(void);
54 88

  
55 89
/*API Mode Functions*/
56 90

  
57
int xbee_handle_packet(char* packet, int len);
58
void xbee_handle_at_command_response(char* command, char result,
59
	char* extra, int extraLen);
60
void xbee_handle_status(char status);
61
int xbee_verify_checksum(char* packet, int len);
62
char xbee_compute_checksum(char* packet, int len);
63
void xbee_send_frame(char* buf, int len);
64
void xbee_send_read_at_command(char* command);
65
void xbee_send_modify_at_command(char* command, char* value);
91
static int xbee_handle_packet(char* packet, int len);
92
static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen);
93
static void xbee_handle_status(char status);
94
static int xbee_verify_checksum(char* packet, int len);
95
static char xbee_compute_checksum(char* packet, int len);
96
static int xbee_send_frame(char* buf, int len);
97
static int xbee_send_read_at_command(char* command);
98
static int xbee_send_modify_at_command(char* command, char* value);
66 99

  
67 100
/*Global Variables*/
68 101

  
69 102
#ifndef ROBOT
70
int xbee_stream;
71
pthread_t* xbee_listen_thread;
103
static char* xbee_com_port = XBEE_PORT_DEFAULT;
104
static int xbee_stream;
105
static pthread_t* xbee_listen_thread;
72 106
#endif
73 107

  
74
Queue* xbee_queue;
108
// TODO: is this a good size?
109
#define XBEE_BUFFER_SIZE	128
110
#define PACKET_BUFFER_SIZE	108
111
// a buffer for data received from the XBee
112
char arrival_buf[XBEE_BUFFER_SIZE];
113
// location of last unread byte in buffer
114
volatile int buffer_last = 0;
115
// first unread byte in buffer
116
volatile int buffer_first = 0;
75 117

  
118

  
76 119
//used to store packets as they are read
77
char xbee_buf[128];
78
int currentBufPos = 0;
120
static char xbee_buf[PACKET_BUFFER_SIZE];
121
static int currentBufPos = 0;
79 122

  
80 123
//XBee status
81
unsigned int xbee_panID = XBEE_PAN_DEFAULT;
82
unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;
83
int xbee_channel = XBEE_CHANNEL_DEFAULT;
84
int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;
85
unsigned int xbee_address = 0;
124
static unsigned int xbee_panID = XBEE_PAN_DEFAULT;
125
static unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;
126
static int xbee_channel = XBEE_CHANNEL_DEFAULT;
127
static int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;
128
static volatile unsigned int xbee_address = 0;
86 129

  
87 130
/*Function Implementations*/
88 131

  
......
90 133

  
91 134
/**
92 135
 * Interrupt for the robot. Adds bytes received from the xbee
93
 * to the queue.
136
 * to the buffer.
94 137
 **/
95 138
#ifndef FIREFLY
96 139
ISR(USART1_RX_vect)
97 140
{
98 141
	char c = UDR1;
99
	queue_add(xbee_queue, (void*)(int)c);
142
	arrival_buf[buffer_last] = c;
143
	int t = buffer_last + 1;
144
	if (t == XBEE_BUFFER_SIZE)
145
		t = 0;
146
	if (t == buffer_first)
147
	{
148
		WL_DEBUG_PRINT("\nOut of space in buffer.\n");
149
	}
150
	buffer_last = t;
100 151
}
101 152
#else
102 153
SIGNAL(SIG_USART0_RECV)
103 154
{
104 155
	char c = UDR0;
105
	queue_add(xbee_queue, (void*)(int)c);
156
	arrival_buf[buffer_last] = c;
157
	int t = buffer_last + 1;
158
	if (t == XBEE_BUFFER_SIZE)
159
		t = 0;
160
	if (t == buffer_first)
161
	{
162
		WL_DEBUG_PRINT("Out of space in buffer.\n");
163
	}
164
	buffer_last = t;
106 165
}
107 166
#endif
108 167

  
109 168
#else
110 169

  
170
// Computer code
171

  
111 172
/**
112 173
 * Thread that listens to the xbee.
113 174
 **/
114
void* listen_to_xbee(void* x)
175
static void* listen_to_xbee(void* x)
115 176
{
116 177
	char c;
117 178
	while (1)
118 179
	{
119
		xbee_read(&c, 1);
120
		queue_add(xbee_queue, (void*)(int)c);
180
		if (xbee_read(&c, 1) != 0) {
181
			WL_DEBUG_PRINT("xbee_read failed.\n");
182
			return NULL;
183
		}
184

  
185
		arrival_buf[buffer_last] = c;
186
		int t = buffer_last + 1;
187
		if (t == XBEE_BUFFER_SIZE)
188
			t = 0;
189
		if (t == buffer_first)
190
		{
191
			WL_DEBUG_PRINT("Out of space in buffer.\n");
192
		}
193
		buffer_last = t;
194

  
195
		usleep(1000);
121 196
	}
122
	return 0;
197

  
198
	return NULL;
123 199
}
124 200

  
125 201
#endif
126 202

  
127 203
/**
128 204
 * Initializes the XBee library so that other functions may be used.
129
 *
130
 * @param pan_id the PAN to join initially. Use XBEE_PAN_DEFAULT
131
 * to leave the PAN as it is initially.
132 205
 **/
133
void xbee_lib_init(void)
206
int xbee_lib_init()
134 207
{
135
	xbee_queue = queue_create();
136
	
137
	#ifdef ROBOT
208
	WL_DEBUG_PRINT("in xbee_init\n");
209
#ifdef ROBOT
138 210

  
139 211
	//enable the receiving interrupt
140
	#ifdef FIREFLY
212
#ifdef FIREFLY
141 213
	UCSR0B |= _BV(RXCIE) | _BV(RXEN);
142
	#else
214
#else
215
#ifdef BAYBOARD
216
	UCSR1B |= _BV(RXCIE1);
217
#else
143 218
	UCSR1B |= _BV(RXCIE);
144
	#endif
219
#endif
220
#endif
145 221
	sei();
146
	#else
147
	xbee_stream = open("/dev/ttyUSB0", O_RDWR);
148
	if (xbee_stream == -1 || lockf(xbee_stream, F_TEST, 0) != 0)
149
		xbee_stream = open("/dev/ttyUSB1", O_RDWR);
150
	if (xbee_stream == -1 || lockf(xbee_stream, F_TEST, 0) != 0)
222
#else
223
	xbee_stream = open(xbee_com_port, O_RDWR);
224
	if (xbee_stream == -1/* || lockf(xbee_stream, F_TEST, 0) != 0*/)
151 225
	{
152
		printf("Failed to open connection to XBee.\r\n");
153
		exit(0);
226
		WL_DEBUG_PRINT("Failed to open connection to XBee on port ");
227
		WL_DEBUG_PRINT_INT(xbee_com_port);
228
		WL_DEBUG_PRINT(".\n");
229
		return -1;
230
	} else {
231
	  WL_DEBUG_PRINT("Successfully opened connection to XBee on port ");
232
		WL_DEBUG_PRINT_INT(xbee_com_port);
233
		WL_DEBUG_PRINT(".\n");
154 234
	}
155
	lockf(xbee_stream, F_LOCK, 0);
156
	
157
	xbee_listen_thread =
158
		(pthread_t*)malloc(sizeof(pthread_t));
159
	
160
	int ret = pthread_create(xbee_listen_thread, NULL, 
161
		listen_to_xbee, NULL);
235

  
236
	// set baud rate, etc. correctly
237
	struct termios options;
238

  
239
	tcgetattr(xbee_stream, &options);
240
	cfsetispeed(&options, B9600);
241
	cfsetospeed(&options, B9600);
242
	options.c_iflag &= ~ICRNL;
243
	options.c_oflag &= ~OCRNL;
244
	options.c_cflag |= (CLOCAL | CREAD);
245
	options.c_cflag &= ~PARENB;
246
	options.c_cflag &= ~CSTOPB;
247
	options.c_cflag &= ~CSIZE;
248
	options.c_cflag |= CS8;
249
	options.c_lflag &= ~ICANON;
250
	options.c_cc[VMIN] = 1;
251
	options.c_cc[VTIME] = 50;
252

  
253
	if (tcsetattr(xbee_stream, TCSANOW, &options))
254
	{
255
		WL_DEBUG_PRINT("Error setting attributes.\n");
256
		return -1;
257
	}
258

  
259
	xbee_listen_thread = (pthread_t*)malloc(sizeof(pthread_t));
260
	if (xbee_listen_thread == NULL)
261
	{
262
		WL_DEBUG_PRINT("Malloc failed.\n");
263
		return -1;
264
	}
265

  
266
	int ret = pthread_create(xbee_listen_thread, NULL, listen_to_xbee, NULL);
162 267
	if (ret)
163 268
	{
164
		printf("Failed to create listener thread.\r\n");
165
		exit(0);
269
		WL_DEBUG_PRINT("Failed to create listener thread.\n");
270
		return -1;
166 271
	}
167
	#endif
168
	xbee_enter_command_mode();
169
	xbee_enter_api_mode();
170
	xbee_exit_command_mode();
171
	xbee_send_read_at_command("MY");
272
#endif
273

  
274
	WL_DEBUG_PRINT("Entering command mode.\n");
275

  
276
	if (xbee_enter_command_mode() != 0) {
277
		return -1;
278
	}
279

  
280
	WL_DEBUG_PRINT("Entered command mode.\n");
281

  
282
	if (xbee_enter_api_mode() != 0) {
283
		return -1;
284
	}
285

  
286
	WL_DEBUG_PRINT("Entered api mode.\n");
287

  
288
	if (xbee_exit_command_mode() != 0) {
289
	     return -1;
290
	}
172 291
	
292
	WL_DEBUG_PRINT("Left command mode.\n");
293

  
294
	if (xbee_send_read_at_command("MY")) {
295
		return -1;
296
	}
297
	WL_DEBUG_PRINT("Getting ATMY address.\n");
298

  
299
#ifndef ROBOT
300
	int i;
301
	for (i = 0; xbee_address == 0 && i < XBEE_GET_PACKET_TIMEOUT; i++) {
302
	  ret = xbee_get_packet(NULL);
303

  
304
	  usleep(1000);
305
	  
306
/* 	  if (ret == -1) { */
307
/* 	    WL_DEBUG_PRINT("xbee_get_packet(NULL) failed.\n"); */
308
/* 	    return -1; */
309
/* 	  } */
310
	}
311
#else
173 312
	//wait to return until the address is set
174
	while (xbee_address == 0) xbee_get_packet(NULL);
313
  //TODO: this shouldn't wait indefinitely.  There should be some sort of reasonable timeout
314
  // so if the address is never set right, an error can be returned instead of having the
315
  // robot hang forever
316
	while (xbee_address == 0) {
317
	  xbee_get_packet(NULL);
318
	}
319
#endif
320
	WL_DEBUG_PRINT("Got ATMY address.\n");
321

  
322
#ifndef ROBOT
323
	if (i == XBEE_GET_PACKET_TIMEOUT) { // We timed-out.
324

  
325
	  WL_DEBUG_PRINT("xbee_get_packet timed out.\n");
326
	  return -1;
327
	} else {
328
	  return 0;
329
	}
330
#else
331
	return 0;
332
#endif
175 333
}
176 334

  
177 335
/**
......
186 344
	lockf(xbee_stream, F_ULOCK, 0);
187 345
	close(xbee_stream);
188 346
	#endif
189
	queue_destroy(xbee_queue);
190 347
}
191 348

  
192 349
/**
193 350
 * Send a buffer buf of size bytes to the XBee.
194
 * 
351
 *
195 352
 * @param buf the buffer of data to send
196 353
 * @param size the number of bytes to send
197 354
 **/
198
void xbee_send(char* buf, int size)
355
static int xbee_send(char* buf, int size)
199 356
{
200
	#ifdef ROBOT
357
#ifdef ROBOT
201 358
	int i;
202
	for (i = 0; i < size; i++)
359
	for (i = 0; i < size; i++) {
203 360
		xbee_putc(buf[i]);
204
	#else
361
	}
362

  
363
	return 0;
364

  
365
#else
366

  
205 367
	int ret = write(xbee_stream, buf, size);
206 368
	//success
207 369
	if (ret == size)
208
		return;
370
		return 0;
209 371
	if (ret == -1)
210 372
	{
211 373
		//interrupted by system signal, probably timer interrupt.
212 374
		//just try again
213 375
		if (errno == 4)
214 376
		{
215
			xbee_send(buf, size);
216
			return;
377
			return xbee_send(buf, size);
217 378
		}
218
		printf("Failed to write to xbee, error %i.\r\n", errno);
219
		return;
379
		WL_DEBUG_PRINT("Failed to write to xbee\r\n");
380
		return -1;
220 381
	}
221 382

  
222 383
	//write was interrupted after writing ret bytes
223
	xbee_send(buf + ret, size - ret);
224
	#endif
384
	return xbee_send(buf + ret, size - ret);
385
#endif
225 386
}
226 387

  
227 388
/**
......
229 390
 *
230 391
 * @param c the string to send to the XBEE
231 392
 **/
232
void xbee_send_string(char* c)
393
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
394
// it reduces code size or not should be done to be sure.
395
static int xbee_send_string(char* c)
233 396
{
234
	xbee_send(c, strlen(c));
397
	return xbee_send(c, strlen(c));
235 398
}
236 399

  
237 400
#ifndef ROBOT
238
void xbee_read(char* buf, int size)
401
static int xbee_read(char* buf, int size)
239 402
{
240
	if (read(xbee_stream, buf, size) == -1)
241
		printf("Failed to read from xbee.\r\n");
403
	if (read(xbee_stream, buf, size) == -1) {
404
		WL_DEBUG_PRINT("Failed to read from xbee.\r\n");
405
		return -1;
406
	}
407

  
408
	return 0;
242 409
}
243 410
#endif
244 411

  
245 412
/**
246 413
 * Enter into command mode.
247 414
 **/
248
void xbee_enter_command_mode()
415
static int xbee_enter_command_mode()
249 416
{
250
	xbee_send_string("+++");
251
	xbee_wait_for_ok();
417
	if (xbee_send_string("+++") != 0) {
418
		return -1;
419
	}
420

  
421
	if (xbee_wait_for_ok() != 0) {
422
	  return -1;
423
	}
424
	  return 0;
252 425
}
253 426

  
254 427
/**
255 428
 * Exit from command mode.
256 429
 **/
257
void xbee_exit_command_mode()
430
static int xbee_exit_command_mode()
258 431
{
259
	xbee_send_string("ATCN\r");
432
	if (xbee_send_string("ATCN\r") != 0) {
433
		return -1;
434
	}
435

  
260 436
	xbee_wait_for_ok();
437

  
438
	return 0;
261 439
}
262 440

  
263 441
/**
264 442
 * Enter API mode.
265 443
 **/
266
void xbee_enter_api_mode()
444
static int xbee_enter_api_mode()
267 445
{
268
	xbee_send_string("ATAP 1\r");
446
	if (xbee_send_string("ATAP 1\r") != 0) {
447
		return -1;
448
	}
269 449
	xbee_wait_for_ok();
450

  
451
	return 0;
270 452
}
271 453

  
272 454
/**
273 455
 * Wait until the string "OK\r" is received from the XBee.
274 456
 **/
275
void xbee_wait_for_ok()
457
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
458
// it reduces code size or not should be done to be sure.
459
static int xbee_wait_for_ok()
276 460
{
277
	xbee_wait_for_string("OK\r", 3);
461
	return xbee_wait_for_string("OK\r", 3);
278 462
}
279 463

  
280 464
/**
......
284 468
 * @param s the string to receive
285 469
 * @param len the length of the string
286 470
 **/
287
void xbee_wait_for_string(char* s, int len)
471
static int xbee_wait_for_string(char* s, int len)
288 472
{
289 473
	char* curr = s;
290
	while (curr - s < len)
291
	{
292
		if (queue_is_empty(xbee_queue))
293
			continue;
294
		char c = (char)(int)queue_remove(xbee_queue);
295
		if (c == *curr)
296
			curr++;
297
		else
298
			curr = s;
474
	while (curr - s < len) {
475
		// check if buffer is empty
476
		if (buffer_last != buffer_first) {
477
			char c = arrival_buf[buffer_first++];
478
			if (buffer_first == XBEE_BUFFER_SIZE) {
479
				buffer_first = 0;
480
			}
481

  
482
			if (c == *curr) {
483
				curr++;
484
			} else {
485
#ifndef ROBOT
486
			  //return -1; // Computer is less forgiving.
487
			  curr = s;
488
#else
489
			  curr = s;
490
#endif
491
			}
492
		} // else buffer is empty.
493

  
494
#ifndef ROBOT
495
		usleep(100);
496
#endif
299 497
	}
498

  
499
	return 0;
300 500
}
301 501

  
302 502
/**
......
348 548
 * @param len the size in bytes of the packet data
349 549
 *
350 550
 **/
351
void xbee_send_frame(char* buf, int len)
551
static int xbee_send_frame(char* buf, int len)
352 552
{
353 553
	char prefix[3];
354 554
	prefix[0] = XBEE_FRAME_START;
355 555
	prefix[1] = (len & 0xFF00) >> 8;
356 556
	prefix[2] = len & 0xFF;
357 557
	char checksum = xbee_compute_checksum(buf, len);
358
	xbee_send(prefix, 3);
359
	xbee_send(buf, len);
360
	xbee_send(&checksum, 1);
558

  
559
	if (xbee_send(prefix, 3) != 0) {
560
		return -1;
561
	}
562

  
563
	if (xbee_send(buf, len) != 0) {
564
		return -1;
565
	}
566

  
567
	if (xbee_send(&checksum, 1) != 0) {
568
		return -1;
569
	}
570

  
571
	return 0;
361 572
}
362 573

  
363 574
/**
......
367 578
 * use ID to read the PAN ID and MY to return the XBee ID.
368 579
 * See the XBee reference guide for a complete listing.
369 580
 **/
370
void xbee_send_read_at_command(char* command)
581
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
582
// it reduces code size or not should be done to be sure.
583
static int xbee_send_read_at_command(char* command)
371 584
{
372
	xbee_send_modify_at_command(command, NULL);
585
	return xbee_send_modify_at_command(command, NULL);
373 586
}
374 587

  
375 588
/**
......
379 592
 * @param value the value to pass as a parameter
380 593
 * (or NULL if there is no parameter)
381 594
 **/
382
void xbee_send_modify_at_command(char* command, char* value)
595
static int xbee_send_modify_at_command(char* command, char* value)
383 596
{
384 597
	char buf[16];
385 598
	int i;
386
	
599

  
387 600
	buf[0] = XBEE_FRAME_AT_COMMAND;
388 601
	buf[1] = 1;
389 602
	buf[2] = command[0];
......
395 608
		if (valueLen > 8)
396 609
		{
397 610
			WL_DEBUG_PRINT("AT Command too large.\r\n");
398
			return;
611
			return -1;
399 612
		}
400
		for (i = 0; i < valueLen; i++)
613

  
614
		for (i = 0; i < valueLen; i++) {
401 615
			buf[4 + i] = value[i];
616
		}
402 617
	}
403
	xbee_send_frame(buf, 4 + valueLen);
618

  
619
	return xbee_send_frame(buf, 4 + valueLen);
404 620
}
405 621

  
406 622
/**
407 623
 * Send the specified packet.
408
 * 
624
 *
409 625
 * @param packet the packet data to send
410 626
 * @param len the number of bytes in the packet
411
 * 
627
 *
412 628
 * @param dest the ID of the XBee to send the packet to,
413 629
 * or XBEE_BROADCAST to send the message to all robots
414 630
 * in the PAN.
415
 * 
631
 *
416 632
 * @param options a combination of the flags
417
 * XBEE_OPTIONS_NONE, XBEE_OPTIONS_DISABLE_RESPONSE and 
633
 * XBEE_OPTIONS_NONE, XBEE_OPTIONS_DISABLE_RESPONSE and
418 634
 * XBEE_OPTIONS_BROADCAST_ALL_PANS
419 635
 *
420 636
 * @param frame the frame number to associate this packet
......
422 638
 * the XBee alerts us as to whether or not our message
423 639
 * was received.
424 640
 **/
425
void xbee_send_packet(char* packet, int len, int dest,
426
	char options, char frame)
641
int xbee_send_packet(char* packet, int len, int dest, char options, char frame)
427 642
{
428 643
	char buf[5];
429 644
	char prefix[3];
......
433 648
	if (len > 100)
434 649
	{
435 650
		WL_DEBUG_PRINT("Packet is too large.\r\n");
436
		return;
651
		return -1;
437 652
	}
438 653

  
439 654
	//data for sending request
......
453 668
	for (i = 0; i < len; i++)
454 669
		checksum += (unsigned char)packet[i];
455 670
	checksum = 0xFF - checksum;
456
	xbee_send(prefix, 3);
457
	xbee_send(buf, 5);
458
	xbee_send(packet, len);
459
	xbee_send((char*)&checksum, 1);
671

  
672
	if (xbee_send(prefix, 3) != 0) {
673
		return -1;
674
	}
675

  
676
	if (xbee_send(buf, 5) != 0) {
677
		return -1;
678
	}
679

  
680
	if (xbee_send(packet, len) != 0) {
681
		return -1;
682
	}
683

  
684
	if (xbee_send((char*)&checksum, 1) != 0) {
685
		return -1;
686
	}
687

  
688
	return 0;
460 689
}
461 690

  
462 691
/**
......
471 700
 *
472 701
 * The first byte of the packet will be either
473 702
 * XBEE_TX_STATUS or XBEE_RX to indicated
474
 * a response to a sent message or a received message, 
703
 * a response to a sent message or a received message,
475 704
 * respectively.<br><br>
476 705
 *
477 706
 * For a status response packet:<br>
478 707
 * The first byte will be XBEE_TX_STATUS.<br>
479 708
 * The second byte will be the frame number.<br>
480 709
 * The third byte will be the result. 0 indicates success,
481
 * and nonzero indicates that an error ocurred in 
710
 * and nonzero indicates that an error ocurred in
482 711
 * transmitting the packet.<br><br>
483 712
 *
484 713
 * For a received packet:<br>
......
488 717
 * The fourth byte is the signal strength.<br>
489 718
 * The fifth byte is 1 if the packet were sent to
490 719
 * a specific address, and 2 if it is a broadcast packet.<br><br>
491
 * 
720
 *
492 721
 * @param dest set to the packet data
493 722
 * @return the length of the packet, or -1 if no packet
494 723
 * is available
495 724
 **/
496 725
int xbee_get_packet(unsigned char* dest)
497 726
{
727
     int ret;
498 728
	//start reading a packet with XBEE_FRAME_START
499 729
	if (currentBufPos == 0)
500 730
	{
501 731
		do
502
			if (queue_is_empty(xbee_queue))
732
		{
733
			if (buffer_first == XBEE_BUFFER_SIZE)
734
				buffer_first = 0;
735
			// check if buffer is empty
736
			if (buffer_first == buffer_last) {
503 737
				return -1;
504
		while ((char)(int)queue_remove(xbee_queue) != XBEE_FRAME_START);
738
			}
739
		} while (arrival_buf[buffer_first++] != XBEE_FRAME_START);
740

  
741
		if (buffer_first == XBEE_BUFFER_SIZE) {
742
			buffer_first = 0;
743
		}
505 744
		xbee_buf[0] = XBEE_FRAME_START;
506 745
		currentBufPos++;
507 746
	}
508 747

  
509 748
	int len = -1;
510
	if (currentBufPos >= 3)
749
	if (currentBufPos >= 3) {
511 750
		len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
512
		
751
	}
752

  
513 753
	while (len == -1 //packet length has not been read yet
514
			|| currentBufPos < len + 4)
754
		|| currentBufPos < len + 4)
515 755
	{
516 756
		if (currentBufPos == 3)
757
		{
517 758
			len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
518
		if (queue_is_empty(xbee_queue))
759
			if (len > 120)
760
			{
761
				WL_DEBUG_PRINT("Packet too large. Probably error in XBee transmission.\n");
762
				currentBufPos = 0;
763
				return -1;
764
			}
765
		}
766

  
767
		// check if buffer is empty
768
		if (buffer_first == buffer_last) {
519 769
			return -1;
520
		xbee_buf[currentBufPos++] = (char)(int)queue_remove(xbee_queue);
770
		}
771
		xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
772
		if (buffer_first == XBEE_BUFFER_SIZE) {
773
			buffer_first = 0;
774
		}
521 775
	}
522
	
776

  
523 777
	currentBufPos = 0;
524
	
778

  
525 779
	if (!xbee_verify_checksum(xbee_buf, len + 4))
526 780
	{
527 781
		WL_DEBUG_PRINT("XBee checksum failed.\r\n");
......
529 783
	}
530 784

  
531 785
	//we will take care of the packet
532
	if (xbee_handle_packet(xbee_buf + 3, len))
533
		return -1;
534 786
	
535
	if (dest == NULL)
787
	ret = xbee_handle_packet(xbee_buf+3, len);
788
	if (ret == 1) {
789
		return 3;
790
	}
791
	
792
	if (dest == NULL) {
536 793
		return -1;
537
	
794
	}
795

  
538 796
	int i;
539
	for (i = 3; i < len + 3; i++)
797
	for (i = 3; i < len + 3; i++) {
540 798
		dest[i - 3] = xbee_buf[i];
799
	}
541 800
	return len;
542 801
}
543 802

  
......
581 840
 * @param extra the hex value of the requested register
582 841
 * @param extraLen the length in bytes of extra
583 842
 **/
584
void xbee_handle_at_command_response(char* command, char result,
585
	char* extra, int extraLen)
843
static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen)
586 844
{
587 845
	if (result == 1)
588 846
	{
......
593 851
	WL_DEBUG_PRINT("AT");
594 852
	WL_DEBUG_PRINT(command);
595 853
	WL_DEBUG_PRINT(" command was successful.\r\n");
596
		
854

  
597 855
	if (command[0] == 'I' && command[1] == 'D')
598 856
	{
599 857
		xbee_panID = xbee_pending_panID;
......
611 869
		WL_DEBUG_PRINT(".\r\n");
612 870
		return;
613 871
	}
614
	
872

  
615 873
	if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)
616 874
	{
617 875
		xbee_address = 0;
618 876
		int i;
619
		for (i = 0; i < extraLen; i++)
877
		for (i = 0; i < extraLen; i++) {
620 878
			xbee_address = (xbee_address << 8) + extra[i];
879
		}
621 880

  
622 881
		WL_DEBUG_PRINT("XBee address is ");
623 882
		WL_DEBUG_PRINT_INT(xbee_address);
......
626 885
		if (xbee_address == 0)
627 886
		{
628 887
			WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");
888
			#ifndef ROBOT
629 889
			exit(0);
890
			#endif
630 891
		}
631 892
	}
632 893
}
......
640 901
 *
641 902
 * @param packet the packet to handle
642 903
 * @param len the length of the packet
643
 * 
904
 *
644 905
 * @return 1 if we have handled the packet, 0 otherwise
645 906
 */
646
int xbee_handle_packet(char* packet, int len)
907
static int xbee_handle_packet(char* packet, int len)
647 908
{
909

  
648 910
	char command[3] = {1, 2, 3};
649 911
	if (len <= 0) //this should not happend
650 912
	{
651 913
		WL_DEBUG_PRINT("Non-positive packet length.\r\n");
652 914
		return 0;
653 915
	}
654
	
916

  
655 917
	switch ((unsigned char)packet[0]) //packet type
656 918
	{
657 919
		case XBEE_FRAME_STATUS:
......
661 923
			command[0] = packet[2];
662 924
			command[1] = packet[3];
663 925
			command[2] = 0;
664
			xbee_handle_at_command_response(command,
665
				packet[4], packet + 5, len - 5);
926
			xbee_handle_at_command_response(command, packet[4], packet + 5, len - 5);
666 927
			return 1;
667 928
	}
668 929
	return 0;
......
673 934
 *
674 935
 * @param id the new personal area network (PAN) id
675 936
 **/
676
void xbee_set_pan_id(int id)
937
int xbee_set_pan_id(int id)
677 938
{
678 939
	char s[3];
679 940
	s[0] = (id >> 8) & 0xFF;
680 941
	s[1] = id & 0xFF;
681 942
	s[2] = 0;
682 943
	xbee_pending_panID = id;
683
	xbee_send_modify_at_command("ID", s);
944
	return xbee_send_modify_at_command("ID", s);
684 945
}
685 946

  
686 947
/**
687 948
 * Get the PAN ID for the XBee.
688
 * 
949
 *
689 950
 * @return the personal area network id, or
690 951
 * XBEE_PAN_DEFAULT if it has not yet been set.
691 952
 **/
953
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
954
// it reduces code size or not should be done to be sure.
692 955
unsigned int xbee_get_pan_id()
693 956
{
694 957
	return xbee_panID;
......
697 960
/**
698 961
 * Set the channel the XBee is using.
699 962
 *
700
 * @param channel the channel the XBee will not use, 
963
 * @param channel the channel the XBee will not use,
701 964
 * between 0x0B and 0x1A
702 965
 *
703 966
 * @see xbee_get_channel
704 967
 **/
705
void xbee_set_channel(int channel)
968
int xbee_set_channel(int channel)
706 969
{
707 970
	if (channel < 0x0B || channel > 0x1A)
708 971
	{
709 972
		WL_DEBUG_PRINT("Channel out of range.\r\n");
710
		return;
973
		return -1;
711 974
	}
975

  
712 976
	char s[3];
713 977
	s[0] = channel & 0xFF;
714 978
	s[1] = 0;
715 979
	xbee_pending_channel = channel;
716
	xbee_send_modify_at_command("CH", s);
980

  
981
	return xbee_send_modify_at_command("CH", s);
717 982
}
718 983

  
719 984
/**
......
723 988
 *
724 989
 * @see xbee_set_channel
725 990
 **/
991
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
992
// it reduces code size or not should be done to be sure.
726 993
int xbee_get_channel(void)
727 994
{
728 995
	return xbee_channel;
......
735 1002
 *
736 1003
 * @return the 16-bit address of the XBee.
737 1004
 **/
1005
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
1006
// it reduces code size or not should be done to be sure.
738 1007
unsigned int xbee_get_address()
739 1008
{
740 1009
	return xbee_address;
741 1010
}
742 1011

  
1012
#ifndef ROBOT
1013
void xbee_set_com_port(char* port)
1014
{
1015
	xbee_com_port = port;
1016
}
1017
#endif

Also available in: Unified diff