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 <string.h>

#include <avr/interrupt.h>

#include <time.h>

#include "wl_defs.h"

#include "wireless.h"

#include "xbee.h"

/* Internal Function Prototypes */

// TODO: convert all int references to int16_t syntax (see stdint.h)

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

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 result, uint8_t len);

static int8_t xbee_handle_at_command_response(uint16_t command, uint8_t result, 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);

/*Global Variables*/

// array for basic packets

static uint8_t xbee_basic_buf[PACKET_BUFFER_SIZE];

// beginning of first packet in basic buffer

static uint8_t basic_buf_first = 0;

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

// aka, the first free byte in basic buffer

static uint8_t basic_buf_last = 0;

// array for other packets

static uint8_t xbee_other_buf[PACKET_BUFFER_SIZE];

// beginning of first packet in other buffer

static uint8_t other_buf_first = 0;

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

// aka, the first free byte in other buffer

static uint8_t other_buf_last = 0;

// xbee status

#define XBEE_API_OFF 0x0

#define XBEE_API_ON 0x1

#define XBEE_API_ESCAPE 0x2

#define XBEE_NORMAL 0x00

#define XBEE_COMMAND_WAIT 0x80

#define XBEE_COMMAND_RESPONSE 0xC0

#define XBEE_INITD 0x08

static uint8_t xbee_status = XBEE_API_OFF|XBEE_NORMAL;

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

static uint8_t xbee_command[4];

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

ISR(USART1_RX_vect)

#else

#define PORT UDR0

#define FLAG 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;

  // check that we're in API mode

  if ((xbee_status&0x03) == XBEE_API_OFF || apitype != XBEE_FRAME_START) {

    // not in API mode

    if ((xbee_status&0xC0) == XBEE_COMMAND_WAIT) {

      // get rest of command and put in basic buf

      xbee_basic_buf[0] = 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;

          }

        }

      }

      // signal handler that command response is done

      xbee_status = (xbee_status&0x3F)|XBEE_COMMAND_RESPONSE;

    }

    return;

  }

  // get length and type

  while(i<3) {

    if (FLAG) {

      if (i==0)

        len |= PORT<<8;

      else if (i==1)

        len |= PORT;

      else if (i==2)

        apitype = PORT;

      i++;

    }

  }

  // do something based on the type

  i=1;

  switch(apitype) {

  case XBEE_FRAME_AT_COMMAND_RESPONSE: {

    // AT command response

    if ((xbee_status&0xC0) == XBEE_COMMAND_RESPONSE)

      return; // we're currently processing a command, so drop the incoming one

    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

    xbee_handle_at_command_response(atcommand,apitype,i-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) != 0) {  // TODO: write function

            // 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; }

  } // end of switch statement

  while (1) {

    if (FLAG) {

      apitype = PORT; // get checksum, and ignore

      break;

    }

  }

} // 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;

}

/**

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

 **/

int8_t xbee_init()

{

        WL_DEBUG_PRINT("in xbee_init\n");

  if((xbee_status&0x08) == XBEE_INITD) {

    return WL_ERROR_INIT_ALREADY_INITD;

  }

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

  UCSR0A |= (1<<U2X0);

  UBRR0H = 0x00;

  UBRR0L = 103;

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

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

#else

  // Bayboard or robot

  UCSR1A |= (1<<U2X1);

  UBRR1H = 0x00;

  UBRR1L = 103;

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

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

#endif

        sei();

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

        if (xbee_enter_command_mode() != 0) {

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

                return -1;

        }

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

  // reset baud rate

  WL_DEBUG_PRINT("Resetting Baud to ");

  WL_DEBUG_PRINT(XBEE_BAUD_STR);

  WL_DEBUG_PRINT("\r\n");

  // set baud on xbee

#if (XBEE_BAUD == 115200)

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

  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

  delay_ms(50);

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

        }

        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;

        }

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

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

  // set status

  xbee_status |= XBEE_INITD;

        return WL_SUCCESS;

}

/**

 * Call when finished using the XBee library.

 **/

int8_t xbee_terminate()

{

  if (xbee_exit_api_mode() != WL_SUCCESS) {

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

    return WL_ERROR_TERMINATION_FAILED;

  }

  xbee_status = xbee_status&0xF7; // 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) {

  if(!(xbee_status&XBEE_INITD))

    return WL_ERROR_LIBRARY_NOT_INITD;

  // Wait until buffer is clear for sending

  // Then load buffer with your character

#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) {

  if(!(xbee_status&XBEE_INITD))

    return WL_ERROR_LIBRARY_NOT_INITD;

  // 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) {

  if(!(xbee_status&XBEE_INITD))

    return WL_ERROR_LIBRARY_NOT_INITD;

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

 **/

static 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;

        }

  if (xbee_putc(buf[0]) != 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;

        }

        if (xbee_wait_for_ok() != WL_SUCCESS) {

          return WL_ERROR_XBEE_COMMAND;

        }

        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;

        }

        xbee_wait_for_ok();

        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;

        }

        xbee_wait_for_ok();

  xbee_status = (xbee_status&0xFC)|XBEE_API_ON;

        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;

        }

        xbee_wait_for_ok();

  xbee_status = (xbee_status&0xFC)|XBEE_API_ESCAPE;

        return WL_SUCCESS;

}

/**

 * Exit API mode.

 **/

static int8_t xbee_exit_api_mode()

{

        if ((xbee_status&0xC0) == XBEE_COMMAND_WAIT

    || (xbee_status&0xC0) == XBEE_COMMAND_RESPONSE)

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

  int16_t i=0;

  // change status to command wait

  xbee_status = (xbee_status&0x3F)|XBEE_COMMAND_WAIT;

  xbee_send_modify_at_command((uint8_t*)"AP",(uint8_t*)"0",1); // send command

  // wait for up to 30 ms

  while((xbee_status&0xC0) != XBEE_COMMAND_RESPONSE && i++ < 10000) {

    delay_us(1); // wait 3us

  }

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

    i = WL_SUCCESS;

  else

    i = WL_ERROR_XBEE_COMMAND; // set error code

  xbee_status = (xbee_status&0x3C)|XBEE_API_OFF; // reset status

        return (int8_t)i; // return

}

/**

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

 **/

static int8_t xbee_wait_for_ok()

{

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

}

/**

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

  if ((xbee_status&0x03) == XBEE_API_OFF) {

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

    while((xbee_status&0xC0) != 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;

    }

    // clear response

    xbee_status = xbee_status&0x3F;

  }

        return 0;

}

/**

 * Delay until we receive a command response.

 * (either OK\r or some actual value)

 *

 * 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 ((xbee_status&0x03) == XBEE_API_OFF) {

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

    while((xbee_status&0xC0) != 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);

        xbee_status = xbee_status&0x3F; // 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

 **/

static 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];

}

/**

 * 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

 *

 **/

static int8_t xbee_send_header(uint8_t type, 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 & 0xFF00) >> 8)) != WL_SUCCESS)

    return WL_ERROR_SEND;

  if (xbee_putc((uint8_t)(len & 0x00FF)) != 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++)