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 wireless_send.c

 * @brief Wireless library send functions

 *

 * Implementation of high level wireless communication.

 * This is the send functions portion of the library.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#include "wl_defs.h"

#include "wireless.h"

#include "xbee.h"

#include <string.h>

/**

 * Definition for wireless library send packet structure

 * byte 1: length of packet (from frame number to end of data)

 * byte 2: frame number

 * bytes 3-n: data (dest, options, frame number, group code, packet)

 * byte n+1: num retries

 *

 * Definition for ack buffer

 * 2 bit system: 0=still sending

 *               1=OK

 *               2=ack failure

 *               3=CCA failure

 **/

/* global variables */

uint8_t nextframe = 1; // number of next frame

uint8_t ack_buf[64]; // ack buffer (holds 255 packet return codes)

uint8_t send_buf[PACKET_BUFFER_SIZE]; // sending buffer for retries

uint8_t send_buf_first = 0; // first byte of data on buffer

uint8_t send_buf_last = 0; // next free byte on buffer

uint8_t send_buf_num_packets = 0; // number of packets in sending buffer

/* private function prototypes */

void setack(uint8_t num,uint8_t val);

void ackhandle(uint8_t num,uint8_t val);

int8_t send_buf_add(uint8_t *ptr, uint8_t byte);

uint8_t send_buf_get(uint8_t *ptr);

// the send functions

/**

 * @addtogroup wireless Wireless

 * @{

 **/

/**

 * The core send function. This will take all possible arguments and send all types of packets.

 * 

 * @param data pointer to the byte array of data to be included in the packet

 * @param length the length of the data array

 * @param group the packet group of the packet

 * @param scope flag for sending global packet or your current PAN

 * @param dest robot ID (for robot to robot packet)

 * @param mode flag for using FAST or RELIABLE sending

 *

 * @return positive packet number for tracking acks, or error code

 **/

int16_t wl_send(uint8_t *data, uint8_t length, uint8_t group, uint8_t scope, uint16_t dest, uint8_t mode) {

  uint8_t packet[6];

  int16_t ret = WL_SUCCESS;

  // build packet header

  packet[0] = nextframe;

  packet[1] = (dest&0xFF00)>>8;

  packet[2] = dest&0x00FF;

  packet[3] = XBEE_OPTIONS_NONE;

  packet[4] = nextframe;

  packet[5] = group;

  // set scope

  if (scope == GLOBAL)

    packet[3] &= XBEE_OPTIONS_BROADCAST_ALL_PANS;

  else if (scope != PAN) {

    WL_DEBUG_PRINT_P("Error - bad scope in core send function\r\n");

    return WL_ERROR_SCOPE;

  }

  // set mode

  if (mode == FAST) {

    packet[3] &= XBEE_OPTIONS_DISABLE_RESPONSE;

  } else if (mode != RELIABLE) {

    WL_DEBUG_PRINT_P("Error - bad mode in core send function\r\n");

    return WL_ERROR_MODE;

  }

  // do checksum

  group = XBEE_FRAME_TX_REQUEST_16;

  xbee_checksum_add(packet,6,&group);

  xbee_checksum_add(data,length,&group); 

  group = 0xFF-group;

  // send the packet

  if (xbee_send_header(length+7) != WL_SUCCESS) {

    WL_DEBUG_PRINT_P("1Error sending packet from core send function\r\n");

    return WL_ERROR_SEND;

  }

  if (xbee_sendc(XBEE_FRAME_TX_REQUEST_16) != WL_SUCCESS) {

    WL_DEBUG_PRINT_P("2Error sending packet from core send function\r\n");

    return WL_ERROR_SEND;

  }

  if (xbee_send(packet,6) != WL_SUCCESS) {

    WL_DEBUG_PRINT_P("3Error sending packet from core send function\r\n");

    return WL_ERROR_SEND;

  }

  if (xbee_send(data,length) != WL_SUCCESS) {

    WL_DEBUG_PRINT_P("4Error sending packet from core send function\r\n");

    return WL_ERROR_SEND;

  }

  if (xbee_sendc(group) != WL_SUCCESS) {

    WL_DEBUG_PRINT_P("5Error sending packet from core send function\r\n");

    return WL_ERROR_SEND;

  }

  // save in ack system

  if (mode == FAST) {

    setack(nextframe,ACK_OK); // assume the send was successful

    nextframe = (nextframe == 0xFF)?1:nextframe+1; // increment frame number

    return ret; // no frame number

  } else if (mode == RELIABLE) {

    setack(nextframe,SENDING); // set status to SENDING

    // save packet on sending buffer

    scope = send_buf_last; // use as ptr to send buffer    

    if (send_buf_add(&scope,length+6) != WL_SUCCESS) { // add length

      WL_DEBUG_PRINT_P("Error adding length to sending buffer\r\n");

      ret = WL_ERROR_SENDING_BUFFER_FULL;

    }    

    for(mode=0;mode<6;mode++) { // add header

      if (send_buf_add(&scope,packet[mode]) != WL_SUCCESS) {

        WL_DEBUG_PRINT_P("Error adding header to sending buffer\r\n");

        ret = WL_ERROR_SENDING_BUFFER_FULL;

      }

    }

    for(mode=0;mode<length;mode++) { // add data

      if (send_buf_add(&scope,data[mode]) != WL_SUCCESS) {

        WL_DEBUG_PRINT_P("Error adding data to sendig buffer\r\n");

        ret = WL_ERROR_SENDING_BUFFER_FULL;

      }

    }

    if (send_buf_add(&scope,0) != WL_SUCCESS) { // add num retries=0

      WL_DEBUG_PRINT_P("Error adding num_retries to sending buffer\r\n");

      ret = WL_ERROR_SENDING_BUFFER_FULL;

    }

    send_buf_last = scope;

    send_buf_num_packets++;

    WL_DEBUG_PRINT_P("send_buf_last:");

    WL_DEBUG_PRINT_INT(send_buf_last);

    WL_DEBUG_PRINT_P("pos:");

    WL_DEBUG_PRINT_INT(scope);

    WL_DEBUG_PRINT_P("send_buf_num_packets:");

    WL_DEBUG_PRINT_INT(send_buf_num_packets);

    nextframe = (nextframe == 0xFF)?1:nextframe+1; // increment frame number

    return (ret == WL_SUCCESS)?packet[0]:ret; // return frame number for ack tracking

  }

  return WL_ERROR_SEND; // shouldn't get here, but if it does there's a problem

}

/**

 * Wrapper for core send function that will send a global packet across the current channel.

 *

 * @param data pointer to the byte array of data to be included in the packet

 * @param length the length of the data array

 * @param group the packet group of the packet

 *

 * @return positive packet number for tracking acks, or error code

 **/

int16_t wl_send_global(uint8_t *data, uint8_t length, uint8_t group) {

  return wl_send(data, length, group, GLOBAL, BROADCAST, RELIABLE);

}

/**

 * Wrapper for core send function that will send a packet across the current channel on the current pan.

 *

 * @param data pointer to the byte array of data to be included in the packet

 * @param length the length of the data array

 * @param group the packet group of the packet

 *

 * @return positive packet number for tracking acks, or error code (TBD)

 **/

int16_t wl_send_pan(uint8_t *data, uint8_t length, uint8_t group) {

  return wl_send(data, length, group, PAN, BROADCAST, RELIABLE);

}

/**

 * Wrapper for core send function that will send a packet across the current channel to a specific robot. 

 *

 * @param data pointer to the byte array of data to be included in the packet

 * @param length the length of the data array

 * @param group the packet group of the packet 

 * @param dest robot ID (for robot to robot packet)

 * @param mode flag for using FAST or RELIABLE sending

 *

 * @return positive packet number for tracking acks, or error code (TBD)

 **/

int16_t wl_send_robot(uint8_t *data, uint8_t length, uint8_t group, uint16_t dest, uint8_t mode) {

  return wl_send(data, length, group, GLOBAL, dest, mode);

}

/**

 * Default (i.e. basic) send wrapper.

 *

 * @param data pointer to the byte array of data to be included in the packet

 * @param length the length of the data array

 *

 * @return positive packet number for tracking acks, or error code (TBD)

 **/

int16_t wl_send_basic(uint8_t *data, uint8_t length) {

/** Check if it needs to adjust according to data type. **/

  return wl_send(data, length, BASIC, GLOBAL, BROADCAST, RELIABLE);

}

/**

 * acknowledgment error

 * check if any of the packets you have sent have been lost.

 *

 * note that all other error checking will be handled by library, 

 * so your user behavior won't have to worry about it

 *

 * @return the # of packets lost (up to 255)

 **/

uint8_t wl_ack_error(void) {

  WL_DEBUG_PRINT_P("entering wl_ack_error function");

  uint8_t val=0,i=1;

  while(1) {

    if (((ack_buf[i/4])&(0x2<<(i%4))) != 0) // if the ack is in an error state

      val++;

    if (i==255)

      break;

    i++;

  }  

  WL_DEBUG_PRINT_P("|num_errors:");

  WL_DEBUG_PRINT_INT(val);

  WL_DEBUG_PRINT_P("\r\n");

  return val;

}

/**

 * acknowledgement error check

 * check if a specific packet has been lost

 * note: buffer will overflow every 255 packets

 * 

 * @param packet packet number

 *

 * @return {SENDING,ACK_OK,ACK_FAILURE,CCA_FAILURE}

 **/

int8_t wl_ack_check(uint8_t packet) {

  WL_DEBUG_PRINT_P("entering wl_ack_check function|ack=");

  if (packet == 0) {

    // no ack information here

    WL_DEBUG_PRINT_P("packet number cannot be 0\r\n");

    return WL_ERROR_ARGUMENT;

  }

  WL_DEBUG_PRINT_HEX(ack_buf[packet/4]&(0x3<<(packet%4)));

  WL_DEBUG_PRINT_P("\r\n");

  // check ack

  return ack_buf[packet/4]&(0x3<<(packet%4));

}

/**

 * acknowledgement reset

 * reset the acknowledgement buffer

 **/

void wl_ack_reset(void) {

  WL_DEBUG_PRINT_P("entering wl_ack_reset function\r\n");

  memset(ack_buf,0,64);

}

/**@} **/ //end defgroup

// Define all private functions down here

/*

 * sets 2 bits in the ack_buf

 *

 * @param num the ack number

 * @param val {SENDING,ACK_OK,ACK_FAILURE,CCA_FAILURE}

 */

void setack(uint8_t num,uint8_t val) {

  WL_DEBUG_PRINT_P("entering setack function|num=");

  WL_DEBUG_PRINT_INT(num);

  WL_DEBUG_PRINT_P("|val=");

  WL_DEBUG_PRINT_HEX(val);

  WL_DEBUG_PRINT_P("\r\n");

  switch(num%4) {

  case 0:

    ack_buf[num/4] &= (0xFC|val);

    break;

  case 1:

    ack_buf[num/4] &= (0xF3|(val<<2));

    break;

  case 2:

    ack_buf[num/4] &= (0xCF|(val<<4));

    break;

  case 3:

    ack_buf[num/4] &= (0x3F|(val<<6));

    break;

  }

}

/* ack handler */

void ackhandle(uint8_t num,uint8_t val) {

  uint8_t len;

  uint8_t packets = 0;

  uint8_t num_packet = 0;

  WL_DEBUG_PRINT_P("entering ackhandle function: num:");

  WL_DEBUG_PRINT_HEX(num);

  WL_DEBUG_PRINT_P("\r\n");

  switch(val) {

  case 0:

    WL_DEBUG_PRINT_P("ack success\r\n");

    // success

    setack(num,ACK_OK); // set status code

    // remove from buffer

    val = send_buf_first;

    if (send_buf_num_packets == 0)

      return;

    WL_DEBUG_PRINT_P("|num_send_packets:");

    WL_DEBUG_PRINT_INT(send_buf_num_packets);

    while (1) {

      len = send_buf_get(&val);

      WL_DEBUG_PRINT_P("|send_packet_buf_pos:");

      WL_DEBUG_PRINT_INT(val);

      WL_DEBUG_PRINT_P("|send_packet_len:");

      WL_DEBUG_PRINT_INT(len);

      WL_DEBUG_PRINT_P("|send_packet_first:");

      WL_DEBUG_PRINT_INT(send_buf_first);

      WL_DEBUG_PRINT_P("|send_packet_last:");

      WL_DEBUG_PRINT_INT(send_buf_last);

      WL_DEBUG_PRINT_P("|send_buf:");

      for(int i=send_buf_first;i<send_buf_last;i++)

        WL_DEBUG_PRINT_HEX(send_buf[i]);

      num_packet = send_buf_get(&val);

      if (num_packet != num) {

        // not the correct packet

        if ((num_packet < num && num > 9 && num_packet < num-10)

            || (num_packet > num && num > 9)) {

          ackhandle(num_packet,1); // resend

          ackhandle(num,0); // redo this packet

          return;

        }

        WL_DEBUG_PRINT_P("|not correct packet");

        val += len;

        WL_DEBUG_PRINT_P("|send_packet_buf_pos:");

        WL_DEBUG_PRINT_INT(val);

        if (val >= PACKET_BUFFER_SIZE)

          val -= PACKET_BUFFER_SIZE;

        if (val == send_buf_last || ++packets >= send_buf_num_packets) {

          // reached end of bufer, so assume not present

          return;

        }

      } else {

        // remove packet

        WL_DEBUG_PRINT_P("|remove packet");

        num = val+len;

        val = (val<2)?PACKET_BUFFER_SIZE+val-2:val-2;

        if (num >= PACKET_BUFFER_SIZE)

          num -= PACKET_BUFFER_SIZE;

        while(num != send_buf_last) {

          if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)

            return; // error

        }

        send_buf_last = val; // set new end of buffer

        send_buf_num_packets--; // remove packet from count

        WL_DEBUG_PRINT_P("|send_packet_first:");

        WL_DEBUG_PRINT_INT(send_buf_first);

        WL_DEBUG_PRINT_P("|send_packet_last:");

        WL_DEBUG_PRINT_INT(send_buf_last);

        WL_DEBUG_PRINT_P("|num_send_packets:");

        WL_DEBUG_PRINT_INT(send_buf_num_packets);

        return;

      }

    }

    break;

  case 1:

    WL_DEBUG_PRINT_P("no ack\r\n");

    // no ack

    // check resend attempts

    val = send_buf_first;

    if (send_buf_num_packets == 0)

      return;

    while (1) {

      len = send_buf_get(&val);

      if (send_buf_get(&val) != num) {

        // not the correct packet, so continue

        val += len;

        if (val >= PACKET_BUFFER_SIZE)

          val -= PACKET_BUFFER_SIZE;

        if (val == send_buf_last) {

          // reached end of bufer, so assume not present

          setack(num,ACK_FAILURE); // mark as failed send

          return;

        }

      } else {

        // check attempts on packet

        num = val+len-1; // set to end of packet

        if (num >= PACKET_BUFFER_SIZE)

          num -= PACKET_BUFFER_SIZE;

        if (send_buf[num] < NUM_RETRIES) {

          val = (val<2)?PACKET_BUFFER_SIZE+val-2:val-2; // set to start of this packet

          // retry sending

          send_buf[num]++; // increment retries

          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // go back one byte

          if (xbee_send_header(len+1) != WL_SUCCESS) {

            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");

            return;

          }

          len = XBEE_FRAME_TX_REQUEST_16;

          while(val!=num) {

            // compute checksum

            len += send_buf[val];

            if (xbee_putc(send_buf_get(&val)) != WL_SUCCESS) { // send  byte

              WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");

              return;

            }

          }

          len = 0xFF - len;

          if (xbee_putc(len) != WL_SUCCESS) { // send

            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");

            return;

          }

          return;

        } else {

          // done sending, mark as failed send

          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to frame num

          setack(send_buf[val],ACK_FAILURE);

          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to start of this packet

          // remove packet

          while(num != send_buf_last) {

            if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)

              return; // error

          }

          return;

        }

      }

    }    

    break; // shouldn't get here, but just in case

  case 2:

    WL_DEBUG_PRINT_P("CCA failure\r\n");

    // CCA failure

    // check resend attempts

    val = send_buf_first;

    if (send_buf_num_packets == 0)

      return;

    while (1) {

      len = send_buf_get(&val);

      if (send_buf_get(&val) != num) {

        // not the correct packet, so continue

        val += len;

        if (val >= PACKET_BUFFER_SIZE)

          val -= PACKET_BUFFER_SIZE;

        if (val == send_buf_last) {

          // reached end of bufer, so assume not present

          setack(num,CCA_FAILURE); // mark as failed send

          return;

        }

      } else {

        // check attempts on packet

        num = val+len-1; // set to end of packet

        if (num >= PACKET_BUFFER_SIZE)

          num -= PACKET_BUFFER_SIZE;

        if (send_buf[num] < NUM_RETRIES) {

          val = (val<2)?PACKET_BUFFER_SIZE+val-2:val-2; // set to start of this packet

          // retry sending

          send_buf[num]++; // increment retries

          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // go back one byte

          if (xbee_send_header(len+1) != WL_SUCCESS) {

            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");

            return;

          }

          len = XBEE_FRAME_TX_REQUEST_16;

          while(val!=num) {

            // compute checksum

            len += send_buf[val];

            if (xbee_putc(send_buf_get(&val)) != WL_SUCCESS) { // send  byte

              WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");

              return;

            }

          }

          len = 0xFF - len;

          if (xbee_putc(len) != WL_SUCCESS) { // send

            WL_DEBUG_PRINT_P("Error sending packet from ackhandle function\r\n");

            return;

          }

          return;

        } else {

          // done sending, mark as failed send

          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to frame num

          setack(send_buf[val],CCA_FAILURE);

          val = (val==0)?PACKET_BUFFER_SIZE-1:val-1; // set to start of this packet

          // remove packet

          while(num != send_buf_last) {

            if (send_buf_add(&val,send_buf_get(&num)) != WL_SUCCESS)

              return; // error

          }

          return;

        }

      }

    }    

    break; // shouldn't get here, but just in case

  }

}

/* adds a byte to the send buffer */

int8_t send_buf_add(uint8_t *ptr, uint8_t byte) {

  if (*ptr == send_buf_first && send_buf_num_packets > 0) {

    // buffer full

    WL_DEBUG_PRINT_P("send buffer full\r\n");

    return WL_ERROR_SENDING_BUFFER_FULL;

  }

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

  if (*ptr == PACKET_BUFFER_SIZE)

    *ptr = 0;

  return WL_SUCCESS;

}

/* gets a byte from the send buffer */

uint8_t send_buf_get(uint8_t *ptr) {

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

  if (*ptr == PACKET_BUFFER_SIZE)

    *ptr = 0;

  return byte;

}