Colony

/**

 * Copyright (c) 2009 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 Colony Project, CMU Robotics Club

 **/

#include <lights.h>

#include <string.h>

#include <avr/io.h>

#include <avr/interrupt.h>

#include <time.h>

#include <wl_defs.h>

#include <wireless.h>

#include "xbee.h"

/* Internal Function Prototypes */

/* I/O Functions */

static int8_t xbee_send_string(uint8_t* c);

/* Command Mode Functions */

static int8_t xbee_enter_command_mode(void);

static int8_t xbee_exit_command_mode(void);

static int8_t xbee_enter_api_mode(void);

static int8_t xbee_exit_api_mode(void);

static int8_t xbee_wait_for_string(uint8_t* s, uint16_t len);

static int8_t xbee_wait_for_ok(void);

/* API Mode Functions */

//TODO: does this exist?  static int8_t xbee_handle_packet(uint8_t* packet, uint16_t len);

static int8_t xbee_handle_at_command_response(uint16_t command, uint8_t status, uint8_t len);

static void xbee_handle_status(uint8_t status);

static int8_t xbee_verify_checksum(uint8_t* packet, uint16_t len);

static uint8_t xbee_compute_checksum(uint8_t* packet, uint16_t len);

static int8_t xbee_send_frame(uint8_t* buf, uint16_t len);

int8_t xbee_send_read_at_command(uint8_t* command);

static int8_t xbee_send_modify_at_command(uint8_t* command, uint8_t* value, uint8_t len);

/* Buffer Manipulation Functions */

int8_t xbee_basic_buf_add(uint8_t *ptr, uint8_t byte);

uint8_t xbee_basic_buf_get(uint8_t *ptr);

int8_t xbee_other_buf_add(uint8_t *ptr, uint8_t byte);

/* private functions */

int8_t check_last_receive(uint16_t source,uint8_t framenum);

inline uint8_t getStatus(uint8_t mask);

inline void setStatus(uint8_t mask,uint8_t value);

/*Global Variables*/

// last few packet sources and frame nums

#define NUM_LAST_PACKETS 10

struct {

  uint16_t source;

  uint8_t framenum;

} lastPacket[NUM_LAST_PACKETS];

// array for basic packets

uint8_t xbee_basic_buf[PACKET_BUFFER_SIZE];

// beginning of first packet in basic buffer

uint8_t basic_buf_first = 0xFF;

// byte after end of last packet in basic buffer (only access < basic_buf_last)

// aka, the first free byte in basic buffer

uint8_t basic_buf_last = 0;

// array for other packets

uint8_t xbee_other_buf[PACKET_BUFFER_SIZE];

// beginning of first packet in other buffer

uint8_t other_buf_first = 0xFF;

// byte after end of last packet in other buffer (only access < other_buf_last)

// aka, the first free byte in other buffer

uint8_t other_buf_last = 0;

// xbee status

#define XBEE_API_OFF 0x00

#define XBEE_API_ON 0x10

#define XBEE_API_ESCAPE 0x20

#define XBEE_API_MASK 0x30

#define XBEE_COMMAND_WAIT 0x80

#define XBEE_COMMAND_RESPONSE 0xC0

#define XBEE_COMMAND_NONE 0x00

#define XBEE_COMMAND_MASK 0xC0

#define XBEE_NOT_INITD 0xF0

#define LAST_PACKET_MASK 0x0F

uint8_t xbee_status = XBEE_NOT_INITD;

// xbee command response (for PAN, channel, address, etc)

static uint8_t xbee_command[8];

// external ack handler (wireless_send.c)

extern void ackhandle(uint8_t num,uint8_t val);

/**@addtogroup xbee

 * @{ **/

/*Function Implementations*/

/**

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

 * to the buffer.

 **/

#ifndef FIREFLY

#define PORT UDR1

#define FLAG (UCSR1A & (1<<RXC1))

ISR(USART1_RX_vect) {

#else

#define PORT UDR0

#define FLAG (UCSR0A & (1<<RXC0))

SIGNAL(SIG_USART0_RECV)

#endif

  // start of frame

  uint8_t apitype = PORT; // get frame start byte

  uint16_t i=0;

  uint16_t len=0;

  /*WL_DEBUG_PRINT("in interrupt|status=");

  WL_DEBUG_PRINT_HEX(xbee_status);

  WL_DEBUG_PRINT("|frame_start=");

  WL_DEBUG_PRINT_HEX(apitype);

  if (getStatus(XBEE_API_MASK) == XBEE_API_OFF) {

    WL_DEBUG_PRINT("|API OFF");

  } else if (getStatus(XBEE_API_MASK) == XBEE_API_ON) {

    WL_DEBUG_PRINT("|API ON");

  } else {

    WL_DEBUG_PRINT("|API ERROR");

  }*/

  // check that we're in API mode

  if (getStatus(XBEE_API_MASK) == XBEE_API_OFF || apitype != XBEE_FRAME_START) {

    // not in API mode

    WL_DEBUG_PRINT("|api off branch");

    if (getStatus(XBEE_COMMAND_MASK) == XBEE_COMMAND_WAIT) {

      // get rest of command and put in basic buf

      xbee_basic_buf[i] = apitype;

      if (xbee_basic_buf[i++] != '\r') {

        while(i < PACKET_BUFFER_SIZE) {

          if (FLAG) {

            xbee_basic_buf[i] = PORT;

            if (xbee_basic_buf[i] == '\r')

              break;

            i++;

          }

        }

      }

      WL_DEBUG_PRINT("got packet, len=");

      WL_DEBUG_PRINT_INT(i);

      WL_DEBUG_PRINT("str=");

      WL_DEBUG_PRINT(xbee_basic_buf);

      // signal handler that command response is done

      setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_RESPONSE);

    }

    WL_DEBUG_PRINT("return\r\n");

    return;

  }  

  //WL_DEBUG_PRINT("|api on branch");

  // get length and type

  while(i<3) {

    if (FLAG) {

      if (i==0) {

        len |= PORT<<8;

        //WL_DEBUG_PRINT("|len_hi");        

      } else if (i==1) {

        len |= PORT;

        //WL_DEBUG_PRINT("|len_lo");

      } else if (i==2) {      

        apitype = PORT;

        //WL_DEBUG_PRINT("|GOT APITYPE");

      }

      i++;

    }

  }

  // do something based on the type

  i=1;

  switch(apitype) {

  case XBEE_FRAME_AT_COMMAND_RESPONSE: {

    // AT command response

    if (getStatus(XBEE_COMMAND_MASK) == XBEE_COMMAND_RESPONSE)

      return; // we're currently processing a command, so drop the incoming one  ***** TODO: we need to read the whole length of the frame to do this right ****

    uint16_t atcommand=0;

    uint8_t ptr=basic_buf_last;

    while(i<len) {

      if (FLAG) {

        if (i==1)

          apitype = PORT; // get frame id, but ignore it

        else if (i==2)

          atcommand |= PORT<<8; // get command char1

        else if (i==3)

          atcommand |= PORT; // get command char2

        else if (i==4)

          apitype = PORT; // get status

        else {

          // put the command response on the basic buf temporarily

          if (xbee_basic_buf_add(&ptr,PORT) != 0)

            break;

        }

        i++;

      }

    }

    // handle AT command

    WL_DEBUG_PRINT("i=");

    WL_DEBUG_PRINT_INT(i);

    WL_DEBUG_PRINT("|len=");

    WL_DEBUG_PRINT_INT(len);

    WL_DEBUG_PRINT("|status:");

    WL_DEBUG_PRINT_INT(apitype);

    WL_DEBUG_PRINT("|atcommand:");

    WL_DEBUG_PRINT_CHAR((uint8_t)(atcommand>>8));

    WL_DEBUG_PRINT_CHAR((uint8_t)(atcommand&0xFF));

    WL_DEBUG_PRINT("|buf=");

    uint8_t ptr2 = basic_buf_last;

    for(uint8_t j=0;j<i;j++)

      WL_DEBUG_PRINT_HEX(xbee_basic_buf_get(&ptr2));

    WL_DEBUG_PRINT("|\r\n");

    xbee_handle_at_command_response(atcommand,apitype,len-5); // TODO: rewrite function

    break; }

  case XBEE_FRAME_TX_STATUS: {

    // TX status

    uint8_t frame_id = 0;

    while(i<len) {

      if (FLAG) {

        if (i==1)

          frame_id = PORT;

        else {

          ackhandle(frame_id,PORT); // handle the status

          break;

        }

        i++;

      }

    }

    break; }

  case XBEE_FRAME_RX_64: {

    // receive a packet with 64bit address

    break; } // TODO: implement this (even if we don't use it)

  case XBEE_FRAME_RX_16: {

    // receive a packet with 16bit address

    uint16_t source = 0;

    uint8_t framenum = 0;

    uint8_t group = 0;

    uint8_t ptr=basic_buf_last;

    while(i<len) {

      if (FLAG) {

        if (i==1)

          source |= PORT<<8; // get source hi byte

        else if (i==2)

          source |= PORT; // get source lo byte

        else if (i==3)

          apitype = PORT; // get RSSI, and ignore

        else if (i==4)

          apitype = PORT; // get options, and ignore

        else if (i==5) {

          framenum = PORT; // get the frame number

          if (check_last_receive(source,framenum) != WL_SUCCESS) {

            // we've already received this frame

            ptr = 0xFF; // signal to skip processing

            break;

          }

        }

        else if (i==6) {

          group = PORT; // get group number

          if (group == 0) {

            ptr = basic_buf_last+1;

            // add source to buffer

            if (xbee_basic_buf_add(&ptr,(uint8_t)((source&0xFF00)>>8)) != 0)

              break;

            if (xbee_basic_buf_add(&ptr,(uint8_t)(source&0x00FF)) != 0)

              break;

          } else {

            ptr = other_buf_last+1;

            // add source and group to buffer

            if (xbee_other_buf_add(&ptr,group) != 0)

              break;

            if (xbee_other_buf_add(&ptr,(uint8_t)((source&0xFF00)>>8)) != 0)

              break;

            if (xbee_other_buf_add(&ptr,(uint8_t)(source&0x00FF)) != 0)

              break;

          }

        }

        else { // TODO: handle escaped characters supported by APIv2

          // put packet data on the correct buffer

          if (group == 0 && xbee_basic_buf_add(&ptr,PORT) != 0)

            break;

          else if (xbee_other_buf_add(&ptr,PORT) != 0)

            break;

        }

        i++;

      }

    }

    if (ptr != 0xFF && i > 6) {

      if (group == 0) {

        xbee_basic_buf[basic_buf_last] = i-6; // set length

        basic_buf_last = ptr;

      }

      else {        

        xbee_other_buf[other_buf_last] = i-6; // set length

        // check if we have a high priority group

        for(;;)

          if (HIGH_PRIORITY) {

            // handle receive now

            ptr = 0xFF;

            break;

          }        

        if (ptr != 0xFF) {

          // handle receive later

          other_buf_last = ptr;

        }

      }

    }

    break; }

    default:

      WL_DEBUG_PRINT("|BAD APITYPE");

  } // end of switch statement

  while (1) {

    if (FLAG) {

      apitype = PORT; // get checksum, and ignore

      break;

    }

  }

  WL_DEBUG_PRINT("|interrupt - return\r\n");

} // end of interrupt 

/* adds a byte to the basic buffer */

int8_t xbee_basic_buf_add(uint8_t *ptr, uint8_t byte) {

  if (*ptr == basic_buf_first) {

    // buffer full

    WL_DEBUG_PRINT("basic buffer full\r\n");

    return -1;

  }

  xbee_basic_buf[(*ptr)++] = byte;

  if (*ptr == PACKET_BUFFER_SIZE)

    *ptr = 0;

  return 0;

}

/* gets a byte from the basic buffer */

uint8_t xbee_basic_buf_get(uint8_t *ptr) {

  uint8_t byte = xbee_basic_buf[(*ptr)++];

  if (*ptr == PACKET_BUFFER_SIZE)

    *ptr = 0;

  return byte;

}

/* adds a byte to the other buffer */

int8_t xbee_other_buf_add(uint8_t *ptr, uint8_t byte) {

  if (*ptr == other_buf_first) {

    // buffer full

    WL_DEBUG_PRINT("other buffer full\r\n");

    return -1;

  }

  xbee_other_buf[(*ptr)++] = byte;

  if (*ptr == PACKET_BUFFER_SIZE)

    *ptr = 0;

  return 0;

}

/**

 * Checks if packet is a duplicate

 **/

int8_t check_last_receive(uint16_t source,uint8_t framenum) {

  uint8_t i=0;

  for(;i<NUM_LAST_PACKETS;i++) {

    if (lastPacket[i].source == source && lastPacket[i].framenum == framenum)

      return -1; // duplicate packet, so return error

  }

  // save packet source and framenum

  i=getStatus(LAST_PACKET_MASK);

  lastPacket[i].source = source;

  lastPacket[i].framenum = framenum;

  if (++i>=NUM_LAST_PACKETS)

    i = 0;

  setStatus(LAST_PACKET_MASK,i);

  return WL_SUCCESS;

}

/** status functions **/

inline uint8_t getStatus(uint8_t mask) { return xbee_status&mask; }

void setStatus(uint8_t mask,uint8_t value) { xbee_status = ((xbee_status&(~mask))|value); }

// test interrupt

/*ISR(USART1_RX_vect) {

  WL_DEBUG_PRINT("in interrupt|");

  char c = UDR1;

  WL_DEBUG_PRINT_HEX(c);

  WL_DEBUG_PRINT("|in interrupt");

}*/

/**

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

 **/

int8_t xbee_init()

{

  WL_DEBUG_PRINT("in xbee_init\r\n");

  if(getStatus(XBEE_NOT_INITD) != XBEE_NOT_INITD) {

    return WL_ERROR_INIT_ALREADY_INITD;

  }

  // set status

  setStatus(XBEE_NOT_INITD,XBEE_COMMAND_NONE);

  // clear last packet buffer

  for(int i=0;i<NUM_LAST_PACKETS;i++)

    lastPacket[i].source = lastPacket[i].framenum = 0;

  // Set startup baud rate of 9600

  // Set frame format: 8data, 1stop bit, asynchronous normal mode

  // Enable receiver and transmitter and the receiving interrupt

#ifdef FIREFLY

  UBRR0H = 0x00;

  UBRR0L = 103;

  UCSR0A |= (1<<U2X0);

  UCSR0C |= (1<<UCSZ00) | (1<<UCSZ01);

  UCSR0B |= (1<<RXEN0) | (1<<TXEN0) | (1<<RXCIE); 

#else

  // Bayboard or robot  

  UBRR1H = 0x00; // baud rate

  UBRR1L = 103;  // baud rate

  UCSR1A |= (1<<U2X1); // double transmit speed 

  //Enable receiver and transmitter on USART1

  UCSR1B |= (1<<RXEN1)|(1<<TXEN1);

  // Set frame format: 8data, 1stop bit, asynchronous normal mode

  UCSR1C |= (1<<UCSZ10) | (1<<UCSZ11);

  UCSR1B |= (1<<RXCIE1);  // enable receive, transmit (1<<RXCIE)

#endif

  sei();

  // enter command mode

  WL_DEBUG_PRINT("entering command mode\r\n");

  xbee_send_string((uint8_t*)"+++");

  xbee_wait_for_ok();

  WL_DEBUG_PRINT("entered command mode\r\n");

  // set baud on xbee

#if (XBEE_BAUD == 115200)

  xbee_send_string((uint8_t*)"ATBD7\r");

#elif (XBEE_BAUD == 57600)

  xbee_send_string((uint8_t*)"ATBD6\r");

#elif (XBEE_BAUD == 38400)

  xbee_send_string((uint8_t*)"ATBD5\r");

#elif (XBEE_BAUD == 19200)

  xbee_send_string((uint8_t*)"ATBD4\r");

#elif (XBEE_BAUD == 9600)

  // already at this baud rate

  xbee_send_string("ATBD3\r\n");

#else

  WL_DEBUG_PRINT("undefined baud rate\r\n");

  return WL_ERROR_BAUD;

#endif  

  // exit command mode

  xbee_wait_for_ok();

  WL_DEBUG_PRINT("got ok from baud reset\r\n");

  xbee_send_string((uint8_t*)"ATCN\r");

  xbee_wait_for_ok();

  WL_DEBUG_PRINT("got ok from exiting command mode\r\n");

  // set UART baud

#ifdef FIREFLY

#if (XBEE_BAUD == 115200)

  UBRR0H = 0x00;

  UBRR0L = 8;

#elif (XBEE_BAUD == 57600)

  UBRR0H = 0x00;

  UBRR0L = 16;

#elif (XBEE_BAUD == 38400)

  UBRR0H = 0x00;

  UBRR0L = 25;

#elif (XBEE_BAUD == 19200)

  UBRR0H = 0x00;

  UBRR0L = 51;

#elif (XBEE_BAUD == 9600)

  /* this is the default baud rate, so do nothing

  UBRR0H = 0x00;

  UBRR0L = 103;*/

#else

  WL_DEBUG_PRINT("undefined baud rate\r\n");

  return WL_ERROR_BUAD;

#endif

#else // Bayboard or robot

#if (XBEE_BAUD == 115200)

  UBRR1H = 0x00;

  UBRR1L = 8;

#elif (XBEE_BAUD == 57600)

  UBRR1H = 0x00;

  UBRR1L = 16;

#elif (XBEE_BAUD == 38400)

  UBRR1H = 0x00;

  UBRR1L = 25;

#elif (XBEE_BAUD == 19200)

  UBRR1H = 0x00;

  UBRR1L = 51;

#elif (XBEE_BAUD == 9600)

  /* this is the default baud rate, so do nothing

  UBRR1H = 0x00;

  UBRR1L = 103;*/

#else

  WL_DEBUG_PRINT("undefined baud rate\r\n");

  return WL_ERROR_BUAD;

#endif

#endif

  // wait half second for the baud change to stabalize

  delay_ms(500);

  // enter command mode

  WL_DEBUG_PRINT("entering command mode 2\r\n");

  xbee_send_string((uint8_t*)"+++");

  xbee_wait_for_ok();

  WL_DEBUG_PRINT("entered command mode 2\r\n");

  if (xbee_enter_api_mode() != 0) {

    WL_DEBUG_PRINT("can't enter api mode\r\n");

    return -1;

  }

  /*xbee_send_string((uint8_t*)"ATAP 1\r");

  xbee_wait_for_ok();*/

  /*WL_DEBUG_PRINT("Entered api mode.\r\n");

  if (xbee_exit_command_mode() != 0) {

    WL_DEBUG_PRINT("can't exit command mode\r\n");

    return -1;

  }

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_NONE); // clear status

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

  WL_DEBUG_PRINT("before status=");

  WL_DEBUG_PRINT_HEX(getStatus(0xFF));

  WL_DEBUG_PRINT("\r\n");

  setStatus(XBEE_API_MASK,XBEE_API_ON); // set status to API_ON (API mode 1)*/

#ifdef WL_DEBUG

  if (getStatus(XBEE_API_MASK) == XBEE_API_OFF) {

    WL_DEBUG_PRINT("|API OFF|");

  } else if (getStatus(XBEE_API_MASK) == XBEE_API_ON) {

    WL_DEBUG_PRINT("|API ON|");

  } else {

    WL_DEBUG_PRINT("|API ERROR|");

  }

#endif

  // TODO: we should set the MY address to the robot address from eeprom  

  uint16_t i=0;

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_WAIT);  

  uint16_t newmy = 0x9876;

  if (xbee_send_modify_at_command((uint8_t*)"MY",&newmy,2) != WL_SUCCESS) {

    WL_DEBUG_PRINT("setting MY address failed\r\n");

  }

  // wait for up to 30 ms

  while(getStatus(XBEE_COMMAND_MASK) != XBEE_COMMAND_RESPONSE && i++ < 10000) {

    delay_us(1); // wait 3us

  }  

  if (i < 1000 && xbee_command[0] == 'M' && xbee_command[1] == 'Y') {

    WL_DEBUG_PRINT("setting MY address successful\r\n");

  } else

    WL_DEBUG_PRINT("setting MY address failed\r\n");

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_NONE); // reset status

  uint16_t address = xbee_get_address();

  if (address == WL_ERROR_XBEE_COMMAND_16BIT) {

    WL_DEBUG_PRINT("getting MY address failed\r\n");

  }

  WL_DEBUG_PRINT("MY address:");

  WL_DEBUG_PRINT_HEX((uint8_t)(address>>8));

  WL_DEBUG_PRINT_HEX((uint8_t)(address&0xFF));

  WL_DEBUG_PRINT("\r\n");

  return WL_SUCCESS;

}

/**

 * Call when finished using the XBee library.

 **/

int8_t xbee_terminate()

{

  int ret=xbee_exit_api_mode();

  if (ret != WL_SUCCESS) {

    WL_DEBUG_PRINT("xbee termination failed\r\n");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT("|\r\n");

    return WL_ERROR_TERMINATION_FAILED;

  }

  setStatus(XBEE_NOT_INITD,XBEE_NOT_INITD); // clean initd status

  return WL_SUCCESS;

}

/**

 * Sends a character to the XBee.

 *

 * @param c the byte to send

 * @return 0 for success, nonzero for failure

 **/

int8_t xbee_putc(uint8_t c) {

  // Wait until buffer is clear for sending

  // Then load buffer with your character

  /*WL_DEBUG_PRINT("xbee_putc:");

  WL_DEBUG_PRINT_HEX(c);

  WL_DEBUG_PRINT("|\r\n");*/

#ifdef FIREFLY

  loop_until_bit_is_set(UCSR0A, UDRE0);  

  UDR0 = c;

#else

  loop_until_bit_is_set(UCSR1A, UDRE1);

  UDR1 = c;

#endif

  return WL_SUCCESS;

}

/**

 * Returns the first byte in the buffer received from xbee.

 * This function blocks execution until a character has been

 * received. xbee_init must be called before this function

 * may be used.

 * 

 * @return the first character in the xbee buffer, -1 on error

 * 

 * @see xbee_init, xbee_getc_nb

 **/

int16_t xbee_getc(void) {

  // Wait for the receive buffer to be filled

  // Then read the receive buffer

#ifdef FIREFLY

  loop_until_bit_is_set(UCSR0A, RXC0);  

  return UDR0;

#else

  loop_until_bit_is_set(UCSR1A, RXC1);

  return UDR1;

#endif

}

/**

 * Non blocking version of xbee_getc. If a byte is present in the buffer,

 * it is returned, otherwise -1 is returned immediately. xbee_init

 * must be called before this function can be used.

 *

 * @param c The received byte. This will be set if a byte has been received.

 * 

 * @return -1 If no byte is available, 0 otherwise, positive for error

 *

 * @see xbee_getc

 **/

int8_t xbee_getc_nb(uint8_t *c) {

  // check if the receive buffer is filled

#ifdef FIREFLY

  if (UCSR0A & (1<<RXC0)) {

    (*c) = UDR0;

#else

  if (UCSR1A & (1<<RXC1)) {

    (*c) = UDR1;

#endif

    return WL_SUCCESS;

  }

  return -1; // Return empty

}

/**

 * 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

 **/

int8_t xbee_send(uint8_t* buf, uint16_t size)

{

  uint16_t i=0; // check if we need this variable

  while(i<size) {

    if (xbee_putc(buf[i++]) != WL_SUCCESS)

      return WL_ERROR_SEND;

  }

  return WL_SUCCESS;

}

/**

 * Sends a string to the XBee.

 *

 * @param c the string to send to the XBEE

 **/

static int8_t xbee_send_string(uint8_t* c)

{

  return xbee_send(c, strlen((char*)c));

}

/**

 * Enter into command mode.

 **/

static int8_t xbee_enter_command_mode(void)

{

  if (xbee_send_string((uint8_t*)"+++") != WL_SUCCESS) {

    return WL_ERROR_XBEE_COMMAND;

  }

  WL_DEBUG_PRINT("sent command +++|");

  if (xbee_wait_for_ok() != WL_SUCCESS) {

    return WL_ERROR_XBEE_COMMAND;

  }

  WL_DEBUG_PRINT("got OK\r\n");

  return WL_SUCCESS;

}

/**

 * Exit from command mode.

 **/

static int8_t xbee_exit_command_mode()

{

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

    return WL_ERROR_SEND;

  }

  if (xbee_wait_for_ok() != WL_SUCCESS) {

    WL_DEBUG_PRINT("failed to exit command mode\r\n");

    return WL_ERROR_SEND;

  }

  return WL_SUCCESS;

}

/**

 * Enter API mode.

 **/

static int8_t xbee_enter_api_mode(void) {

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

    return WL_ERROR_SEND;

  }

  if (xbee_wait_for_ok() != WL_SUCCESS) {

    WL_DEBUG_PRINT("failed to enter API mode\r\n");

    return WL_ERROR_SEND;

  }

  if (xbee_exit_command_mode() != WL_SUCCESS) {

    WL_DEBUG_PRINT("failed to enter API mode\r\n");

    return WL_ERROR_SEND;

  }    

  WL_DEBUG_PRINT("got OK after entering API mode\r\n");

  setStatus(XBEE_API_MASK,XBEE_API_ON); // set status

  return WL_SUCCESS;

}

/**

 * Enter API mode 2.

 **/

static int8_t xbee_enter_api_mode2(void) {

  if (xbee_send_string((uint8_t*)"ATAP 2\r") != 0) {

    return WL_ERROR_SEND;

  }

  if (xbee_wait_for_ok() != WL_SUCCESS) {

    WL_DEBUG_PRINT("failed to enter API mode2\r\n");

    return WL_ERROR_SEND;

  }

  if (xbee_exit_command_mode() != WL_SUCCESS) {

    WL_DEBUG_PRINT("failed to enter API mode2\r\n");

    return WL_ERROR_SEND;

  }    

  WL_DEBUG_PRINT("got OK after entering API mode2\r\n");

  setStatus(XBEE_API_MASK,XBEE_API_ESCAPE); // set status

  return WL_SUCCESS;

}

/**

 * Exit API mode.

 **/

static int8_t xbee_exit_api_mode()

{

  if (getStatus(XBEE_COMMAND_MASK) == XBEE_COMMAND_WAIT

    || getStatus(XBEE_COMMAND_MASK) == XBEE_COMMAND_RESPONSE)

    return WL_ERROR_XBEE_COMMAND; // can't do command right now

  WL_DEBUG_PRINT("now exiting API mode\r\n");

  int16_t i=0;

  // change status to command wait

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_WAIT);

  if (xbee_send_modify_at_command((uint8_t*)"AP",(uint8_t*)(&i),1) != WL_SUCCESS) { // send command

    WL_DEBUG_PRINT("error sending AP 0 command\r\n");

    setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_NONE); // reset status

    return WL_ERROR_XBEE_COMMAND;

  }

  // wait for up to 30 ms

  while(getStatus(XBEE_COMMAND_MASK) != XBEE_COMMAND_RESPONSE && i++ < 10000) {

    delay_us(1); // wait 3us

  }

  if (i < 1000 && xbee_command[0] == 'A' && xbee_command[1] == 'P') {

    WL_DEBUG_PRINT("done exiting API mode\r\n");

    i = WL_SUCCESS;

  } else {

    WL_DEBUG_PRINT("failed to exit API mode\r\n");

    i = WL_ERROR_XBEE_COMMAND; // set error code

  }

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_NONE); // reset status

  setStatus(XBEE_API_MASK,XBEE_API_OFF);

  return (int8_t)i; // return

}

/**

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

 **/

static int8_t xbee_wait_for_ok()

{

  //delay_ms(1000);

  //return WL_SUCCESS;

  return xbee_wait_for_string((uint8_t*)"OK", 2);

}

/**

 * Delay until the specified string is received from

 * the XBee.

 *

 * Only works when not in API mode

 *

 * @param s the string to receive

 * @param len the length of the string

 **/

static int8_t xbee_wait_for_string(uint8_t* s, uint16_t len)

{

  uint8_t i=0;  

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_WAIT); // set status flag

  if (getStatus(XBEE_API_MASK) == XBEE_API_OFF) {

    // wait until the response is received (only wait 1 second)

    while(getStatus(XBEE_COMMAND_MASK) != XBEE_COMMAND_RESPONSE && i++ < 1000) {

      delay_us(1);

    }

    // check response

    if (i >= 1000 || memcmp(s,xbee_basic_buf,len) != 0) {

      // bad response

      WL_DEBUG_PRINT("Bad response when waiting for string ");

      WL_DEBUG_PRINT(s);

      WL_DEBUG_PRINT("\r\n");

      return -1;

    }

    setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_NONE); // clear status

  }

  WL_DEBUG_PRINT("done waiting for string.\r\n");

  return 0;

}

/**

 * Delay until we receive a command response.

 * Then copy the response to S and set the len

 *

 * Only works when not in API mode

 *

 * @param s the string to store the response in

 * @param len the length of the string

 */

static int8_t xbee_wait_for_response(uint8_t* s, int16_t len) {

  uint8_t i=0;

  if (getStatus(XBEE_API_MASK) == XBEE_API_OFF) {

    // wait until the response is received (only wait 1 second)

    while(getStatus(XBEE_COMMAND_MASK) != XBEE_COMMAND_RESPONSE && i++ < 1000) {

      delay_us(1);

    }

    // check response

    if (i >= 1000) {

      return -1;

    } else {

      i=strcspn((char*)xbee_basic_buf,"\r");

      if (i<PACKET_BUFFER_SIZE) {

        memcpy(s,xbee_basic_buf,i);

        len = i;

        setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_NONE); // clear response

        return 0;

      }

      else

        return -1;      

    }

  }

  // TODO: do something for API mode

  return 0;

}

/**  TODO: since we don't use this, do we need it?

 *

 * 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

 **/

int8_t xbee_verify_checksum(uint8_t* packet, uint16_t len)

{

  uint8_t sum = 0;

  while(--len > 0) {

    sum += packet[len];

  }

  sum += packet[0];

  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

 **/

uint8_t xbee_compute_checksum(uint8_t* buf, uint16_t len)

{

  uint8_t sum = 0;

  while(--len > 0) {

    sum += buf[len];

  }

  sum += buf[0];

  return 0xFF - sum;

}

/**

 * Adds buf to the previous checksum total

 *

 * @param buf a byte buffer to add to the checksum

 * @param len the length of the buffer

 * @param sum the previous sum

 *

 * @return error code

 **/

int8_t xbee_checksum_add(uint8_t *buf, uint8_t len, uint8_t* sum) {

  if (buf == NULL || sum == NULL)

    return WL_ERROR_ARGUMENT;

  while(--len > 0) {

    *sum += buf[len];

  }

  *sum += buf[0];

  return WL_SUCCESS;

}

/**

 * Sends header information. Header information includes

 * XBEE_FRAME_START and the packet length, as two bytes.

 *

 * @param type the packet type

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

 *

 **/

int8_t xbee_send_header(uint16_t len)

{  

  //packet prefix

  if (xbee_putc(XBEE_FRAME_START) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc((uint8_t)((len & 0xFF00) >> 8)) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc((uint8_t)(len & 0x00FF)) != WL_SUCCESS)

    return WL_ERROR_SEND;

  return WL_SUCCESS;

}

/**

 * 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 int8_t xbee_send_frame(uint8_t* buf, uint16_t len)

{

  uint8_t checksum = xbee_compute_checksum(buf, len);

  //packet prefix

  if (xbee_putc(XBEE_FRAME_START) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc((uint8_t)(len>>8)) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc((uint8_t)(len&0xFF)) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_send(buf, len) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc(checksum) != WL_SUCCESS)

    return WL_ERROR_SEND;

  return WL_SUCCESS;

}

/**

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

 **/

int8_t xbee_send_read_at_command(uint8_t* command)

{

  return xbee_send_modify_at_command(command, NULL, 0);

}

/**

 * 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 int8_t xbee_send_modify_at_command(uint8_t* command, uint8_t* value, uint8_t len)

{

  uint8_t buf[12];

  buf[0] = XBEE_FRAME_AT_COMMAND;

  buf[1] = 1;

  buf[2] = command[0];

  buf[3] = command[1];

  if (value != NULL)

  {

    if (len > 8)

    {

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

      return WL_ERROR_ARGUMENT;

    }

    memcpy(buf+4,value,len);

  }

  return xbee_send_frame(buf, 4 + len);

}

/**

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

 **/

int8_t xbee_send_packet(uint8_t* packet, uint8_t len, uint16_t dest, uint8_t options, uint8_t frame)

{

  uint8_t sum = XBEE_FRAME_TX_REQUEST_16;

  uint8_t i = 0;

  if (len > 100)

  {

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

    return WL_ERROR_ARGUMENT;

  }

  // calculate checksum

  for(;i<len;i++)

    sum += packet[len];

  sum += frame;

  sum += (dest&0xFF00) >> 8;

  sum += dest&0x00FF;

  sum += options;

  sum = 0xFF - sum;

  //packet prefix

  if (xbee_putc(XBEE_FRAME_START) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc(0x00) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc(len+5) != WL_SUCCESS)

    return WL_ERROR_SEND;

  //send header for TX request

  if (xbee_putc(XBEE_FRAME_TX_REQUEST_16) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc(frame) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc((uint8_t)((dest&0xFF00) >> 8)) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc((uint8_t)(dest&0x00FF)) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc(options) != WL_SUCCESS)

    return WL_ERROR_SEND;

  // send packet

  if (xbee_send(packet, len) != WL_SUCCESS)

    return WL_ERROR_SEND;

  // send checksum

  if (xbee_putc(sum) != WL_SUCCESS)

    return WL_ERROR_SEND;

  return WL_SUCCESS;

}

/**

 * Handles modem status packets.

 *

 * @param status the type of status packet received.

 **/

void xbee_handle_status(uint8_t 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 len the length in bytes of extra

 **/

static int8_t xbee_handle_at_command_response(uint16_t command, uint8_t status, uint8_t len)

{

  WL_DEBUG_PRINT("HANDLE AT COMMAND\r\n");

  if (status != 0)

  {

    WL_DEBUG_PRINT("Error with AT");

    WL_DEBUG_PRINT(command);

    WL_DEBUG_PRINT(" packet. Result = ");

    switch(status) {

    case 1:

      WL_DEBUG_PRINT("ERROR\r\n");

      break;

    case 2:

      WL_DEBUG_PRINT("Invalid Command\r\n");

      break;

    case 3:

      WL_DEBUG_PRINT("Invalid Parameter\r\n");

      break;

    }

    return WL_SUCCESS;

  }

  WL_DEBUG_PRINT("AT");

  WL_DEBUG_PRINT_CHAR((uint8_t)(command>>8));

  WL_DEBUG_PRINT_CHAR((uint8_t)(command));

  WL_DEBUG_PRINT(" command is being handled\r\n");

  // TODO: program more command responses here (ND, etc)

  switch(command) {

  case ('I'<<8)+'D': // PAN

  case ('C'<<8)+'H': // channel

  case ('M'<<8)+'Y': // address

  case ('A'<<8)+'P': // api mode

    // copy command to handler

    xbee_command[0] = (command&0xFF00)>>8;

    xbee_command[1] = command&0x00FF;

    uint8_t ptr = basic_buf_last;

    for(command=2;command<len+2;command++) {

      xbee_command[command] = xbee_basic_buf_get(&ptr);

      if (xbee_command[command] == '\r')

        break; // done with command

      WL_DEBUG_PRINT_HEX(xbee_command[command]);

    }

    xbee_command[command] = '\0';

    WL_DEBUG_PRINT("len=");

    WL_DEBUG_PRINT_INT(len);

    WL_DEBUG_PRINT("ID,CH,or MY command result:");

    WL_DEBUG_PRINT(xbee_command);

    WL_DEBUG_PRINT("\r\n");

    break;

  default:

    WL_DEBUG_PRINT("unknown AT command");

  }

  // signal handler that command response is done

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_RESPONSE);

  return WL_SUCCESS;

}

/**

 * Sets the personal area network id.

 *

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

 **/

int8_t xbee_set_pan(uint16_t id)

{

  if (getStatus(XBEE_COMMAND_MASK) == XBEE_COMMAND_WAIT

    || getStatus(XBEE_COMMAND_MASK) == XBEE_COMMAND_RESPONSE)

    return WL_ERROR_XBEE_COMMAND; // can't do command right now

  int16_t i=0;

  // change status to command wait

  setStatus(XBEE_COMMAND_MASK,XBEE_COMMAND_WAIT);

  xbee_send_modify_at_command((uint8_t*)"ID",(uint8_t*)(&id),2); // send command to set the channel

  // wait for up to 30 ms

  while(getStatus(XBEE_COMMAND_MASK) != XBEE_COMMAND_RESPONSE && i++ < 10000) {

    delay_us(1); // wait 3us

  }

  if (i < 1000 && xbee_command[0] == 'I' && xbee_command[1] == 'D')