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 data

 *   bytes 2-3: source

 *   bytes 4-n: data

 *

 * Other Buffer Packet:

 *   byte 1: length of 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

/**

 * @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 = xbee_basic_buf[buf_pos % PACKET_BUFFER_SIZE];  // get packet length

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

        return WL_ERROR_TOO_SMALL;

    // packet has not fully arrived yet

    if (buf_pos + data_length > basic_buf_last        // length goes farther than end of buffer

            && !(buf_pos >= basic_buf_last && buf_pos + data_length > buf_pos))

        /* basic test is Beginning + Length > Buffer End means packet isn't all there yet

           if end counter overflows, a complete packet's end may still be greater than the buffer end index,

           but such a packet can be read from the buffer

         */

        return WL_SUCCESS;  // no data written

    buf_pos++;

    buf_pos++;        // ignore the source ID

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

    // don't copy the 3 header bytes. nobody wants those.

    for (; buf_pos < basic_buf_first + data_length; buf_pos++)

        memcpy(data + buf_pos - 3, xbee_basic_buf + (buf_pos % PACKET_BUFFER_SIZE), 1);

    basic_buf_first += data_length; // "free up" this packet's buffer space

    return data_length;

}

/*

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

 *

 * @return error codes (TBD)

 */

int8_t wl_dispatch(void) {

    uint8_t buf_pos = other_buf_first;        // current position in buffer

    uint8_t group;  // group of packet under consideration

    void (*handler)(uint8_t *data, uint8_t length, uint8_t source);  // handler for group of packet under consideration

    uint8_t length = xbee_other_buf[buf_pos % PACKET_BUFFER_SIZE];  // length of packet under consideration

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

    // consider packets one at a time until buffer contains no complete packets

    while (buf_pos + length <= other_buf_last        // length goes farther than end of buffer

            || (buf_pos >= other_buf_last && buf_pos + length > buf_pos)) {

        /* basic test is Beginning + Length > Buffer End means packet isn't all there yet

           if end counter overflows, a complete packet's end may still be greater than the buffer end index,

           but such a packet can be read from the buffer

         */

        buf_pos++;

        group = xbee_other_buf[buf_pos];    // get group from packet

        if (group >= MAX_PACKET_GROUPS)  // invalid group number

            buf_pos += length - 1 + 1;        // go to first byte of next packet

        else {        // valid group number

            handler = wl_packet_handlers[group].FUNC;        // get function from handler array

            buf_pos++;

            buf_pos++;  // ignore 1st byte of ID (will be all 0s)

            source = xbee_other_buf[buf_pos];   // get sender ID from packet

            buf_pos++;  // now index of beginning of packet data (mod PACKET_BUFFER_SIZE)

            if (handler)        // check whether group is handled

                (*handler)(xbee_other_buf + (buf_pos & PACKET_BUFFER_SIZE), length - 4, source);

            // go to next packet

            buf_pos += length - 4 + 1;  // go to first byte of next packet

        }

        length = xbee_other_buf[buf_pos & PACKET_BUFFER_SIZE];        // get length of next packet

    }

    return WL_SUCCESS;

}

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

// Define all private functions down here