Colony

#include "xbee.h"

#include "wl_defs.h"

#ifndef ROBOT

#include <fcntl.h>

#include <unistd.h>

#include <pthread.h>

#include <errno.h>

#else

#include <serial.h>

#include <avr/interrupt.h>

#endif

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <queue.h>

#define XBEE_FRAME_START 0x7E

/*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*/

void xbee_send(char* buf, int size);

void xbee_send_string(char* c);

#ifndef ROBOT

void xbee_read(char* buf, int size);

#endif

/*Command Mode Functions

 * Called during initialization.

 */

void xbee_enter_command_mode(void);

void xbee_exit_command_mode(void);

void xbee_enter_api_mode(void);

void xbee_wait_for_string(char* s, int len);

void xbee_wait_for_ok(void);

/*API Mode Functions*/

int xbee_handle_packet(char* packet, int len);

void xbee_handle_at_command_response(char* command, char result,

        char* extra, int extraLen);

void xbee_handle_status(char status);

int xbee_verify_checksum(char* packet, int len);

char xbee_compute_checksum(char* packet, int len);

void xbee_send_frame(char* buf, int len);

void xbee_send_read_at_command(char* command);

void xbee_send_modify_at_command(char* command, char* value);

/*Global Variables*/

#ifndef ROBOT

int xbee_stream;

pthread_t* xbee_listen_thread;

#endif

Queue* xbee_queue;

//used to store packets as they are read

char xbee_buf[128];

int currentBufPos = 0;

//XBee status

unsigned int xbee_panID = XBEE_PAN_DEFAULT;

unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;

unsigned int xbee_address = 0;

/*Function Implementations*/

#ifdef ROBOT

/**

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

 * to the queue.

 **/

ISR(USART1_RX_vect)

{

        char c = UDR1;

        queue_add(xbee_queue, (void*)(int)c);

}

#else

/**

 * Thread that listens to the xbee.

 **/

void* listen_to_xbee(void* x)

{

        char c;

        while (1)

        {

                xbee_read(&c, 1);

                queue_add(xbee_queue, (void*)(int)c);

        }

        return 0;

}

#endif

/**

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

 *

 * @param pan_id the PAN to join initially. Use XBEE_PAN_DEFAULT

 * to leave the PAN as it is initially.

 **/

void xbee_lib_init(int pan_id)

{

        xbee_queue = queue_create();

        #ifdef ROBOT

        //enable the receiving interrupt

        UCSR1B |= _BV(RXCIE);

        sei();

        #else

        xbee_stream = open("/dev/ttyUSB0", O_RDWR);

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

                xbee_stream = open("/dev/ttyUSB1", O_RDWR);

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

        {

                printf("Failed to open connection to XBee.\r\n");

                exit(0);

        }

        lockf(xbee_stream, F_LOCK, 0);

        xbee_listen_thread =

                (pthread_t*)malloc(sizeof(pthread_t));

        int ret = pthread_create(xbee_listen_thread, NULL, 

                listen_to_xbee, NULL);

        if (ret)

        {

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

                exit(0);

        }

        #endif

        xbee_enter_command_mode();

        xbee_enter_api_mode();

        xbee_exit_command_mode();

        xbee_send_read_at_command("MY");

        //wait to return until the address is set

        while (xbee_address == 0) xbee_get_packet(NULL);

        if (pan_id != XBEE_PAN_DEFAULT)

        {

                xbee_set_pan_id(pan_id);

                while (xbee_panID == XBEE_PAN_DEFAULT) xbee_get_packet(NULL);

        }

}

/**

 * Call when finished using the XBee library. This releases

 * all sued resources.

 **/

void xbee_terminate()

{

        #ifndef ROBOT

        pthread_cancel(*xbee_listen_thread);

        free(xbee_listen_thread);

        lockf(xbee_stream, F_ULOCK, 0);

        close(xbee_stream);

        #endif

        queue_destroy(xbee_queue);

}

/**

 * 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

 **/

void xbee_send(char* buf, int size)

{

        #ifdef ROBOT

        int i;

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

                xbee_putc(buf[i]);

        #else

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

        //success

        if (ret == size)

                return;

        if (ret == -1)

        {

                //interrupted by system signal, probably timer interrupt.

                //just try again

                if (errno == 4)

                {

                        xbee_send(buf, size);

                        return;

                }

                printf("Failed to write to xbee, error %i.\r\n", errno);

                return;

        }

        //write was interrupted after writing ret bytes

        xbee_send(buf + ret, size - ret);

        #endif

}

/**

 * Sends a string to the XBee.

 *

 * @param c the string to send to the XBEE

 **/

void xbee_send_string(char* c)

{

        xbee_send(c, strlen(c));

}

#ifndef ROBOT

void xbee_read(char* buf, int size)

{

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

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

}

#endif

/**

 * Enter into command mode.

 **/

void xbee_enter_command_mode()

{

        xbee_send_string("+++");

        xbee_wait_for_ok();

}

/**

 * Exit from command mode.

 **/

void xbee_exit_command_mode()

{

        xbee_send_string("ATCN\r");

        xbee_wait_for_ok();

}

/**

 * Enter API mode.

 **/

void xbee_enter_api_mode()

{

        xbee_send_string("ATAP 1\r");

        xbee_wait_for_ok();

}

/**

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

 **/

void xbee_wait_for_ok()

{

        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

 **/

void xbee_wait_for_string(char* s, int len)

{

        char* curr = s;

        while (curr - s < len)

        {

                if (queue_is_empty(xbee_queue))

                        continue;

                char c = (char)(int)queue_remove(xbee_queue);

                if (c == *curr)

                        curr++;

                else

                        curr = s;

        }

}

/**

 * 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

 *

 **/

void 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);

        xbee_send(prefix, 3);

        xbee_send(buf, len);

        xbee_send(&checksum, 1);

}

/**

 * 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.

 **/

void xbee_send_read_at_command(char* command)

{

        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)

 **/

void xbee_send_modify_at_command(char* command, char* value)

{

        char buf[16];

        int i;

        buf[0] = XBEE_FRAME_AT_COMMAND;

        buf[1] = 1;

        buf[2] = command[0];

        buf[3] = command[1];

        int valueLen = 0;

        if (value != NULL)

        {

                valueLen = strlen(value);

                if (valueLen > 8)

                {

                        WL_DEBUG_PRINT("AT Command too large.\r\n");

                        return;

                }

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

                        buf[4 + i] = value[i];

        }

        xbee_send_frame(buf, 4 + valueLen);

}

/**

 * Send the specified packet.

 * 

 * @param packet the packet data to send

 * @param len the number of bytes in the packet

 * 

 * @param dest the ID of the XBee to send the packet to,

 * or XBEE_BROADCAST to send the message to all robots

 * in the PAN.

 * 

 * @param options a combination of the flags

 * XBEE_OPTIONS_NONE, XBEE_OPTIONS_DISABLE_RESPONSE and 

 * XBEE_OPTIONS_BROADCAST_ALL_PANS

 *

 * @param frame the frame number to associate this packet

 * with. This will be used to identify the response when

 * the XBee alerts us as to whether or not our message

 * was received.

 **/

void xbee_send_packet(char* packet, int len, int dest,

        char options, char frame)

{

        char buf[5];

        char prefix[3];

        int i;

        unsigned char checksum = 0;

        if (len > 100)

        {

                WL_DEBUG_PRINT("Packet is too large.\r\n");

                return;

        }

        //data for sending request

        buf[0] = XBEE_FRAME_TX_REQUEST_16;

        buf[1] = frame;

        buf[2] = (dest >> 8) & 0xFF;

        buf[3] = dest & 0xFF;

        buf[4] = options;

        //packet prefix, do this here so we don't need an extra buffer

        prefix[0] = XBEE_FRAME_START;

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

        prefix[2] = (5 + len) & 0xFF;

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

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

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

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

        checksum = 0xFF - checksum;

        xbee_send(prefix, 3);

        xbee_send(buf, 5);

        xbee_send(packet, len);

        xbee_send((char*)&checksum, 1);

}

/**

 * Reads a packet received from the XBee. This function

 * is non-blocking. The resulting packet is stored in dest.

 * Only returns transmission response packets and

 * received packets. The returned packet does not include

 * header information or the checksum. This method also

 * handles special packets dealt with by the XBee library,

 * and so should be called frequently while the XBee is in

 * use.<br><br>

 *

 * The first byte of the packet will be either

 * XBEE_TX_STATUS or XBEE_RX to indicated

 * a response to a sent message or a received message, 

 * respectively.<br><br>

 *

 * For a status response packet:<br>

 * The first byte will be XBEE_TX_STATUS.<br>

 * The second byte will be the frame number.<br>

 * The third byte will be the result. 0 indicates success,

 * and nonzero indicates that an error ocurred in 

 * transmitting the packet.<br><br>

 *

 * For a received packet:<br>

 * The first byte will be XBEE_RX.<br>

 * The second and third bytes will be the 16-bit

 * address of the packet's sender.<br>

 * The fourth byte is the signal strength.<br>

 * The fifth byte is 1 if the packet were sent to

 * a specific address, and 2 if it is a broadcast packet.<br><br>

 * 

 * @param dest set to the packet data

 * @return the length of the packet, or -1 if no packet

 * is available

 **/

int xbee_get_packet(unsigned char* dest)

{

        //start reading a packet with XBEE_FRAME_START

        if (currentBufPos == 0)

        {

                do

                        if (queue_is_empty(xbee_queue))

                                return -1;

                while ((char)(int)queue_remove(xbee_queue) != XBEE_FRAME_START);

                xbee_buf[0] = XBEE_FRAME_START;

                currentBufPos++;

        }

        int len = -1;

        if (currentBufPos >= 3)

                len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);

        while (len == -1 //packet length has not been read yet

                        || currentBufPos < len + 4)

        {

                if (currentBufPos == 3)

                        len = (int)xbee_buf[2] + ((int)xbee_buf[1] << 8);

                if (queue_is_empty(xbee_queue))

                        return -1;

                xbee_buf[currentBufPos++] = (char)(int)queue_remove(xbee_queue);

        }

        currentBufPos = 0;

        if (!xbee_verify_checksum(xbee_buf, len + 4))

        {

                WL_DEBUG_PRINT("XBee checksum failed.\r\n");

                return -1;

        }

        //we will take care of the packet

        if (xbee_handle_packet(xbee_buf + 3, len))

                return -1;

        if (dest == NULL)

                return -1;

        int i;

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

                dest[i - 3] = xbee_buf[i];

        return len;

}

/**

 * Handles modem status packets.

 *

 * @param status the type of status packet received.

 **/

void xbee_handle_status(char status)

{

        switch (status)

        {

                case 0:

                        WL_DEBUG_PRINT("XBee hardware reset.\r\n");

                        break;

                case 1:

                        WL_DEBUG_PRINT("Watchdog timer reset.\r\n");

                        break;

                case 2:

                        WL_DEBUG_PRINT("Associated.\r\n");

                        break;

                case 3:

                        WL_DEBUG_PRINT("Disassociated.\r\n");

                        break;

                case 4:

                        WL_DEBUG_PRINT("Synchronization lost.\r\n");

                        break;

                case 5:

                        WL_DEBUG_PRINT("Coordinator realignment.\r\n");

                        break;

                case 6:

                        WL_DEBUG_PRINT("Coordinator started.\r\n");

                        break;

        }

}

/**

 * Handles AT command response packets.

 * @param command the two character AT command, e.g. MY or ID

 * @param result 0 for success, 1 for an error

 * @param extra the hex value of the requested register

 * @param extraLen the length in bytes of extra

 **/

void xbee_handle_at_command_response(char* command, char result,

        char* extra, int extraLen)

{

        if (result == 1)

        {

                WL_DEBUG_PRINT("Error with AT");

                WL_DEBUG_PRINT(command);

                WL_DEBUG_PRINT(" packet.\r\n");

        }

        WL_DEBUG_PRINT("AT");

        WL_DEBUG_PRINT(command);

        WL_DEBUG_PRINT(" command was successful.\r\n");

        if (command[0] == 'I' && command[1] == 'D')

        {

                xbee_panID = xbee_pending_panID;

                WL_DEBUG_PRINT("PAN ID set to ");

                WL_DEBUG_PRINT_INT(xbee_panID);

                WL_DEBUG_PRINT(".\r\n");

                return;

        }

        if (command[0] == 'M' && command[1] == 'Y' && extraLen != 0)

        {

                xbee_address = 0;

                int i;

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

                        xbee_address = (xbee_address << 8) + extra[i];

                WL_DEBUG_PRINT("XBee address is ");

                WL_DEBUG_PRINT_INT(xbee_address);

                WL_DEBUG_PRINT(".\r\n");

                if (xbee_address == 0)

                {

                        WL_DEBUG_PRINT("XBee 16-bit address must be set using ATMY.\r\n");

                        exit(0);

                }

        }

}

/**

 * Attempts to handle the packet if it is dealt with

 * by the library.

 * We will handle the following packet types:

 *    Modem Status

 *    AT Command Response

 *

 * @param packet the packet to handle

 * @param len the length of the packet

 * 

 * @return 1 if we have handled the packet, 0 otherwise

 */

int xbee_handle_packet(char* packet, int len)

{

        char command[3] = {1, 2, 3};

        if (len <= 0) //this should not happend

        {

                WL_DEBUG_PRINT("Non-positive packet length.\r\n");

                return 0;

        }

        switch ((unsigned char)packet[0]) //packet type

        {

                case XBEE_FRAME_STATUS:

                        xbee_handle_status(packet[1]);

                        return 1;

                case XBEE_FRAME_AT_COMMAND_RESPONSE:

                        command[0] = packet[2];

                        command[1] = packet[3];

                        command[2] = 0;

                        xbee_handle_at_command_response(command,

                                packet[4], packet + 5, len - 5);

                        return 1;

        }

        return 0;

}

/**

 * Sets the personal area network id.

 *

 * @param id the new personal area network (PAN) id

 **/

void xbee_set_pan_id(int id)

{

        char s[3];

        s[0] = (id >> 8) & 0xFF;

        s[1] = id & 0xFF;

        s[2] = 0;

        xbee_pending_panID = id;

        xbee_send_modify_at_command("ID", s);

}

/**

 * Get the PAN ID for the XBee.

 * 

 * @return the personal area network id, or

 * XBEE_PAN_DEFAULT if it has not yet been set.

 **/

unsigned int xbee_get_pan_id()

{

        return xbee_panID;

}

/**

 * Get the 16-bit address of the XBee.

 * This is used to specify who to send messages to

 * and who messages are from.

 *

 * @return the 16-bit address of the XBee.

 **/

unsigned int xbee_get_address()

{

        return xbee_address;

}