Colony

/**

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

 **/

#include "xbee.h"

#include "wl_defs.h"

#ifndef ROBOT

#include <fcntl.h>

#include <unistd.h>

#include <pthread.h>

#include <errno.h>

#include <termios.h>

#include <stdio.h>

#include <stdlib.h>

#else

#include <serial.h>

#include <avr/interrupt.h>

#include <util/delay.h>

#endif

#include <string.h>

#define XBEE_FRAME_START 0x7E

#define XBEE_GET_PACKET_TIMEOUT 1000

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

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

static int xbee_send_string(char* c);

#ifndef ROBOT

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

#endif

/*Command Mode Functions

 * Called during initialization.

 */

static int xbee_enter_command_mode(void);

static int xbee_exit_command_mode(void);

static int xbee_enter_api_mode(void);

static int xbee_exit_api_mode(void);

static int xbee_wait_for_string(char* s, int len);

static int xbee_wait_for_ok(void);

/*API Mode Functions*/

static int xbee_handle_packet(char* packet, int len);

static void xbee_handle_at_command_response(char* command, char result, char* extra, int extraLen);

static void xbee_handle_status(char status);

static int xbee_verify_checksum(char* packet, int len);

static char xbee_compute_checksum(char* packet, int len);

static int xbee_send_frame(char* buf, int len);

static int xbee_send_read_at_command(char* command);

static int xbee_send_modify_at_command(char* command, char* value);

/*Global Variables*/

#ifndef ROBOT

static char* xbee_com_port = XBEE_PORT_DEFAULT;

static int xbee_stream;

static pthread_t* xbee_listen_thread;

#endif

// TODO: is this a good size?

#define XBEE_BUFFER_SIZE	128

#define PACKET_BUFFER_SIZE	108

// a buffer for data received from the XBee

char arrival_buf[XBEE_BUFFER_SIZE];

// location of last unread byte in buffer

volatile int buffer_last = 0;

// first unread byte in buffer

volatile int buffer_first = 0;

//used to store packets as they are read

static char xbee_buf[PACKET_BUFFER_SIZE];

static int currentBufPos = 0;

//XBee status

static unsigned int xbee_panID = XBEE_PAN_DEFAULT;

static unsigned int xbee_pending_panID = XBEE_PAN_DEFAULT;

static int xbee_channel = XBEE_CHANNEL_DEFAULT;

static int xbee_pending_channel = XBEE_CHANNEL_DEFAULT;

static volatile unsigned int xbee_address = 0;

/*Function Implementations*/

#ifdef ROBOT

/**

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

 * to the buffer.

 **/

#ifndef FIREFLY

ISR(USART1_RX_vect)

{

	char c = UDR1;

	arrival_buf[buffer_last] = c;

	int t = buffer_last + 1;

	if (t == XBEE_BUFFER_SIZE)

		t = 0;

	if (t == buffer_first)

	{

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

	}

	buffer_last = t;

}

#else

SIGNAL(SIG_USART0_RECV)

{

	char c = UDR0;

	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;

}

#endif

#else

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

 **/

int xbee_lib_init()

{

	WL_DEBUG_PRINT("in xbee_init\n");

#ifdef ROBOT

  // enable basic xbee serial communications

  xbee_init();

	// enable the receiving interrupt for USART1

#ifdef FIREFLY

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

#else

#ifdef BAYBOARD

	UCSR1B |= _BV(RXCIE1);

#else

  // dragonfly register 

	UCSR1B |= _BV(RXCIE);

#endif

#endif

	sei();

#else

  // this is computer only code

	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.\n");

	if (xbee_enter_command_mode() != 0) {

		return -1;

	}

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

	if (xbee_enter_api_mode() != 0) {

		return -1;

	}

	WL_DEBUG_PRINT("Entered api mode.\n");

	if (xbee_exit_command_mode() != 0) {

	     return -1;

	}

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

	if (xbee_send_read_at_command("MY")) {

		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

}

/**

 * Call when finished using the XBee library. This releases

 * all sued resources.

 **/

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

}

/**

 * 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 int xbee_send(char* buf, int size)

{

#ifdef ROBOT

	int i;

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

		xbee_putc(buf[i]);

	}

	return 0;

#else

	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

}

/**

 * Sends a string to the XBee.

 *

 * @param c the string to send to the XBEE

 **/

static int xbee_send_string(char* c)

{

	return xbee_send(c, strlen(c));

}

#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

/**

 * Enter into command mode.

 **/

static int xbee_enter_command_mode()

{

	if (xbee_send_string("+++") != 0) {

		return -1;

	}

	if (xbee_wait_for_ok() != 0) {

	  return -1;

	}

	  return 0;

}

/**

 * Exit from command mode.

 **/

static int xbee_exit_command_mode()

{

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

		return -1;

	}

	xbee_wait_for_ok();

	return 0;

}

/**

 * Enter API mode.

 **/

static int xbee_enter_api_mode()

{

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

		return -1;

	}

	xbee_wait_for_ok();

	return 0;

}

/**

 * Exit API mode.

 **/

static int xbee_exit_api_mode()

{

	if (xbee_send_modify_at_command("AP","0") != 0) {

		return -1;

	}

	return 0;

}

/**

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

 **/

static int xbee_wait_for_ok()

{

	return 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

 **/

static int xbee_wait_for_string(char* s, int len)

{

	char* curr = s;

	while (curr - s < len) {

		// check if buffer is empty

		if (buffer_last != buffer_first) {

			char c = arrival_buf[buffer_first++];

			if (buffer_first == XBEE_BUFFER_SIZE) {

				buffer_first = 0;

			}

			if (c == *curr) {

				curr++;

			} else {

#ifndef ROBOT

			  //return -1; // Computer is less forgiving.

			  curr = s;

#else

			  curr = s;

#endif

			}

		} // else buffer is empty.

#ifndef ROBOT

		usleep(100);

#endif

	}

	return 0;

}

/**

 * 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

 *

 **/

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

	if (xbee_send(prefix, 3) != 0) {

		return -1;

	}

	if (xbee_send(buf, len) != 0) {

		return -1;

	}

	if (xbee_send(&checksum, 1) != 0) {

		return -1;

	}

	return 0;

}

/**

 * Resets the XBee Modem (including baud rate when on robot)

 * 

 * Used for launching bootloader

 **/

int xbee_reset(void)

{

#ifdef ROBOT

	xbee_send_modify_at_command("BD", "3");

#endif

	xbee_send_read_at_command("FR");

    // delay 150 ms to wait for reset

#ifndef ROBOT

	usleep(150000);

#else

	_delay_ms(150);

#endif

	return 0;

}

/**

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

 **/

static int xbee_send_read_at_command(char* command)

{

	return 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)

 **/

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

		}

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

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

		}

	}

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

 **/

int 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 -1;

	}

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

	if (xbee_send(prefix, 3) != 0) {

		return -1;

	}

	if (xbee_send(buf, 5) != 0) {

		return -1;

	}

	if (xbee_send(packet, len) != 0) {

		return -1;

	}

	if (xbee_send((char*)&checksum, 1) != 0) {

		return -1;

	}

	return 0;

}

/**

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

{

     int ret;

	//start reading a packet with XBEE_FRAME_START

	if (currentBufPos == 0)

	{

		do

		{

			if (buffer_first == XBEE_BUFFER_SIZE)

				buffer_first = 0;

			// check if buffer is empty

			if (buffer_first == buffer_last) {

				return -1;

			}

		} while (arrival_buf[buffer_first++] != XBEE_FRAME_START);

		if (buffer_first == XBEE_BUFFER_SIZE) {

			buffer_first = 0;

		}

		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 (len > 120)

			{

				WL_DEBUG_PRINT("Packet too large. Probably error in XBee transmission.\n");

				currentBufPos = 0;

				return -1;

			}

		}

		// check if buffer is empty

		if (buffer_first == buffer_last) {

			return -1;

		}

		xbee_buf[currentBufPos++] = arrival_buf[buffer_first++];

		if (buffer_first == XBEE_BUFFER_SIZE) {

			buffer_first = 0;

		}

	}

	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

	ret = xbee_handle_packet(xbee_buf+3, len);

	if (ret == 1) {

		return 3;

	}

	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

 **/

static 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] == 'C' && command[1] == 'H')

	{

		xbee_channel = xbee_pending_channel;

		WL_DEBUG_PRINT("Channel set to ");

		WL_DEBUG_PRINT_INT(xbee_channel);

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

			#ifndef ROBOT

			exit(0);

			#endif

		}

	}

}

/**

 * 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

 */

static 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

 **/

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

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

}

/**

 * Set the channel the XBee is using.

 *

 * @param channel the channel the XBee will not use,

 * between 0x0B and 0x1A

 *

 * @see xbee_get_channel

 **/

int xbee_set_channel(int channel)

{

	if (channel < 0x0B || channel > 0x1A)

	{

		WL_DEBUG_PRINT("Channel out of range.\r\n");

		return -1;

	}

	char s[3];

	s[0] = channel & 0xFF;

	s[1] = 0;

	xbee_pending_channel = channel;

	return xbee_send_modify_at_command("CH", s);

}

/**

 * Returns the channel which the XBee is currently using.

 *

 * @return the channel the XBee is using

 *

 * @see xbee_set_channel

 **/

int xbee_get_channel(void)

{

	return xbee_channel;

}

/**

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

}

#ifndef ROBOT

void xbee_set_com_port(char* port)

{

	xbee_com_port = port;

}

#endif

/**

 * Copyright (c) 2007 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 wl_defs.h

 * @brief Definitions for Wireless

 *

 * Contains definitions for wireless packet groups, packet types,

 * debugging information, etc.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

#ifndef WL_DEFS_H

#define WL_DEFS_H

//comment out this line if using a computer hooked up to an xbee

//#define ROBOT

//uncomment this line for debug information

//#define WL_DEBUG

//return value definitions

/** @brief Error code for init failure **/

#define WL_ERROR_INIT_FAILED 1

/** @brief Error code for duplicate init calls **/

#define WL_ERROR_INIT_ALREADY_INITD 2

/** @brief Error code for not calling init **/

#define WL_ERROR_LIBRARY_NOT_INITD 3

// Packet Groups and Types

// Error group

#define WL_ERROR_GROUP 1

#define WL_ERROR_STRING_TYPE 1

// Token Ring group

#define WL_TOKEN_RING_GROUP 2

#define WL_TOKEN_PASS 1

#define WL_TOKEN_SENSOR_MATRIX 2

#define WL_TOKEN_BOM_ON 3

#define WL_TOKEN_JOIN 4

#define WL_TOKEN_JOIN_ACCEPT 5

// timing constants

#ifndef FIREFLY

#define BOM_DELAY 100

#else

#define BOM_DELAY 200

#endif

#define DEATH_DELAY 4

#define JOIN_DELAY 8

#ifdef WL_DEBUG

#ifdef ROBOT

#include <serial.h>

#endif

#ifdef ROBOT

#define WL_DEBUG_PRINT( s ) usb_puts( s )

#else

#define WL_DEBUG_PRINT( s ) printf( s )

#endif

#ifdef ROBOT

#define WL_DEBUG_PRINT_INT( i ) usb_puti(i)

#else

#define WL_DEBUG_PRINT_INT( i ) printf("%i", i)

#endif

#else

#define WL_DEBUG_PRINT( s )

#define WL_DEBUG_PRINT_INT( i )

#endif

#endif

/**

 * Copyright (c) 2007 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 wl_token_ring.c

 * @brief Token Ring Implementation

 *

 * Implementation of the token ring packet group.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

#include <wl_token_ring.h>

#include <stdlib.h>

#include <wl_defs.h>

#include <wireless.h>

#include <sensor_matrix.h>

#ifdef ROBOT

#ifndef FIREFLY

#include <bom.h>

#endif

#include <time.h>

#endif

//#define DEFAULT_SENSOR_MATRIX_SIZE 20

/*Ring States*/

#define NONMEMBER 0

#define MEMBER 1

#define JOINING 2

#define ACCEPTED 3

#define LEAVING 4

/*Frame Types*/

#define TOKEN_JOIN_ACCEPT_FRAME	1

#define WL_TOKEN_PASS_FRAME	2

/*Function Prototypes*/

/*Wireless Library Prototypes*/

static void wl_token_ring_timeout_handler(void);

static void wl_token_ring_response_handler(int frame, int received);

static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length);

static void wl_token_ring_cleanup(void);

/*Helper Functions*/

static int wl_token_pass_token(void);

static int get_token_distance(int robot1, int robot2);

static void wl_token_get_token(void);

/*Packet Handling Routines*/

static void wl_token_pass_receive(int source);

static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength);

static void wl_token_bom_on_receive(int source);

static void wl_token_join_receive(int source);

static void wl_token_join_accept_receive(int source);

/*Global Variables*/

//the robot we are waiting to say it has received the token. -1 if unspecified

static int wl_token_next_robot = -1;

//true if the robot should be in the token ring, 0 otherwise

static int ringState = NONMEMBER;

//the id of the robot who accepted us into the token ring, only used in ACCEPTED state

static int acceptor = -1;

//id of the robot we are accepting

static int accepted = -1;

//the counter for when we assume a robot is dead

static int deathDelay = -1;

//the counter for joining, before we form our own token ring

static int joinDelay = -1;

//current robot to check in the iterator

static int iteratorCount = 0;

// the amount of time a robot has had its BOM on for

static int bom_on_count = 0;

#ifndef ROBOT

static void do_nothing(void) {}

static int get_nothing(void) {return -1;}

#endif

#ifdef ROBOT

#ifndef FIREFLY

static void (*bom_on_function) (void) = bom_on;

static void (*bom_off_function) (void) = bom_off;

static int (*get_max_bom_function) (void) = get_max_bom;

#else

static void (*bom_on_function) (void) = do_nothing;

static void (*bom_off_function) (void) = do_nothing;

static int (*get_max_bom_function) (void) = get_nothing;

#endif

#else

static void (*bom_on_function) (void) = do_nothing;

static void (*bom_off_function) (void) = do_nothing;

static int (*get_max_bom_function) (void) = get_nothing;

#endif

static PacketGroupHandler wl_token_ring_handler =

	{WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,

		wl_token_ring_response_handler, wl_token_ring_receive_handler,

		wl_token_ring_cleanup};

/**

 * Causes the robot to join an existing token ring, or create one

 * if no token ring exists. The token ring uses global and robot to robot

 * packets, and does not rely on any PAN.

 **/

int wl_token_ring_join()

{

	WL_DEBUG_PRINT("Joining the token ring.\r\n");

	ringState = JOINING;

	joinDelay = DEATH_DELAY * 2;

	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) {

		return -1;

	}

	return 0;

}

/**

 * Causes the robot to leave the token ring. The robot stops

 * alerting others of its location, but continues storing the

 * locations of other robots.

 **/

void wl_token_ring_leave()

{

	ringState = LEAVING;

}

/**

 * Initialize the token ring packet group and register it with the

 * wireless library. The robot will not join a token ring.

 **/

int wl_token_ring_register()

{

	if (wl_get_xbee_id() > 0xFF)

	{

		//Note: if this becomes an issue (unlikely), we could limit sensor information

		//to half a byte and use 12 bits for the id

		WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");

		WL_DEBUG_PRINT_INT(wl_get_xbee_id());

		WL_DEBUG_PRINT(".\r\n");

		return -1;

	}

	sensor_matrix_create();

	//add ourselves to the sensor matrix

	sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);

	wl_register_packet_group(&wl_token_ring_handler);

	return 0;

}

/**

 * Removes the packet group from the wireless library.

 **/

void wl_token_ring_unregister()

{

	wl_unregister_packet_group(&wl_token_ring_handler);

}

/**

 * Sets the functions that are called when the BOM ought to be

 * turned on or off. This could be used for things such as

 * charging stations, which have multiple BOMs.

 *

 * @param on_function the function to be called when the BOM

 * should be turned on

 * @param off_function the function to be called when the BOM

 * should be turned off

 * @param max_bom_function the function to be called when a

 * measurement of the maximum BOM reading is needed.

 **/

void wl_token_ring_set_bom_functions(void (*on_function) (void),

	void (*off_function) (void), int (*max_bom_function) (void))

{

	bom_on_function = on_function;

	bom_off_function = off_function;

	get_max_bom_function = max_bom_function;

}

/**

 * Called to cleanup the token ring packet group.

 **/

static void wl_token_ring_cleanup()

{

}

/**

 * Called approximately every quarter second by the wireless library.

 **/

static void wl_token_ring_timeout_handler()

{

	//someone is not responding, assume they are dead

	if (deathDelay == 0)

	{

		//pass the token to the next robot if we think someone has died

		//also, declare that person dead, as long as it isn't us

		if (wl_token_next_robot != wl_get_xbee_id())

		{

			sensor_matrix_set_in_ring(wl_token_next_robot, 0);

			WL_DEBUG_PRINT("Robot ");

			WL_DEBUG_PRINT_INT(wl_token_next_robot);

			WL_DEBUG_PRINT(" has died.\r\n");

			wl_token_next_robot = -1;

			deathDelay = DEATH_DELAY;

		}

		// we may have been dropped from the ring when this is received

		if (ringState == MEMBER) {

			wl_token_pass_token();

		}

	}

	//we must start our own token ring, no one is responding to us

	if (joinDelay == 0)

	{

		if (sensor_matrix_get_joined() == 0)

		{

			WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");

			sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);

			ringState = MEMBER;

			//this will make us pass the token to ourself

			//repeatedly, and other robots when they join

			deathDelay = DEATH_DELAY;

			wl_token_next_robot = wl_get_xbee_id();

		}

		else

		{

			WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");

			//attempt to rejoin with a random delay

      //TODO: should we use the constant JOIN_DELAY ?

			wl_token_ring_join();

			joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;

		}

	}

	if (deathDelay >= 0) {

		deathDelay--;

	}

	if (joinDelay >= 0) {

		joinDelay--;

	}

	if (bom_on_count >= 0) {

		bom_on_count++;

	}

}

/**

 * Called when the XBee tells us if a packet we sent has been received.

 *

 * @param frame the frame number assigned when the packet was sent

 * @param received 1 if the packet was received, 0 otherwise

 **/

static void wl_token_ring_response_handler(int frame, int received)

{

	if (!received)

	{

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

	}

}

/**

 * Called when we recieve a token ring packet.

 * @param type the type of the packet

 * @param source the id of the robot who sent the packet

 * @param packet the data in the packet

 * @param length the length of the packet in bytes

 **/

static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length)

{

	switch (type)

	{

		case WL_TOKEN_PASS:

			wl_token_pass_receive(source);

			break;

		case WL_TOKEN_SENSOR_MATRIX:

			wl_token_sensor_matrix_receive(source, packet, length);

			break;

		case WL_TOKEN_BOM_ON:

			//add the robot to the sensor matrix if it is not already there

			wl_token_bom_on_receive(source);

			break;

		case WL_TOKEN_JOIN:

			wl_token_join_receive(source);

			break;

		case WL_TOKEN_JOIN_ACCEPT:

			wl_token_join_accept_receive(source);

			break;

		default:

			WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");

			break;

	}

}

/**