Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / libwireless / lib / xbee.c @ 1435

History | View | Annotate | Download (21.4 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
    pthread_join(*xbee_listen_thread, NULL);
345
        free(xbee_listen_thread);
346
        lockf(xbee_stream, F_ULOCK, 0);
347
        close(xbee_stream);
348
  #else
349
  xbee_exit_api_mode();
350
        #endif
351
}
352

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

    
367
        return 0;
368

    
369
#else
370

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

    
387
        //write was interrupted after writing ret bytes
388
        return xbee_send(buf + ret, size - ret);
389
#endif
390
}
391

    
392
/**
393
 * Sends a string to the XBee.
394
 *
395
 * @param c the string to send to the XBEE
396
 **/
397
static int xbee_send_string(char* c)
398
{
399
        return xbee_send(c, strlen(c));
400
}
401

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

    
410
        return 0;
411
}
412
#endif
413

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

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

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

    
438
        xbee_wait_for_ok();
439

    
440
        return 0;
441
}
442

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

    
453
        return 0;
454
}
455

    
456
/**
457
 * Exit API mode.
458
 **/
459
static int xbee_exit_api_mode()
460
{
461
        if (xbee_send_modify_at_command("AP","0") != 0) {
462
                return -1;
463
        }
464

    
465
        return 0;
466
}
467

    
468
/**
469
 * Wait until the string "OK\r" is received from the XBee.
470
 **/
471
static int xbee_wait_for_ok()
472
{
473
        return xbee_wait_for_string("OK\r", 3);
474
}
475

    
476
/**
477
 * Delay until the specified string is received from
478
 * the XBee. Discards all other XBee data.
479
 *
480
 * @param s the string to receive
481
 * @param len the length of the string
482
 **/
483
static int xbee_wait_for_string(char* s, int len)
484
{
485
        char* curr = s;
486
        while (curr - s < len) {
487
                // check if buffer is empty
488
                if (buffer_last != buffer_first) {
489
                        char c = arrival_buf[buffer_first++];
490
                        if (buffer_first == XBEE_BUFFER_SIZE) {
491
                                buffer_first = 0;
492
                        }
493

    
494
                        if (c == *curr) {
495
                                curr++;
496
                        } else {
497
#ifndef ROBOT
498
                          //return -1; // Computer is less forgiving.
499
                          curr = s;
500
#else
501
                          curr = s;
502
#endif
503
                        }
504
                } // else buffer is empty.
505

    
506
#ifndef ROBOT
507
                usleep(100);
508
#endif
509
        }
510

    
511
        return 0;
512
}
513

    
514
/**
515
 * Verifies that the packets checksum is correct.
516
 * (If the checksum is correct, the sum of the bytes
517
 * is 0xFF.)
518
 *
519
 * @param packet the packet received. This includes the first
520
 * three bytes, which are header information from the XBee.
521
 *
522
 * @param len The length of the packet received from the XBee
523
 *
524
 * @return 0 if the checksum is incorrect, nonzero
525
 * otherwise
526
 **/
527
int xbee_verify_checksum(char* packet, int len)
528
{
529
        unsigned char sum = 0;
530
        int i;
531
        for (i = 3; i < len; i++)
532
                sum += (unsigned char)packet[i];
533
        return sum == 0xFF;
534
}
535

    
536
/**
537
 * Returns the checksum of the given packet.
538
 *
539
 * @param buf the data for the packet to send
540
 * @param len the length of the packet in bytes
541
 *
542
 * @return the checksum of the packet, which will
543
 * become the last byte sent in the packet
544
 **/
545
char xbee_compute_checksum(char* buf, int len)
546
{
547
        int i;
548
        unsigned char sum = 0;
549
        for (i = 0; i < len; i++)
550
                sum += (unsigned char)buf[i];
551
        return 0xFF - sum;
552
}
553

    
554
/**
555
 * Adds header information and checksum to the given
556
 * packet and sends it. Header information includes
557
 * XBEE_FRAME_START and the packet length, as two bytes.
558
 *
559
 * @param buf the packet data
560
 * @param len the size in bytes of the packet data
561
 *
562
 **/
563
static int xbee_send_frame(char* buf, int len)
564
{
565
        char prefix[3];
566
        prefix[0] = XBEE_FRAME_START;
567
        prefix[1] = (len & 0xFF00) >> 8;
568
        prefix[2] = len & 0xFF;
569
        char checksum = xbee_compute_checksum(buf, len);
570

    
571
        if (xbee_send(prefix, 3) != 0) {
572
                return -1;
573
        }
574

    
575
        if (xbee_send(buf, len) != 0) {
576
                return -1;
577
        }
578

    
579
        if (xbee_send(&checksum, 1) != 0) {
580
                return -1;
581
        }
582

    
583
        return 0;
584
}
585

    
586
/**
587
 * Sends an AT command to read a parameter.
588
 *
589
 * @param command the AT command to send. For exmaple,
590
 * use ID to read the PAN ID and MY to return the XBee ID.
591
 * See the XBee reference guide for a complete listing.
592
 **/
593
static int xbee_send_read_at_command(char* command)
594
{
595
        return xbee_send_modify_at_command(command, NULL);
596
}
597

    
598
/**
599
 * Sends the given AT command.
600
 *
601
 * @param command the AT command to send (e.g., MY, ID)
602
 * @param value the value to pass as a parameter
603
 * (or NULL if there is no parameter)
604
 **/
605
static int xbee_send_modify_at_command(char* command, char* value)
606
{
607
        char buf[16];
608
        int i;
609

    
610
        buf[0] = XBEE_FRAME_AT_COMMAND;
611
        buf[1] = 1;
612
        buf[2] = command[0];
613
        buf[3] = command[1];
614
        int valueLen = 0;
615
        if (value != NULL)
616
        {
617
                valueLen = strlen(value);
618
                if (valueLen > 8)
619
                {
620
                        WL_DEBUG_PRINT("AT Command too large.\r\n");
621
                        return -1;
622
                }
623

    
624
                for (i = 0; i < valueLen; i++) {
625
                        buf[4 + i] = value[i];
626
                }
627
        }
628

    
629
        return xbee_send_frame(buf, 4 + valueLen);
630
}
631

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

    
658
        if (len > 100)
659
        {
660
                WL_DEBUG_PRINT("Packet is too large.\r\n");
661
                return -1;
662
        }
663

    
664
        //data for sending request
665
        buf[0] = XBEE_FRAME_TX_REQUEST_16;
666
        buf[1] = frame;
667
        buf[2] = (dest >> 8) & 0xFF;
668
        buf[3] = dest & 0xFF;
669
        buf[4] = options;
670

    
671
        //packet prefix, do this here so we don't need an extra buffer
672
        prefix[0] = XBEE_FRAME_START;
673
        prefix[1] = ((5 + len) & 0xFF00) >> 8;
674
        prefix[2] = (5 + len) & 0xFF;
675

    
676
        for (i = 0; i < 5; i++)
677
                checksum += (unsigned char)buf[i];
678
        for (i = 0; i < len; i++)
679
                checksum += (unsigned char)packet[i];
680
        checksum = 0xFF - checksum;
681

    
682
        if (xbee_send(prefix, 3) != 0) {
683
                return -1;
684
        }
685

    
686
        if (xbee_send(buf, 5) != 0) {
687
                return -1;
688
        }
689

    
690
        if (xbee_send(packet, len) != 0) {
691
                return -1;
692
        }
693

    
694
        if (xbee_send((char*)&checksum, 1) != 0) {
695
                return -1;
696
        }
697

    
698
        return 0;
699
}
700

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

    
751
                if (buffer_first == XBEE_BUFFER_SIZE) {
752
                        buffer_first = 0;
753
                }
754
                xbee_buf[0] = XBEE_FRAME_START;
755
                currentBufPos++;
756
        }
757

    
758
        int len = -1;
759
        if (currentBufPos >= 3) {
760
                len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
761
        }
762

    
763
        while (len == -1 //packet length has not been read yet
764
                || currentBufPos < len + 4)
765
        {
766
                if (currentBufPos == 3)
767
                {
768
                        len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
769
                        if (len > 120)
770
                        {
771
                                WL_DEBUG_PRINT("Packet too large. Probably error in XBee transmission.\n");
772
                                currentBufPos = 0;
773
                                return -1;
774
                        }
775
                }
776

    
777
                // check if buffer is empty
778
                if (buffer_first == buffer_last) {
779
                        return -1;
780
                }
781
                xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
782
                if (buffer_first == XBEE_BUFFER_SIZE) {
783
                        buffer_first = 0;
784
                }
785
        }
786

    
787
        currentBufPos = 0;
788

    
789
        if (!xbee_verify_checksum(xbee_buf, len + 4))
790
        {
791
                WL_DEBUG_PRINT("XBee checksum failed.\r\n");
792
                return -1;
793
        }
794

    
795
        //we will take care of the packet
796
        
797
        ret = xbee_handle_packet(xbee_buf+3, len);
798
        if (ret == 1) {
799
                return 3;
800
        }
801
        
802
        if (dest == NULL) {
803
                return -1;
804
        }
805

    
806
        int i;
807
        for (i = 3; i < len + 3; i++) {
808
                dest[i - 3] = xbee_buf[i];
809
        }
810
        return len;
811
}
812

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

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

    
865
        if (command[0] == 'I' && command[1] == 'D')
866
        {
867
                xbee_panID = xbee_pending_panID;
868
                WL_DEBUG_PRINT("PAN ID set to ");
869
                WL_DEBUG_PRINT_INT(xbee_panID);
870
                WL_DEBUG_PRINT(".\r\n");
871
                return;
872
        }
873

    
874
        if (command[0] == 'C' && command[1] == 'H')
875
        {
876
                xbee_channel = xbee_pending_channel;
877
                WL_DEBUG_PRINT("Channel set to ");
878
                WL_DEBUG_PRINT_INT(xbee_channel);
879
                WL_DEBUG_PRINT(".\r\n");
880
                return;
881
        }
882

    
883
        if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)
884
        {
885
                xbee_address = 0;
886
                int i;
887
                for (i = 0; i < extraLen; i++) {
888
                        xbee_address = (xbee_address << 8) + extra[i];
889
                }
890

    
891
                WL_DEBUG_PRINT("XBee address is ");
892
                WL_DEBUG_PRINT_INT(xbee_address);
893
                WL_DEBUG_PRINT(".\r\n");
894

    
895
                if (xbee_address == 0)
896
                {
897
                        WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");
898
                        #ifndef ROBOT
899
                        exit(0);
900
                        #endif
901
                }
902
        }
903
}
904

    
905
/**
906
 * Attempts to handle the packet if it is dealt with
907
 * by the library.
908
 * We will handle the following packet types:
909
 *    Modem Status
910
 *    AT Command Response
911
 *
912
 * @param packet the packet to handle
913
 * @param len the length of the packet
914
 *
915
 * @return 1 if we have handled the packet, 0 otherwise
916
 */
917
static int xbee_handle_packet(char* packet, int len)
918
{
919

    
920
        char command[3] = {1, 2, 3};
921
        if (len <= 0) //this should not happend
922
        {
923
                WL_DEBUG_PRINT("Non-positive packet length.\r\n");
924
                return 0;
925
        }
926

    
927
        switch ((unsigned char)packet[0]) //packet type
928
        {
929
                case XBEE_FRAME_STATUS:
930
                        xbee_handle_status(packet[1]);
931
                        return 1;
932
                case XBEE_FRAME_AT_COMMAND_RESPONSE:
933
                        command[0] = packet[2];
934
                        command[1] = packet[3];
935
                        command[2] = 0;
936
                        xbee_handle_at_command_response(command, packet[4], packet + 5, len - 5);
937
                        return 1;
938
        }
939
        return 0;
940
}
941

    
942
/**
943
 * Sets the personal area network id.
944
 *
945
 * @param id the new personal area network (PAN) id
946
 **/
947
int xbee_set_pan_id(int id)
948
{
949
        char s[3];
950
        s[0] = (id >> 8) & 0xFF;
951
        s[1] = id & 0xFF;
952
        s[2] = 0;
953
        xbee_pending_panID = id;
954
        return xbee_send_modify_at_command("ID", s);
955
}
956

    
957
/**
958
 * Get the PAN ID for the XBee.
959
 *
960
 * @return the personal area network id, or
961
 * XBEE_PAN_DEFAULT if it has not yet been set.
962
 **/
963
unsigned int xbee_get_pan_id()
964
{
965
        return xbee_panID;
966
}
967

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

    
984
        char s[3];
985
        s[0] = channel & 0xFF;
986
        s[1] = 0;
987
        xbee_pending_channel = channel;
988

    
989
        return xbee_send_modify_at_command("CH", s);
990
}
991

    
992
/**
993
 * Returns the channel which the XBee is currently using.
994
 *
995
 * @return the channel the XBee is using
996
 *
997
 * @see xbee_set_channel
998
 **/
999
int xbee_get_channel(void)
1000
{
1001
        return xbee_channel;
1002
}
1003

    
1004
/**
1005
 * Get the 16-bit address of the XBee.
1006
 * This is used to specify who to send messages to
1007
 * and who messages are from.
1008
 *
1009
 * @return the 16-bit address of the XBee.
1010
 **/
1011
unsigned int xbee_get_address()
1012
{
1013
        return xbee_address;
1014
}
1015

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