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_O