Project

General

Profile

Statistics
| Revision:

root / branches / autonomous_recharging / code / projects / libwireless / lib / xbee.c @ 883

History | View | Annotate | Download (22.2 KB)

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

    
35
#include "xbee.h"
36
#include "wl_defs.h"
37

    
38
#ifndef ROBOT
39

    
40
#include <fcntl.h>
41
#include <unistd.h>
42
#include <pthread.h>
43
#include <errno.h>
44
#include <termios.h>
45

    
46
#else
47

    
48
#include <serial.h>
49
#include <avr/interrupt.h>
50

    
51
#endif
52

    
53
#include <stdio.h>
54
#include <stdlib.h>
55
#include <string.h>
56

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

    
60
/*Frame Types*/
61
#define XBEE_FRAME_STATUS 0x8A
62
#define XBEE_FRAME_AT_COMMAND 0x08
63
#define XBEE_FRAME_AT_COMMAND_RESPONSE 0x88
64
#define XBEE_FRAME_TX_REQUEST_64 0x00
65
#define XBEE_FRAME_TX_REQUEST_16 0x01
66
#define XBEE_FRAME_TX_STATUS XBEE_TX_STATUS
67
#define XBEE_FRAME_RX_64 0x80
68
#define XBEE_FRAME_RX_16 XBEE_RX
69

    
70
/*Internal Function Prototypes*/
71

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

    
76
#ifndef ROBOT
77
static int xbee_read(char* buf, int size);
78
#endif
79

    
80
/*Command Mode Functions
81
 * Called during initialization.
82
 */
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_exit_api_mode(void);
87
static int xbee_wait_for_string(char* s, int len);
88
static int xbee_wait_for_ok(void);
89

    
90
/*API Mode Functions*/
91

    
92
static int xbee_handle_packet(char* packet, int len);
93
static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen);
94
static void xbee_handle_status(char status);
95
static int xbee_verify_checksum(char* packet, int len);
96
static char xbee_compute_checksum(char* packet, int len);
97
static int xbee_send_frame(char* buf, int len);
98
static int xbee_send_read_at_command(char* command);
99
static int xbee_send_modify_at_command(char* command, char* value);
100

    
101
/*Global Variables*/
102

    
103
#ifndef ROBOT
104
static char* xbee_com_port = XBEE_PORT_DEFAULT;
105
static int xbee_stream;
106
static pthread_t* xbee_listen_thread;
107
#endif
108

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

    
119

    
120
//used to store packets as they are read
121
static char xbee_buf[PACKET_BUFFER_SIZE];
122
static int currentBufPos = 0;
123

    
124
//XBee status
125
static unsigned int xbee_panID = XBEE_PAN_DEFAULT;
126
static unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;
127
static int xbee_channel = XBEE_CHANNEL_DEFAULT;
128
static int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;
129
static volatile unsigned int xbee_address = 0;
130

    
131
/*Function Implementations*/
132

    
133
#ifdef ROBOT
134

    
135
/**
136
 * Interrupt for the robot. Adds bytes received from the xbee
137
 * to the buffer.
138
 **/
139
#ifndef FIREFLY
140
ISR(USART1_RX_vect)
141
{
142
        char c = UDR1;
143
        arrival_buf[buffer_last] = c;
144
        int t = buffer_last + 1;
145
        if (t == XBEE_BUFFER_SIZE)
146
                t = 0;
147
        if (t == buffer_first)
148
        {
149
                WL_DEBUG_PRINT("\nOut of space in buffer.\n");
150
        }
151
        buffer_last = t;
152
}
153
#else
154
SIGNAL(SIG_USART0_RECV)
155
{
156
        char c = UDR0;
157
        arrival_buf[buffer_last] = c;
158
        int t = buffer_last + 1;
159
        if (t == XBEE_BUFFER_SIZE)
160
                t = 0;
161
        if (t == buffer_first)
162
        {
163
                WL_DEBUG_PRINT("Out of space in buffer.\n");
164
        }
165
        buffer_last = t;
166
}
167
#endif
168

    
169
#else
170

    
171
// Computer code
172

    
173
/**
174
 * Thread that listens to the xbee.
175
 **/
176
static void* listen_to_xbee(void* x)
177
{
178
        char c;
179
        while (1)
180
        {
181
                if (xbee_read(&c, 1) != 0) {
182
                        WL_DEBUG_PRINT("xbee_read failed.\n");
183
                        return NULL;
184
                }
185

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

    
196
                usleep(1000);
197
        }
198

    
199
        return NULL;
200
}
201

    
202
#endif
203

    
204
/**
205
 * Initializes the XBee library so that other functions may be used.
206
 **/
207
int xbee_lib_init()
208
{
209
        WL_DEBUG_PRINT("in xbee_init\n");
210
#ifdef ROBOT
211

    
212
        //enable the receiving interrupt
213
#ifdef FIREFLY
214
        UCSR0B |= _BV(RXCIE) | _BV(RXEN);
215
#else
216
#ifdef BAYBOARD
217
        UCSR1B |= _BV(RXCIE1);
218
#else
219
        UCSR1B |= _BV(RXCIE);
220
#endif
221
#endif
222
        sei();
223
#else
224
        xbee_stream = open(xbee_com_port, O_RDWR);
225
        if (xbee_stream == -1/* || lockf(xbee_stream, F_TEST, 0) != 0*/)
226
        {
227
                WL_DEBUG_PRINT("Failed to open connection to XBee on port ");
228
                WL_DEBUG_PRINT_INT(xbee_com_port);
229
                WL_DEBUG_PRINT(".\n");
230
                return -1;
231
        } else {
232
          WL_DEBUG_PRINT("Successfully opened connection to XBee on port ");
233
                WL_DEBUG_PRINT_INT(xbee_com_port);
234
                WL_DEBUG_PRINT(".\n");
235
        }
236

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

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

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

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

    
267
        int ret = pthread_create(xbee_listen_thread, NULL, listen_to_xbee, NULL);
268
        if (ret)
269
        {
270
                WL_DEBUG_PRINT("Failed to create listener thread.\n");
271
                return -1;
272
        }
273
#endif
274

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

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

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

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

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

    
289
        if (xbee_exit_command_mode() != 0) {
290
                return -1;
291
        }
292

    
293
        WL_DEBUG_PRINT("Left command mode.\n");
294

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

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

    
305
          usleep(1000);
306

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

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

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

    
336
/**
337
 * Call when finished using the XBee library. This releases
338
 * all sued resources.
339
 **/
340
void xbee_terminate()
341
{
342
        #ifndef ROBOT
343
        pthread_cancel(*xbee_listen_thread);
344
        free(xbee_listen_thread);
345
        lockf(xbee_stream, F_ULOCK, 0);
346
        close(xbee_stream);
347
        #endif
348
}
349

    
350
/**
351
 * Send a buffer buf of size bytes to the XBee.
352
 *
353
 * @param buf the buffer of data to send
354
 * @param size the number of bytes to send
355
 **/
356
static int xbee_send(char* buf, int size)
357
{
358
#ifdef ROBOT
359
        int i;
360
        for (i = 0; i < size; i++) {
361
                xbee_putc(buf[i]);
362
        }
363

    
364
        return 0;
365

    
366
#else
367

    
368
        int ret = write(xbee_stream, buf, size);
369
        //success
370
        if (ret == size)
371
                return 0;
372
        if (ret == -1)
373
        {
374
                //interrupted by system signal, probably timer interrupt.
375
                //just try again
376
                if (errno == 4)
377
                {
378
                        return xbee_send(buf, size);
379
                }
380
                WL_DEBUG_PRINT("Failed to write to xbee\r\n");
381
                return -1;
382
        }
383

    
384
        //write was interrupted after writing ret bytes
385
        return xbee_send(buf + ret, size - ret);
386
#endif
387
}
388

    
389
/**
390
 * Sends a string to the XBee.
391
 *
392
 * @param c the string to send to the XBEE
393
 **/
394
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
395
// it reduces code size or not should be done to be sure.
396
static int xbee_send_string(char* c)
397
{
398
        return xbee_send(c, strlen(c));
399
}
400

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

    
409
        return 0;
410
}
411
#endif
412

    
413
/**
414
 * Enter into command mode.
415
 **/
416
static int xbee_enter_command_mode()
417
{
418
        if (xbee_send_string("+++") != 0) {
419
                return -1;
420
        }
421

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

    
428
/**
429
 * Exit from command mode.
430
 **/
431
static int xbee_exit_command_mode()
432
{
433
        if (xbee_send_string("ATCN\r") != 0) {
434
                return -1;
435
        }
436

    
437
        xbee_wait_for_ok();
438

    
439
        return 0;
440
}
441

    
442
/**
443
 * Enter API mode.
444
 **/
445
static int xbee_enter_api_mode()
446
{
447
        if (xbee_send_string("ATAP 1\r") != 0) {
448
                return -1;
449
        }
450
        xbee_wait_for_ok();
451

    
452
        return 0;
453
}
454

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

    
465
/**
466
 * Delay until the specified string is received from
467
 * the XBee. Discards all other XBee data.
468
 *
469
 * @param s the string to receive
470
 * @param len the length of the string
471
 **/
472
static int xbee_wait_for_string(char* s, int len)
473
{
474
        char* curr = s;
475
        while (curr - s < len) {
476
                // check if buffer is empty
477
                if (buffer_last != buffer_first) {
478
                        char c = arrival_buf[buffer_first++];
479
                        if (buffer_first == XBEE_BUFFER_SIZE) {
480
                                buffer_first = 0;
481
                        }
482

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

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

    
500
        return 0;
501
}
502

    
503
/**
504
 * Verifies that the packets checksum is correct.
505
 * (If the checksum is correct, the sum of the bytes
506
 * is 0xFF.)
507
 *
508
 * @param packet the packet received. This includes the first
509
 * three bytes, which are header information from the XBee.
510
 *
511
 * @param len The length of the packet received from the XBee
512
 *
513
 * @return 0 if the checksum is incorrect, nonzero
514
 * otherwise
515
 **/
516
int xbee_verify_checksum(char* packet, int len)
517
{
518
        unsigned char sum = 0;
519
        int i;
520
        for (i = 3; i < len; i++)
521
                sum += (unsigned char)packet[i];
522
        return sum == 0xFF;
523
}
524

    
525
/**
526
 * Returns the checksum of the given packet.
527
 *
528
 * @param buf the data for the packet to send
529
 * @param len the length of the packet in bytes
530
 *
531
 * @return the checksum of the packet, which will
532
 * become the last byte sent in the packet
533
 **/
534
char xbee_compute_checksum(char* buf, int len)
535
{
536
        int i;
537
        unsigned char sum = 0;
538
        for (i = 0; i < len; i++)
539
                sum += (unsigned char)buf[i];
540
        return 0xFF - sum;
541
}
542

    
543
/**
544
 * Adds header information and checksum to the given
545
 * packet and sends it. Header information includes
546
 * XBEE_FRAME_START and the packet length, as two bytes.
547
 *
548
 * @param buf the packet data
549
 * @param len the size in bytes of the packet data
550
 *
551
 **/
552
static int xbee_send_frame(char* buf, int len)
553
{
554
        char prefix[3];
555
        prefix[0] = XBEE_FRAME_START;
556
        prefix[1] = (len & 0xFF00) >> 8;
557
        prefix[2] = len & 0xFF;
558
        char checksum = xbee_compute_checksum(buf, len);
559

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

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

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

    
572
        return 0;
573
}
574

    
575
/**
576
 * Sends an AT command to read a parameter.
577
 *
578
 * @param command the AT command to send. For exmaple,
579
 * use ID to read the PAN ID and MY to return the XBee ID.
580
 * See the XBee reference guide for a complete listing.
581
 **/
582
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
583
// it reduces code size or not should be done to be sure.
584
static int xbee_send_read_at_command(char* command)
585
{
586
        return xbee_send_modify_at_command(command, NULL);
587
}
588

    
589
/**
590
 * Sends the given AT command.
591
 *
592
 * @param command the AT command to send (e.g., MY, ID)
593
 * @param value the value to pass as a parameter
594
 * (or NULL if there is no parameter)
595
 **/
596
static int xbee_send_modify_at_command(char* command, char* value)
597
{
598
        char buf[16];
599
        int i;
600

    
601
        buf[0] = XBEE_FRAME_AT_COMMAND;
602
        buf[1] = 1;
603
        buf[2] = command[0];
604
        buf[3] = command[1];
605
        int valueLen = 0;
606
        if (value != NULL)
607
        {
608
                valueLen = strlen(value);
609
                if (valueLen > 8)
610
                {
611
                        WL_DEBUG_PRINT("AT Command too large.\r\n");
612
                        return -1;
613
                }
614

    
615
                for (i = 0; i < valueLen; i++) {
616
                        buf[4 + i] = value[i];
617
                }
618
        }
619

    
620
        return xbee_send_frame(buf, 4 + valueLen);
621
}
622

    
623
/**
624
 * Send the specified packet.
625
 *
626
 * @param packet the packet data to send
627
 * @param len the number of bytes in the packet
628
 *
629
 * @param dest the ID of the XBee to send the packet to,
630
 * or XBEE_BROADCAST to send the message to all robots
631
 * in the PAN.
632
 *
633
 * @param options a combination of the flags
634
 * XBEE_OPTIONS_NONE, XBEE_OPTIONS_DISABLE_RESPONSE and
635
 * XBEE_OPTIONS_BROADCAST_ALL_PANS
636
 *
637
 * @param frame the frame number to associate this packet
638
 * with. This will be used to identify the response when
639
 * the XBee alerts us as to whether or not our message
640
 * was received.
641
 **/
642
int xbee_send_packet(char* packet, int len, int dest, char options, char frame)
643
{
644
        char buf[5];
645
        char prefix[3];
646
        int i;
647
        unsigned char checksum = 0;
648

    
649
        if (len > 100)
650
        {
651
                WL_DEBUG_PRINT("Packet is too large.\r\n");
652
                return -1;
653
        }
654

    
655
        //data for sending request
656
        buf[0] = XBEE_FRAME_TX_REQUEST_16;
657
        buf[1] = frame;
658
        buf[2] = (dest >> 8) & 0xFF;
659
        buf[3] = dest & 0xFF;
660
        buf[4] = options;
661

    
662
        //packet prefix, do this here so we don't need an extra buffer
663
        prefix[0] = XBEE_FRAME_START;
664
        prefix[1] = ((5 + len) & 0xFF00) >> 8;
665
        prefix[2] = (5 + len) & 0xFF;
666

    
667
        for (i = 0; i < 5; i++)
668
                checksum += (unsigned char)buf[i];
669
        for (i = 0; i < len; i++)
670
                checksum += (unsigned char)packet[i];
671
        checksum = 0xFF - checksum;
672

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

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

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

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

    
689
        return 0;
690
}
691

    
692
/**
693
 * Reads a packet received from the XBee. This function
694
 * is non-blocking. The resulting packet is stored in dest.
695
 * Only returns transmission response packets and
696
 * received packets. The returned packet does not include
697
 * header information or the checksum. This method also
698
 * handles special packets dealt with by the XBee library,
699
 * and so should be called frequently while the XBee is in
700
 * use.<br><br>
701
 *
702
 * The first byte of the packet will be either
703
 * XBEE_TX_STATUS or XBEE_RX to indicated
704
 * a response to a sent message or a received message,
705
 * respectively.<br><br>
706
 *
707
 * For a status response packet:<br>
708
 * The first byte will be XBEE_TX_STATUS.<br>
709
 * The second byte will be the frame number.<br>
710
 * The third byte will be the result. 0 indicates success,
711
 * and nonzero indicates that an error ocurred in
712
 * transmitting the packet.<br><br>
713
 *
714
 * For a received packet:<br>
715
 * The first byte will be XBEE_RX.<br>
716
 * The second and third bytes will be the 16-bit
717
 * address of the packet's sender.<br>
718
 * The fourth byte is the signal strength.<br>
719
 * The fifth byte is 1 if the packet were sent to
720
 * a specific address, and 2 if it is a broadcast packet.<br><br>
721
 *
722
 * @param dest set to the packet data
723
 * @return the length of the packet, or -1 if no packet
724
 * is available
725
 **/
726
int xbee_get_packet(unsigned char* dest)
727
{
728
        //start reading a packet with XBEE_FRAME_START
729
        if (currentBufPos == 0)
730
        {
731
                do
732
                {
733
                        if (buffer_first == XBEE_BUFFER_SIZE)
734
                                buffer_first = 0;
735
                        // check if buffer is empty
736
                        if (buffer_first == buffer_last) {
737
                                return -1;
738
                        }
739
                } while (arrival_buf[buffer_first++] != XBEE_FRAME_START);
740

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

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

    
753
        while (len == -1 //packet length has not been read yet
754
                || currentBufPos < len + 4)
755
        {
756
                if (currentBufPos == 3)
757
                {
758
                        len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
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) {
769
                        return -1;
770
                }
771
                xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
772
                if (buffer_first == XBEE_BUFFER_SIZE) {
773
                        buffer_first = 0;
774
                }
775
        }
776

    
777
        currentBufPos = 0;
778

    
779
        usb_puts("Got packet.\n\r");
780
        if (!xbee_verify_checksum(xbee_buf, len + 4))
781
        {
782
                usb_puts("Checksum failed.\n\r");
783
                WL_DEBUG_PRINT("XBee checksum failed.\r\n");
784
                return -1;
785
        }
786

    
787
        //we will take care of the packet
788
        if (xbee_handle_packet(xbee_buf + 3, len) != 0) {
789
                usb_puts("We handled a packet.\n\r");
790
                return -1;
791
        }
792

    
793
        if (dest == NULL) {
794
                usb_puts("dest == NULL");
795
                return -1;
796
        }
797

    
798
        int i;
799
        for (i = 3; i < len + 3; i++) {
800
                dest[i - 3] = xbee_buf[i];
801
        }
802

    
803
        return len;
804
}
805

    
806
/**
807
 * Handles modem status packets.
808
 *
809
 * @param status the type of status packet received.
810
 **/
811
void xbee_handle_status(char status)
812
{
813
        switch (status)
814
        {
815
                case 0:
816
                        WL_DEBUG_PRINT("XBee hardware reset.\r\n");
817
                        break;
818
                case 1:
819
                        WL_DEBUG_PRINT("Watchdog timer reset.\r\n");
820
                        break;
821
                case 2:
822
                        WL_DEBUG_PRINT("Associated.\r\n");
823
                        break;
824
                case 3:
825
                        WL_DEBUG_PRINT("Disassociated.\r\n");
826
                        break;
827
                case 4:
828
                        WL_DEBUG_PRINT("Synchronization lost.\r\n");
829
                        break;
830
                case 5:
831
                        WL_DEBUG_PRINT("Coordinator realignment.\r\n");
832
                        break;
833
                case 6:
834
                        WL_DEBUG_PRINT("Coordinator started.\r\n");
835
                        break;
836
        }
837
}
838

    
839
/**
840
 * Handles AT command response packets.
841
 * @param command the two character AT command, e.g. MY or ID
842
 * @param result 0 for success, 1 for an error
843
 * @param extra the hex value of the requested register
844
 * @param extraLen the length in bytes of extra
845
 **/
846
static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen)
847
{
848
        if (result == 1)
849
        {
850
                WL_DEBUG_PRINT("Error with AT");
851
                WL_DEBUG_PRINT(command);
852
                WL_DEBUG_PRINT(" packet.\r\n");
853
        }
854
        WL_DEBUG_PRINT("AT");
855
        WL_DEBUG_PRINT(command);
856
        WL_DEBUG_PRINT(" command was successful.\r\n");
857

    
858
        if (command[0] == 'I' && command[1] == 'D')
859
        {
860
                xbee_panID = xbee_pending_panID;
861
                WL_DEBUG_PRINT("PAN ID set to ");
862
                WL_DEBUG_PRINT_INT(xbee_panID);
863
                WL_DEBUG_PRINT(".\r\n");
864
                return;
865
        }
866

    
867
        if (command[0] == 'C' && command[1] == 'H')
868
        {
869
                xbee_channel = xbee_pending_channel;
870
                WL_DEBUG_PRINT("Channel set to ");
871
                WL_DEBUG_PRINT_INT(xbee_channel);
872
                WL_DEBUG_PRINT(".\r\n");
873
                return;
874
        }
875

    
876
        if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)
877
        {
878
                xbee_address = 0;
879
                int i;
880
                for (i = 0; i < extraLen; i++) {
881
                        xbee_address = (xbee_address << 8) + extra[i];
882
                }
883

    
884
                WL_DEBUG_PRINT("XBee address is ");
885
                WL_DEBUG_PRINT_INT(xbee_address);
886
                WL_DEBUG_PRINT(".\r\n");
887

    
888
                if (xbee_address == 0)
889
                {
890
                        WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");
891
                        #ifndef ROBOT
892
                        exit(0);
893
                        #endif
894
                }
895
        }
896
}
897

    
898
/**
899
 * Attempts to handle the packet if it is dealt with
900
 * by the library.
901
 * We will handle the following packet types:
902
 *    Modem Status
903
 *    AT Command Response
904
 *
905
 * @param packet the packet to handle
906
 * @param len the length of the packet
907
 *
908
 * @return 1 if we have handled the packet, 0 otherwise
909
 */
910
static int xbee_handle_packet(char* packet, int len)
911
{
912

    
913
        char command[3] = {1, 2, 3};
914
        if (len <= 0) //this should not happend
915
        {
916
                WL_DEBUG_PRINT("Non-positive packet length.\r\n");
917
                return 0;
918
        }
919

    
920
        switch ((unsigned char)packet[0]) //packet type
921
        {
922
                case XBEE_FRAME_STATUS:
923
                        xbee_handle_status(packet[1]);
924
                        return 1;
925
                case XBEE_FRAME_AT_COMMAND_RESPONSE:
926
                        command[0] = packet[2];
927
                        command[1] = packet[3];
928
                        command[2] = 0;
929
                        xbee_handle_at_command_response(command, packet[4], packet + 5, len - 5);
930
                        return 1;
931
        }
932
        return 0;
933
}
934

    
935
/**
936
 * Sets the personal area network id.
937
 *
938
 * @param id the new personal area network (PAN) id
939
 **/
940
int xbee_set_pan_id(int id)
941
{
942
        char s[3];
943
        s[0] = (id >> 8) & 0xFF;
944
        s[1] = id & 0xFF;
945
        s[2] = 0;
946
        xbee_pending_panID = id;
947
        return xbee_send_modify_at_command("ID", s);
948
}
949

    
950
/**
951
 * Get the PAN ID for the XBee.
952
 *
953
 * @return the personal area network id, or
954
 * XBEE_PAN_DEFAULT if it has not yet been set.
955
 **/
956
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
957
// it reduces code size or not should be done to be sure.
958
unsigned int xbee_get_pan_id()
959
{
960
        return xbee_panID;
961
}
962

    
963
/**
964
 * Set the channel the XBee is using.
965
 *
966
 * @param channel the channel the XBee will not use,
967
 * between 0x0B and 0x1A
968
 *
969
 * @see xbee_get_channel
970
 **/
971
int xbee_set_channel(int channel)
972
{
973
        if (channel < 0x0B || channel > 0x1A)
974
        {
975
                WL_DEBUG_PRINT("Channel out of range.\r\n");
976
                return -1;
977
        }
978

    
979
        char s[3];
980
        s[0] = channel & 0xFF;
981
        s[1] = 0;
982
        xbee_pending_channel = channel;
983

    
984
        return xbee_send_modify_at_command("CH", s);
985
}
986

    
987
/**
988
 * Returns the channel which the XBee is currently using.
989
 *
990
 * @return the channel the XBee is using
991
 *
992
 * @see xbee_set_channel
993
 **/
994
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
995
// it reduces code size or not should be done to be sure.
996
int xbee_get_channel(void)
997
{
998
        return xbee_channel;
999
}
1000

    
1001
/**
1002
 * Get the 16-bit address of the XBee.
1003
 * This is used to specify who to send messages to
1004
 * and who messages are from.
1005
 *
1006
 * @return the 16-bit address of the XBee.
1007
 **/
1008
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
1009
// it reduces code size or not should be done to be sure.
1010
unsigned int xbee_get_address()
1011
{
1012
        return xbee_address;
1013
}
1014

    
1015
#ifndef ROBOT
1016
void xbee_set_com_port(char* port)
1017
{
1018
        xbee_com_port = port;
1019
}
1020
#endif