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

 * @brief Wireless library receive functions

 *

 * Implementation of high level wireless communication.

 * This is the receive functions portion of the library.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#include <string.h>

#include "wl_defs.h"

#include "wireless.h"

#include "xbee.h"

/**

 * Definition for wireless library receive packet structure

 *

 * Basic Buffer Packet:

 *   byte 1: length of source+data

 *   bytes 2-3: source

 *   bytes 4-n: data

 *

 * Other Buffer Packet:

 *   byte 1: length of group+source+data

 *   byte 2: group number

 *   bytes 3-4: source

 *   bytes 5-n: data

 *

 **/

extern uint8_t xbee_basic_buf[PACKET_BUFFER_SIZE];  // buffer for basic-group packets

extern uint8_t xbee_other_buf[PACKET_BUFFER_SIZE];  // buffer for non-basic-group packets

extern uint8_t basic_buf_first;        // beginning of first packet in basic buffer

extern uint8_t basic_buf_last;        // end of last packet in basic buffer

extern uint8_t other_buf_first; // beginning of first packet in other buffer

extern uint8_t other_buf_last;  // end of last packet in other buffer

extern uint8_t xbee_basic_buf_get(uint8_t *ptr); // get byte from basic buf

extern uint8_t xbee_other_buf_get(uint8_t *ptr); // get byte from other buf

/**

 * @addtogroup wireless Wireless

 * @{

 **/

/**

 * the main receive function (similar to wl_do)

 * 

 * when called, this function will receive the next packet on the default packet group

 * it will also dispatch registered packet handler functions for any other packet groups it has received

 *

 * @param data an already-initialized array to store the default group packet data in

 * @param length the length of the initialized data array

 *

 * @return the length of the used portion of data array or error (<0)

 **/

int8_t wl_get(char *data, uint8_t length) {

  int8_t return_code = wl_dispatch(); // run dispatch of other packet groups

  if (return_code != 0)

    return return_code; // error, so end early

  return wl_get_basic(data, length); // get a basic packet

}

/**

 * function to receive only packets on the default group

 * this function is only meant to receive packets sent using wl_send_basic()

 * 

 * @param data an already-initialized array to store the default group packet data in

 * @param length the length of the initialized data array

 * 

 * @return the length of the used portion of data array or error (<0)

 **/

int8_t wl_get_basic(char *data, uint8_t length) {

  uint8_t buf_pos = basic_buf_first;        // start at beginning of first (oldest) basic packet

  uint8_t data_length = 0xFF;

  while((data_length = xbee_basic_buf_get(&buf_pos)) == 0xFF) {  // get packet length

    if (buf_pos == basic_buf_last)

      break;

  }

  if (buf_pos == basic_buf_last)

      return WL_SUCCESS;

  if (data_length - 3 > length)   // not enough room for data in destination

    return WL_ERROR_TOO_SMALL;

  WL_DEBUG_PRINT_P("entered wl_get_basic\r\n");

  // packet has not fully arrived yet

  if (buf_pos + data_length >= PACKET_BUFFER_SIZE) {

    if ((buf_pos + data_length - PACKET_BUFFER_SIZE) > basic_buf_last)

      return WL_SUCCESS;

  } else {

    if ((buf_pos + data_length) > basic_buf_last)

      return WL_SUCCESS;

  }  

  WL_DEBUG_PRINT_P("wl_get_basic() - packet has arrived\r\n");

  // ignore the source ID

  xbee_basic_buf_get(&buf_pos);

  xbee_basic_buf_get(&buf_pos);

  // copy data to destination, byte by byte to account for circular buffer

  data_length-=2;

  uint8_t i;

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

    data[i] = xbee_basic_buf_get(&buf_pos);

  }

  WL_DEBUG_PRINT_P("we advanced ");

  WL_DEBUG_PRINT_INT(i+3);

  WL_DEBUG_PRINT_P(" bytes"); 

  WL_DEBUG_PRINT_P("|first=");

  WL_DEBUG_PRINT_INT(basic_buf_first);

  WL_DEBUG_PRINT_P("|last=");

  WL_DEBUG_PRINT_INT(basic_buf_last);

  // "free up" this packet's buffer space

  if (buf_pos == 0)

    basic_buf_first = PACKET_BUFFER_SIZE-1;

  else

    basic_buf_first = buf_pos-1;

  xbee_basic_buf[basic_buf_first] = 0xFF;

  WL_DEBUG_PRINT_P("|first=");

  WL_DEBUG_PRINT_INT(basic_buf_first);

  WL_DEBUG_PRINT_P("|last=");

  WL_DEBUG_PRINT_INT(basic_buf_last);

  WL_DEBUG_PRINT_P("\r\n");

  return data_length;

}

/*

 * function to dispatch all registered packet handlers for received packets on non-default groups

 *

 * @return error codes

 */

int8_t wl_dispatch(void) {

  uint8_t buf_pos = other_buf_first;        // start at beginning of first (oldest) basic packet

  uint8_t data_length = 0xFF;  

  uint8_t group;  // group of packet under consideration

  FNPTR; // pointer to handler

  uint8_t source=0; // robot ID of sender of packet under consideration

  WL_DEBUG_PRINT_P("entered wl_dispatch\r\n");

  while(1) {

    while((data_length = xbee_other_buf_get(&buf_pos)) == 0xFF) {  // get packet length

      WL_DEBUG_PRINT_P("|searching - data_length=");

      WL_DEBUG_PRINT_HEX(data_length);

      WL_DEBUG_PRINT_P(" on pos=");

      WL_DEBUG_PRINT_INT(buf_pos);

      if (buf_pos == other_buf_last)

        break;

    }

    if (buf_pos == other_buf_last) {

      WL_DEBUG_PRINT_P("|buf_pos=other_buf_last\r\n");

      return WL_SUCCESS;

    }       

    // packet has not fully arrived yet

    if (buf_pos + data_length >= PACKET_BUFFER_SIZE) {

      if ((buf_pos + data_length - PACKET_BUFFER_SIZE) > other_buf_last)

        break;

    } else {

      if ((buf_pos + data_length) > other_buf_last)

        break;

    }  

    WL_DEBUG_PRINT_P("wl_dispatch() - packet has arrived\r\n");

    // get group number

    group = xbee_other_buf_get(&buf_pos);

    if (group >= MAX_PACKET_GROUPS || wl_packet_handlers[group].FUNC == NULL) {

      // this is an invalid group, so go to next packet

      if (buf_pos + data_length - 2 >= PACKET_BUFFER_SIZE)

        buf_pos += data_length - 2 - PACKET_BUFFER_SIZE;

      else

        buf_pos += data_length - 2;

    } else {

      // this is a valid group number, so get handler

      FUNC = wl_packet_handlers[group].FUNC;

      xbee_other_buf_get(&buf_pos); // ignore first byte of source id

      source = xbee_other_buf_get(&buf_pos); // get low byte of source id

      // call handler

      (*FUNC)(xbee_other_buf+buf_pos,data_length-4,source);

      // go to next packet

      if (buf_pos + data_length - 4 >= PACKET_BUFFER_SIZE)

        buf_pos += data_length - 4 - PACKET_BUFFER_SIZE;

      else

        buf_pos += data_length - 4;

    }

  }

  WL_DEBUG_PRINT_P("other_buf_first=");

  WL_DEBUG_PRINT_INT(other_buf_first);

  // mark packets as read

  other_buf_first = (buf_pos==0)?(PACKET_BUFFER_SIZE-1):(buf_pos-1);

  xbee_other_buf[other_buf_first] = 0xFF;

  WL_DEBUG_PRINT_P("Exit wl dispatch\r\n");  

  WL_DEBUG_PRINT_P("other_buf_first=");

  WL_DEBUG_PRINT_INT(other_buf_first);

  WL_DEBUG_PRINT_P("\r\n");

  return WL_SUCCESS;

}

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

// Define all private functions down here