Statistics
| Revision:

root / branches / colonetmk2 / code / projects / libwireless / lib / xbee.c @ 1456

History | View | Annotate | Download (21.7 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
#include <stdio.h>
46
#include <stdlib.h>
47

    
48
#else
49

    
50
#include <serial.h>
51
#include <avr/interrupt.h>
52
#include <util/delay.h>
53

    
54
#endif
55

    
56
#include <string.h>
57

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

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

    
71
/*Internal Function Prototypes*/
72

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

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

    
81
/*Command Mode Functions
82
 * Called during initialization.
83
 */
84
static int xbee_enter_command_mode(void);
85
static int xbee_exit_command_mode(void);
86
static int xbee_enter_api_mode(void);
87
static int xbee_exit_api_mode(void);
88
static int xbee_wait_for_string(char* s, int len);
89
static int xbee_wait_for_ok(void);
90

    
91
/*API Mode Functions*/
92

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

    
102
/*Global Variables*/
103

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

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

    
120

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

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

    
132
/*Function Implementations*/
133

    
134
#ifdef ROBOT
135

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

    
170
#else
171

    
172
// Computer code
173

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

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

    
197
                usleep(1000);
198
        }
199

    
200
        return NULL;
201
}
202

    
203
#endif
204

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    
368
        return 0;
369

    
370
#else
371

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

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

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

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

    
411
        return 0;
412
}
413
#endif
414

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

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

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

    
439
        xbee_wait_for_ok();
440

    
441
        return 0;
442
}
443

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

    
454
        return 0;
455
}
456

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

    
466
        return 0;
467
}
468

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

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

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

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

    
512
        return 0;
513
}
514

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

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

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

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

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

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

    
584
        return 0;
585
}
586

    
587
/**
588
 * Resets the XBee Modem (including baud rate when on robot)
589
 * 
590
 * Used for launching bootloader
591
 **/
592
int xbee_reset(void)
593
{
594
#ifdef ROBOT
595
        xbee_send_modify_at_command("BD", "3");
596
#endif
597
        xbee_send_read_at_command("FR");
598
    // delay 150 ms to wait for reset
599
#ifndef ROBOT
600
        usleep(150000);
601
#else
602
        _delay_ms(150);
603
#endif
604

    
605
        return 0;
606
}
607

    
608
/**
609
 * Sends an AT command to read a parameter.
610
 *
611
 * @param command the AT command to send. For exmaple,
612
 * use ID to read the PAN ID and MY to return the XBee ID.
613
 * See the XBee reference guide for a complete listing.
614
 **/
615
static int xbee_send_read_at_command(char* command)
616
{
617
        return xbee_send_modify_at_command(command, NULL);
618
}
619

    
620
/**
621
 * Sends the given AT command.
622
 *
623
 * @param command the AT command to send (e.g., MY, ID)
624
 * @param value the value to pass as a parameter
625
 * (or NULL if there is no parameter)
626
 **/
627
static int xbee_send_modify_at_command(char* command, char* value)
628
{
629
        char buf[16];
630
        int i;
631

    
632
        buf[0] = XBEE_FRAME_AT_COMMAND;
633
        buf[1] = 1;
634
        buf[2] = command[0];
635
        buf[3] = command[1];
636
        int valueLen = 0;
637
        if (value != NULL)
638
        {
639
                valueLen = strlen(value);
640
                if (valueLen > 8)
641
                {
642
                        WL_DEBUG_PRINT("AT Command too large.\r\n");
643
                        return -1;
644
                }
645

    
646
                for (i = 0; i < valueLen; i++) {
647
                        buf[4 + i] = value[i];
648
                }
649
        }
650

    
651
        return xbee_send_frame(buf, 4 + valueLen);
652
}
653

    
654
/**
655
 * Send the specified packet.
656
 *
657
 * @param packet the packet data to send
658
 * @param len the number of bytes in the packet
659
 *
660
 * @param dest the ID of the XBee to send the packet to,
661
 * or XBEE_BROADCAST to send the message to all robots
662
 * in the PAN.
663
 *
664
 * @param options a combination of the flags
665
 * XBEE_OPTIONS_NONE, XBEE_OPTIONS_DISABLE_RESPONSE and
666
 * XBEE_OPTIONS_BROADCAST_ALL_PANS
667
 *
668
 * @param frame the frame number to associate this packet
669
 * with. This will be used to identify the response when
670
 * the XBee alerts us as to whether or not our message
671
 * was received.
672
 **/
673
int xbee_send_packet(char* packet, int len, int dest, char options, char frame)
674
{
675
        char buf[5];
676
        char prefix[3];
677
        int i;
678
        unsigned char checksum = 0;
679

    
680
        if (len > 100)
681
        {
682
                WL_DEBUG_PRINT("Packet is too large.\r\n");
683
                return -1;
684
        }
685

    
686
        //data for sending request
687
        buf[0] = XBEE_FRAME_TX_REQUEST_16;
688
        buf[1] = frame;
689
        buf[2] = (dest >> 8) & 0xFF;
690
        buf[3] = dest & 0xFF;
691
        buf[4] = options;
692

    
693
        //packet prefix, do this here so we don't need an extra buffer
694
        prefix[0] = XBEE_FRAME_START;
695
        prefix[1] = ((5 + len) & 0xFF00) >> 8;
696
        prefix[2] = (5 + len) & 0xFF;
697

    
698
        for (i = 0; i < 5; i++)
699
                checksum += (unsigned char)buf[i];
700
        for (i = 0; i < len; i++)
701
                checksum += (unsigned char)packet[i];
702
        checksum = 0xFF - checksum;
703

    
704
        if (xbee_send(prefix, 3) != 0) {
705
                return -1;
706
        }
707

    
708
        if (xbee_send(buf, 5) != 0) {
709
                return -1;
710
        }
711

    
712
        if (xbee_send(packet, len) != 0) {
713
                return -1;
714
        }
715

    
716
        if (xbee_send((char*)&checksum, 1) != 0) {
717
                return -1;
718
        }
719

    
720
        return 0;
721
}
722

    
723
/**
724
 * Reads a packet received from the XBee. This function
725
 * is non-blocking. The resulting packet is stored in dest.
726
 * Only returns transmission response packets and
727
 * received packets. The returned packet does not include
728
 * header information or the checksum. This method also
729
 * handles special packets dealt with by the XBee library,
730
 * and so should be called frequently while the XBee is in
731
 * use.<br><br>
732
 *
733
 * The first byte of the packet will be either
734
 * XBEE_TX_STATUS or XBEE_RX to indicated
735
 * a response to a sent message or a received message,
736
 * respectively.<br><br>
737
 *
738
 * For a status response packet:<br>
739
 * The first byte will be XBEE_TX_STATUS.<br>
740
 * The second byte will be the frame number.<br>
741
 * The third byte will be the result. 0 indicates success,
742
 * and nonzero indicates that an error ocurred in
743
 * transmitting the packet.<br><br>
744
 *
745
 * For a received packet:<br>
746
 * The first byte will be XBEE_RX.<br>
747
 * The second and third bytes will be the 16-bit
748
 * address of the packet's sender.<br>
749
 * The fourth byte is the signal strength.<br>
750
 * The fifth byte is 1 if the packet were sent to
751
 * a specific address, and 2 if it is a broadcast packet.<br><br>
752
 *
753
 * @param dest set to the packet data
754
 * @return the length of the packet, or -1 if no packet
755
 * is available
756
 **/
757
int xbee_get_packet(unsigned char* dest)
758
{
759
     int ret;
760
        //start reading a packet with XBEE_FRAME_START
761
        if (currentBufPos == 0)
762
        {
763
                do
764
                {
765
                        if (buffer_first == XBEE_BUFFER_SIZE)
766
                                buffer_first = 0;
767
                        // check if buffer is empty
768
                        if (buffer_first == buffer_last) {
769
                                return -1;
770
                        }
771
                } while (arrival_buf[buffer_first++] != XBEE_FRAME_START);
772

    
773
                if (buffer_first == XBEE_BUFFER_SIZE) {
774
                        buffer_first = 0;
775
                }
776
                xbee_buf[0] = XBEE_FRAME_START;
777
                currentBufPos++;
778
        }
779

    
780
        int len = -1;
781
        if (currentBufPos >= 3) {
782
                len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
783
        }
784

    
785
        while (len == -1 //packet length has not been read yet
786
                || currentBufPos < len + 4)
787
        {
788
                if (currentBufPos == 3)
789
                {
790
                        len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);
791
                        if (len > 120)
792
                        {
793
                                WL_DEBUG_PRINT("Packet too large. Probably error in XBee transmission.\n");
794
                                currentBufPos = 0;
795
                                return -1;
796
                        }
797
                }
798

    
799
                // check if buffer is empty
800
                if (buffer_first == buffer_last) {
801
                        return -1;
802
                }
803
                xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];
804
                if (buffer_first == XBEE_BUFFER_SIZE) {
805
                        buffer_first = 0;
806
                }
807
        }
808

    
809
        currentBufPos = 0;
810

    
811
        if (!xbee_verify_checksum(xbee_buf, len + 4))
812
        {
813
                WL_DEBUG_PRINT("XBee checksum failed.\r\n");
814
                return -1;
815
        }
816

    
817
        //we will take care of the packet
818
        
819
        ret = xbee_handle_packet(xbee_buf+3, len);
820
        if (ret == 1) {
821
                return 3;
822
        }
823
        
824
        if (dest == NULL) {
825
                return -1;
826
        }
827

    
828
        int i;
829
        for (i = 3; i < len + 3; i++) {
830
                dest[i - 3] = xbee_buf[i];
831
        }
832
        return len;
833
}
834

    
835
/**
836
 * Handles modem status packets.
837
 *
838
 * @param status the type of status packet received.
839
 **/
840
void xbee_handle_status(char status)
841
{
842
        switch (status)
843
        {
844
                case 0:
845
                        WL_DEBUG_PRINT("XBee hardware reset.\r\n");
846
                        break;
847
                case 1:
848
                        WL_DEBUG_PRINT("Watchdog timer reset.\r\n");
849
                        break;
850
                case 2:
851
                        WL_DEBUG_PRINT("Associated.\r\n");
852
                        break;
853
                case 3:
854
                        WL_DEBUG_PRINT("Disassociated.\r\n");
855
                        break;
856
                case 4:
857
                        WL_DEBUG_PRINT("Synchronization lost.\r\n");
858
                        break;
859
                case 5:
860
                        WL_DEBUG_PRINT("Coordinator realignment.\r\n");
861
                        break;
862
                case 6:
863
                        WL_DEBUG_PRINT("Coordinator started.\r\n");
864
                        break;
865
        }
866
}
867

    
868
/**
869
 * Handles AT command response packets.
870
 * @param command the two character AT command, e.g. MY or ID
871
 * @param result 0 for success, 1 for an error
872
 * @param extra the hex value of the requested register
873
 * @param extraLen the length in bytes of extra
874
 **/
875
static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen)
876
{
877
        if (result == 1)
878
        {
879
                WL_DEBUG_PRINT("Error with AT");
880
                WL_DEBUG_PRINT(command);
881
                WL_DEBUG_PRINT(" packet.\r\n");
882
        }
883
        WL_DEBUG_PRINT("AT");
884
        WL_DEBUG_PRINT(command);
885
        WL_DEBUG_PRINT(" command was successful.\r\n");
886

    
887
        if (command[0] == 'I' && command[1] == 'D')
888
        {
889
                xbee_panID = xbee_pending_panID;
890
                WL_DEBUG_PRINT("PAN ID set to ");
891
                WL_DEBUG_PRINT_INT(xbee_panID);
892
                WL_DEBUG_PRINT(".\r\n");
893
                return;
894
        }
895

    
896
        if (command[0] == 'C' && command[1] == 'H')
897
        {
898
                xbee_channel = xbee_pending_channel;
899
                WL_DEBUG_PRINT("Channel set to ");
900
                WL_DEBUG_PRINT_INT(xbee_channel);
901
                WL_DEBUG_PRINT(".\r\n");
902
                return;
903
        }
904

    
905
        if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)
906
        {
907
                xbee_address = 0;
908
                int i;
909
                for (i = 0; i < extraLen; i++) {
910
                        xbee_address = (xbee_address << 8) + extra[i];
911
                }
912

    
913
                WL_DEBUG_PRINT("XBee address is ");
914
                WL_DEBUG_PRINT_INT(xbee_address);
915
                WL_DEBUG_PRINT(".\r\n");
916

    
917
                if (xbee_address == 0)
918
                {
919
                        WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");
920
                        #ifndef ROBOT
921
                        exit(0);
922
                        #endif
923
                }
924
        }
925
}
926

    
927
/**
928
 * Attempts to handle the packet if it is dealt with
929
 * by the library.
930
 * We will handle the following packet types:
931
 *    Modem Status
932
 *    AT Command Response
933
 *
934
 * @param packet the packet to handle
935
 * @param len the length of the packet
936
 *
937
 * @return 1 if we have handled the packet, 0 otherwise
938
 */
939
static int xbee_handle_packet(char* packet, int len)
940
{
941

    
942
        char command[3] = {1, 2, 3};
943
        if (len <= 0) //this should not happend
944
        {
945
                WL_DEBUG_PRINT("Non-positive packet length.\r\n");
946
                return 0;
947
        }
948

    
949
        switch ((unsigned char)packet[0]) //packet type
950
        {
951
                case XBEE_FRAME_STATUS:
952
                        xbee_handle_status(packet[1]);
953
                        return 1;
954
                case XBEE_FRAME_AT_COMMAND_RESPONSE:
955
                        command[0] = packet[2];
956
                        command[1] = packet[3];
957
                        command[2] = 0;
958
                        xbee_handle_at_command_response(command, packet[4], packet + 5, len - 5);
959
                        return 1;
960
        }
961
        return 0;
962
}
963

    
964
/**
965
 * Sets the personal area network id.
966
 *
967
 * @param id the new personal area network (PAN) id
968
 **/
969
int xbee_set_pan_id(int id)
970
{
971
        char s[3];
972
        s[0] = (id >> 8) & 0xFF;
973
        s[1] = id & 0xFF;
974
        s[2] = 0;
975
        xbee_pending_panID = id;
976
        return xbee_send_modify_at_command("ID", s);
977
}
978

    
979
/**
980
 * Get the PAN ID for the XBee.
981
 *
982
 * @return the personal area network id, or
983
 * XBEE_PAN_DEFAULT if it has not yet been set.
984
 **/
985
unsigned int xbee_get_pan_id()
986
{
987
        return xbee_panID;
988
}
989

    
990
/**
991
 * Set the channel the XBee is using.
992
 *
993
 * @param channel the channel the XBee will not use,
994
 * between 0x0B and 0x1A
995
 *
996
 * @see xbee_get_channel
997
 **/
998
int xbee_set_channel(int channel)
999
{
1000
        if (channel < 0x0B || channel > 0x1A)
1001
        {
1002
                WL_DEBUG_PRINT("Channel out of range.\r\n");
1003
                return -1;
1004
        }
1005

    
1006
        char s[3];
1007
        s[0] = channel & 0xFF;
1008
        s[1] = 0;
1009
        xbee_pending_channel = channel;
1010

    
1011
        return xbee_send_modify_at_command("CH", s);
1012
}
1013

    
1014
/**
1015
 * Returns the channel which the XBee is currently using.
1016
 *
1017
 * @return the channel the XBee is using
1018
 *
1019
 * @see xbee_set_channel
1020
 **/
1021
int xbee_get_channel(void)
1022
{
1023
        return xbee_channel;
1024
}
1025

    
1026
/**
1027
 * Get the 16-bit address of the XBee.
1028
 * This is used to specify who to send messages to
1029
 * and who messages are from.
1030
 *
1031
 * @return the 16-bit address of the XBee.
1032
 **/
1033
unsigned int xbee_get_address()
1034
{
1035
        return xbee_address;
1036
}
1037

    
1038
#ifndef ROBOT
1039
void xbee_set_com_port(char* port)
1040
{
1041
        xbee_com_port = port;
1042
}
1043
#endif