Colony

/**

 * Copyright (c) 2007 Colony Project

 *

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 *

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 *

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file xbee.c

 * @brief XBee Interface

 *

 * Implementation of low level communication with the XBee in API mode.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

#include "xbee.h"

#include "wl_defs.h"

#ifndef ROBOT

#include <fcntl.h>

#include <unistd.h>

#include <pthread.h>

#include <errno.h>

#include <termios.h>

#else

#include <serial.h>

#include <avr/interrupt.h>

#endif

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#define XBEE_FRAME_START 0x7E

#define XBEE_GET_PACKET_TIMEOUT 1000

/*Frame Types*/

#define XBEE_FRAME_STATUS 0x8A

#define XBEE_FRAME_AT_COMMAND 0x08

#define XBEE_FRAME_AT_COMMAND_RESPONSE 0x88

#define XBEE_FRAME_TX_REQUEST_64 0x00

#define XBEE_FRAME_TX_REQUEST_16 0x01

#define XBEE_FRAME_TX_STATUS XBEE_TX_STATUS

#define XBEE_FRAME_RX_64 0x80

#define XBEE_FRAME_RX_16 XBEE_RX

/*Internal Function Prototypes*/

/*I/O Functions*/

static int xbee_send(char* buf, int size);

static int xbee_send_string(char* c);

#ifndef ROBOT

static int xbee_read(char* buf, int size);

#endif

/*Command Mode Functions

 * Called during initialization.

 */

static int xbee_enter_command_mode(void);

static int xbee_exit_command_mode(void);

static int xbee_enter_api_mode(void);

static int xbee_wait_for_string(char* s, int len);

static int xbee_wait_for_ok(void);

/*API Mode Functions*/

static int xbee_handle_packet(char* packet, int len);

static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen);

static void xbee_handle_status(char status);

static int xbee_verify_checksum(char* packet, int len);

static char xbee_compute_checksum(char* packet, int len);

static int xbee_send_frame(char* buf, int len);

static int xbee_send_read_at_command(char* command);

static int xbee_send_modify_at_command(char* command, char* value);

/*Global Variables*/

#ifndef ROBOT

static char* xbee_com_port = XBEE_PORT_DEFAULT;

static int xbee_stream;

static pthread_t* xbee_listen_thread;

#endif

// TODO: is this a good size?

#define XBEE_BUFFER_SIZE        128

#define PACKET_BUFFER_SIZE        108

// a buffer for data received from the XBee

char arrival_buf[XBEE_BUFFER_SIZE];

// location of last unread byte in buffer

volatile int buffer_last = 0;

// first unread byte in buffer

volatile int buffer_first = 0;

//used to store packets as they are read

static char xbee_buf[PACKET_BUFFER_SIZE];

static int currentBufPos = 0;

//XBee status

static unsigned int xbee_panID = XBEE_PAN_DEFAULT;

static unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;

static int xbee_channel = XBEE_CHANNEL_DEFAULT;

static int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;

static volatile unsigned int xbee_address = 0;

/*Function Implementations*/

#ifdef ROBOT

/**

 * Interrupt for the robot. Adds bytes received from the xbee

 * to the buffer.

 **/

#ifndef FIREFLY

ISR(USART1_RX_vect)

{

        char c = UDR1;

        arrival_buf[buffer_last] = c;

        int t = buffer_last + 1;

        if (t == XBEE_BUFFER_SIZE)

                t = 0;

        if (t == buffer_first)

        {

                WL_DEBUG_PRINT("\nOut of space in buffer.\n");

        }

        buffer_last = t;

}

#else

SIGNAL(SIG_USART0_RECV)

{

        char c = UDR0;

        arrival_buf[buffer_last] = c;

        int t = buffer_last + 1;

        if (t == XBEE_BUFFER_SIZE)

                t = 0;

        if (t == buffer_first)

        {

                WL_DEBUG_PRINT("Out of space in buffer.\n");

        }

        buffer_last = t;

}

#endif

#else

// Computer code

/**

 * Thread that listens to the xbee.

 **/

static void* listen_to_xbee(void* x)

{

        char c;

        while (1)

        {

                if (xbee_read(&c, 1) != 0) {

                        WL_DEBUG_PRINT("xbee_read failed.\n");

                        return NULL;

                }

                arrival_buf[buffer_last] = c;

                int t = buffer_last + 1;

                if (t == XBEE_BUFFER_SIZE)

                        t = 0;

                if (t == buffer_first)

                {

                        WL_DEBUG_PRINT("Out of space in buffer.\n");

                }

                buffer_last = t;

                usleep(1000);

        }

        return NULL;

}

#endif

/**

 * Initializes the XBee library so that other functions may be used.

 **/

int xbee_lib_init()

{

        WL_DEBUG_PRINT("in xbee_init\n");

#ifdef ROBOT

        //enable the receiving interrupt

#ifdef FIREFLY

        UCSR0B |= _BV(RXCIE) | _BV(RXEN);

#else

#ifdef BAYBOARD

        UCSR1B |= _BV(RXCIE1);

#else

        UCSR1B |= _BV(RXCIE);

#endif

#endif

        sei();

#else

        xbee_stream = open(xbee_com_port, O_RDWR);

        if (xbee_stream == -1/* || lockf(xbee_stream, F_TEST, 0) != 0*/)

        {

                WL_DEBUG_PRINT("Failed to open connection to XBee on port ");

                WL_DEBUG_PRINT_INT(xbee_com_port);

                WL_DEBUG_PRINT(".\n");

                return -1;

        } else {

          WL_DEBUG_PRINT("Successfully opened connection to XBee on port ");

                WL_DEBUG_PRINT_INT(xbee_com_port);

                WL_DEBUG_PRINT(".\n");

        }

        // set baud rate, etc. correctly

        struct termios options;

        tcgetattr(xbee_stream, &options);

        cfsetispeed(&options, B9600);

        cfsetospeed(&options, B9600);

        options.c_iflag &= ~ICRNL;

        options.c_oflag &= ~OCRNL;

        options.c_cflag |= (CLOCAL | CREAD);

        options.c_cflag &= ~PARENB;

        options.c_cflag &= ~CSTOPB;

        options.c_cflag &= ~CSIZE;

        options.c_cflag |= CS8;

        options.c_lflag &= ~ICANON;

        options.c_cc[VMIN] = 1;

        options.c_cc[VTIME] = 50;

        if (tcsetattr(xbee_stream, TCSANOW, &options))

        {

                WL_DEBUG_PRINT("Error setting attributes.\n");

                return -1;

        }

        xbee_listen_thread = (pthread_t*)malloc(sizeof(pthread_t));

        if (xbee_listen_thread == NULL)

        {

                WL_DEBUG_PRINT("Malloc failed.\n");

                return -1;

        }

        int ret = pthread_create(xbee_listen_thread, NULL, listen_to_xbee, NULL);

        if (ret)

        {

                WL_DEBUG_PRINT("Failed to create listener thread.\n");

                return -1;

        }

#endif

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

        if (xbee_enter_command_mode() != 0) {

                return -1;

        }

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

        if (xbee_enter_api_mode() != 0) {

                return -1;

        }

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

        if (xbee_exit_command_mode() != 0) {

             return -1;

        }

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

        if (xbee_send_read_at_command("MY")) {

                return -1;

        }

        WL_DEBUG_PRINT("Getting ATMY address.\n");

#ifndef ROBOT

        int i;

        for (i = 0; xbee_address == 0 && i < XBEE_GET_PACKET_TIMEOUT; i++) {

          ret = xbee_get_packet(NULL);

          usleep(1000);

/*           if (ret == -1) { */

/*             WL_DEBUG_PRINT("xbee_get_packet(NULL) failed.\n"); */

/*             return -1; */

/*           } */

        }

#else

        //wait to return until the address is set

  //TODO: this shouldn't wait indefinitely.  There should be some sort of reasonable timeout

  // so if the address is never set right, an error can be returned instead of having the

  // robot hang forever

        while (xbee_address == 0) {

          xbee_get_packet(NULL);

        }

#endif

        WL_DEBUG_PRINT("Got ATMY address.\n");

#ifndef ROBOT

        if (i == XBEE_GET_PACKET_TIMEOUT) { // We timed-out.

          WL_DEBUG_PRINT("xbee_get_packet timed out.\n");

          return -1;

        } else {

          return 0;

        }

#else

        return 0;

#endif

}

/**

 * Call when finished using the XBee library. This releases

 * all sued resources.

 **/

void xbee_terminate()

{

        #ifndef ROBOT

        pthread_cancel(*xbee_listen_thread);

    pthread_join(*xbee_listen_thread, NULL);

        free(xbee_listen_thread);

        lockf(xbee_stream, F_ULOCK, 0);

        close(xbee_stream);

        #endif

}

/**

 * Send a buffer buf of size bytes to the XBee.

 *

 * @param buf the buffer of data to send

 * @param size the number of bytes to send

 **/

static int xbee_send(char* buf, int size)

{

#ifdef ROBOT

        int i;

        for (i = 0; i < size; i++) {

                xbee_putc(buf[i]);

        }

        return 0;

#else

        int ret = write(xbee_stream, buf, size);

        //success

        if (ret == size)

                return 0;

        if (ret == -1)

        {

                //interrupted by system signal, probably timer interrupt.

                //just try again

                if (errno == 4)

                {

                        return xbee_send(buf, size);

                }

                WL_DEBUG_PRINT("Failed to write to xbee\r\n");

                return -1;

        }

        //write was interrupted after writing ret bytes

        return xbee_send(buf + ret, size - ret);

#endif

}

/**

 * Sends a string to the XBee.

 *

 * @param c the string to send to the XBEE

 **/

//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if

// it reduces code size or not should be done to be sure.

static int xbee_send_string(char* c)

{

        return xbee_send(c, strlen(c));

}

#ifndef ROBOT

static int xbee_read(char* buf, int size)

{

        if (read(xbee_stream, buf, size) == -1) {

                WL_DEBUG_PRINT("Failed to read from xbee.\r\n");

                return -1;

        }

        return 0;

}

#endif

/**

 * Enter into command mode.

 **/

static int xbee_enter_command_mode()

{

        if (xbee_send_string("+++") != 0) {

                return -1;

        }

        if (xbee_wait_for_ok() != 0) {

          return -1;

        }

          return 0;

}

/**

 * Exit from command mode.

 **/

static int xbee_exit_command_mode()

{

        if (xbee_send_string("ATCN\r") != 0) {

                return -1;

        }

        xbee_wait_for_ok();

        return 0;

}

/**

 * Enter API mode.

 **/

static int xbee_enter_api_mode()

{

        if (xbee_send_string("ATAP 1\r") != 0) {

                return -1;

        }

        xbee_wait_for_ok();

        return 0;

}

/**

 * Wait until the string "OK\r" is received from the XBee.

 **/

//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if

// it reduces code size or not should be done to be sure.

static int xbee_wait_for_ok()

{

        return xbee_wait_for_string("OK\r", 3);

}

/**

 * Delay until the specified string is received from

 * the XBee. Discards all other XBee data.

 *

 * @param s the string to receive

 * @param len the length of the string

 **/

static int xbee_wait_for_string(char* s, int len)

{

        char* curr = s;

        while (curr - s < len) {

                // check if buffer is empty

                if (buffer_last != buffer_first) {

                        char c = arrival_buf[buffer_first++];

                        if (buffer_first == XBEE_BUFFER_SIZE) {

                                buffer_first = 0;

                        }

                        if (c == *curr) {

                                curr++;

                        } else {

#ifndef ROBOT

                          //return -1; // Computer is less forgiving.

                          curr = s;

#else

                          curr = s;

#endif

                        }

                } // else buffer is empty.

#ifndef ROBOT

                usleep(100);

#endif

        }

        return 0;

}

/**

 * Verifies that the packets checksum is correct.

 * (If the checksum is correct, the sum of the bytes

 * is 0xFF.)

 *

 * @param packet the packet received. This includes the first

 * three bytes, which are header information from the XBee.

 *

 * @param len The length of the packet received from the XBee

 *

 * @return 0 if the checksum is incorrect, nonzero

 * otherwise

 **/

int xbee_verify_checksum(char* packet, int len)

{

        unsigned char sum = 0;

        int i;

        for (i = 3; i < len; i++)

                sum += (unsigned char)packet[i];

        return sum == 0xFF;

}

/**

 * Returns the checksum of the given packet.

 *

 * @param buf the data for the packet to send

 * @param len the length of the packet in bytes

 *

 * @return the checksum of the packet, which will

 * become the last byte sent in the packet

 **/

char xbee_compute_checksum(char* buf, int len)

{

        int i;

        unsigned char sum = 0;

        for (i = 0; i < len; i++)

                sum += (unsigned char)buf[i];

        return 0xFF - sum;

}

/**

 * Adds header information and checksum to the given

 * packet and sends it. Header information includes

 * XBEE_FRAME_START and the packet length, as two bytes.

 *

 * @param buf the packet data

 * @param len the size in bytes of the packet data

 *

 **/

static int xbee_send_frame(char* buf, int len)

{

        char prefix[3];

        prefix[0] = XBEE_FRAME_START;

        prefix[1] = (len & 0xFF00) >> 8;

        prefix[2] = len & 0xFF;

        char checksum = xbee_compute_checksum(buf, len);

        if (xbee_send(prefix, 3) != 0) {

                return -1;

        }

        if (xbee_send(buf, len) != 0) {

                return -1;

        }

        if (xbee_send(&checksum, 1) != 0) {

                return -1;

        }

        return 0;

}

/**

 * Sends an AT command to read a parameter.

 *

 * @param command the AT command to send. For exmaple,

 * use ID to read the PAN ID and MY to return the XBee ID.

 * See the XBee reference guide for a complete listing.

 **/

//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if

// it reduces code size or not should be done to be sure.

static int xbee_send_read_at_command(char* command)

{

        return xbee_send_modify_at_command(command, NULL);

}

/**

 * Sends the given AT command.

 *

 * @param command the AT command to send (e.g., MY, ID)

 * @param value the value to pass as a parameter

 * (or NULL if there is no parameter)

 **/

static int xbee_send_modify_at_command(char* command, char* v