Colony

// this is all the xbee.c computer code

#include <stdio.h>

#include <stdlib.h>

#ifndef ROBOT

#include <fcntl.h>

#include <unistd.h>

#include <pthread.h>

#include <errno.h>

#include <termios.h>

#endif

#ifdef USE_STDIO

#include <stdio.h>

/**

 * For use with fprintf() and related stdio functions

 **/

FILE *xbee_fd;

#endif

uint8_t xbee_initd=0;

/**

 * Initializes communication over the XBee.

 * This must be called before any other xbee function

 * may be used.

 **/

int xbee_init() {

  if(xbee_initd) {

    return ERROR_INIT_ALREADY_INITD;

  }

  //Set baud rate

  // - 115200 (both wired and wireless) is UBRR=8, U2X=1

  // - 9600 is U2X =1, UBRR = 107.

#if (XBEE_BAUD == 115200)

  UBRR1H = 0x00;

  UBRR1L = 8;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 9600)

  UBRR1H = 0x00;

  UBRR1L = 103;

  UCSR1A |= _BV(U2X1);

#else //Baud rate is defined in the header file, we should not get here

  return 0;

#endif

  //Enable receiver and transmitter

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

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

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

  // if we have enabled the stdio stuff, then we init it here

#ifdef USE_STDIO

  /* Open the stdio stream corresponding to this port */

  xbee_fd = fdevopen(xbee_putc, xbee_getc);

#endif

  xbee_initd = 1;

  return 0;

}

/**@brief The port to use the XBee from on the computer. **/

#ifndef ROBOT

#define XBEE_PORT_DEFAULT "/dev/ttyUSB1"

#endif

/**@brief Set the com port on a computer, undefined on the robot**/

void xbee_set_com_port(char* port);

#define XBEE_GET_PACKET_TIMEOUT 1000

#ifndef ROBOT

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

#endif

#ifndef ROBOT

static char* xbee_com_port = XBEE_PORT_DEFAULT;

static int xbee_stream;

static pthread_t* xbee_listen_thread;

#endif

#ifndef ROBOT

// Computer code

/**

 * Thread that listens to the xbee.

 **/

static void* listen_to_xbee(void* x)

{

        char c;

        while (1)

        {

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

                        WL_DEBUG_PRINT("xbee_read failed.\n");

                        return NULL;

                }

                arrival_buf[buffer_last] = c;

                int t = buffer_last + 1;

                if (t == XBEE_BUFFER_SIZE)

                        t = 0;

                if (t == buffer_first)

                {

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

                }

                buffer_last = t;

                usleep(1000);

        }

        return NULL;

}

#endif

/**

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

 **/

int8_t xbee_lib_init()

{

  // do serial.c xbee init     TODO: merge this into xbee.c

  if (xbee_init() != 0) {

    usb_puts("xbee_init error");

    return WL_ERROR_INIT_FAILED;

  }

        WL_DEBUG_PRINT("in xbee_init\n");

#ifdef ROBOT

        //enable the receiving interrupt

#ifdef FIREFLY

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

#else

#ifdef BAYBOARD

        UCSR1B |= _BV(RXCIE1);

#else

        UCSR1B |= _BV(RXCIE);

#endif

#endif

        sei();

#else

        xbee_stream = open(xbee_com_port, O_RDWR);

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

        {

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

                WL_DEBUG_PRINT_INT(xbee_com_port);

                WL_DEBUG_PRINT(".\n");

                return -1;

        } else {

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

                WL_DEBUG_PRINT_INT(xbee_com_port);

                WL_DEBUG_PRINT(".\n");

        }

        // set baud rate, etc. correctly

        struct termios options;

        tcgetattr(xbee_stream, &options);

        cfsetispeed(&options, B9600);

        cfsetospeed(&options, B9600);

        options.c_iflag &= ~ICRNL;

        options.c_oflag &= ~OCRNL;

        options.c_cflag |= (CLOCAL | CREAD);

        options.c_cflag &= ~PARENB;

        options.c_cflag &= ~CSTOPB;

        options.c_cflag &= ~CSIZE;

        options.c_cflag |= CS8;

        options.c_lflag &= ~ICANON;

        options.c_cc[VMIN] = 1;

        options.c_cc[VTIME] = 50;

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

        {

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

                return -1;

        }

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

        if (xbee_listen_thread == NULL)

        {

                WL_DEBUG_PRINT("Malloc failed.\n");

                return -1;

        }

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

        if (ret)

        {

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

                return -1;

        }

#endif

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

        if (xbee_enter_command_mode() != 0) {

    usb_puts("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 == 250000)

#if (XBEE_BAUD == 115200)

  xbee_send_string("ATBD7\r\n");

#elif (XBEE_BAUD == 57600)

  xbee_send_string("ATBD6\r\n");

#elif (XBEE_BAUD == 38400)

  xbee_send_string("ATBD5\r\n");

#elif (XBEE_BAUD == 19200)

  xbee_send_string("ATBD4\r\n");

#elif (XBEE_BAUD == 9600)

  // already at this baud rate

  //xbee_send_string("ATBD3\r\n");

#else

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

#endif  

  // exit command mode

  xbee_wait_for_ok();

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

  xbee_send_string("ATCN\r\n");

  xbee_wait_for_ok();

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

  // set UART baud

#if (XBEE_BAUD == 250000)

  UBRR1H = 0x00;

  UBRR1L = 3;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 115200)

  UBRR1H = 0x00;

  UBRR1L = 8;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 57600)

  UBRR1H = 0x00;

  UBRR1L = 16;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 38400)

  UBRR1H = 0x00;

  UBRR1L = 25;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 19200)

  UBRR1H = 0x00;

  UBRR1L = 51;

  UCSR1A |= _BV(U2X1);

#elif (XBEE_BAUD == 9600)

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

  UBRR1H = 0x00;

  UBRR1L = 103;

  UCSR1A |= _BV(U2X1);*/

#else //Baud rate is defined in the header file, we should not get here

  return 0;

#endif

  delay_ms(50);

  // enter command mode

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

  xbee_send_string("+++");

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

  // get baud rate

        if (xbee_send_read_at_command("BD")) {

    WL_DEBUG_PRINT("can't send BD command\r\n");

                return -1;

        }

        WL_DEBUG_PRINT("Getting ATBD rate.\n");

  // get MY address

        if (xbee_send_read_at_command("MY")) {

    WL_DEBUG_PRINT("can't send my command\r\n");

                return -1;

        }

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

#ifndef ROBOT

        int i;

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

          ret = xbee_get_packet(NULL);

          usleep(1000);

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

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

/*             return -1; */

/*           } */

        }

#else

        //wait to return until the address is set

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

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

  // robot hang forever

        while (xbee_address == 0) {

          xbee_get_packet(NULL);

        }

#endif

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

#ifndef ROBOT

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

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

          return -1;

        } else {

          return 0;

        }

#else

        return 0;

#endif

}

void xbee_terminate()

{

        #ifndef ROBOT

        pthread_cancel(*xbee_listen_thread);

    pthread_join(*xbee_listen_thread, NULL);

        free(xbee_listen_thread);

        lockf(xbee_stream, F_ULOCK, 0);

        close(xbee_stream);

  #else

  xbee_exit_api_mode();

        #endif

}

static int xbee_send(char* buf, int size)

#ifndef ROBOT

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

        //success

        if (ret == size)

                return 0;

        if (ret == -1)

        {

                //interrupted by system signal, probably timer interrupt.

                //just try again

                if (errno == 4)

                {

                        return xbee_send(buf, size);

                }

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

                return -1;

        }

        //write was interrupted after writing ret bytes

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

#endif

#ifndef ROBOT

static int xbee_read(char* buf, int size)

{

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

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

                return -1;

        }

        return 0;

}

#endif

#ifndef ROBOT

void xbee_set_com_port(char* port)

{

        xbee_com_port = port;

}

#endif