Subversion Repositories Code-Repo

#include "maindefs.h"
#include "msg_queues.h"
#include "interrupts.h"
#include "uart.h"
#include "i2c.h"
#include "adc.h"
#include "timers.h"
#include "xbee.h"
#include "led_driver.h"
#include "pwm.h"
#include "delays.h"
#include "pin_interrupts.h"
#include "buffer.h"
#include "imu.h"
#include "sleep.h"

#ifdef _REMOTE
#pragma config WDTEN = OFF          // Turn off watchdog timer
#pragma config XINST = OFF          // Turn off extended instruction set
#pragma config OSC = HSPLL          // Use external oscillator (101)
#pragma config IOL1WAY = OFF        // IOLOCK bit can be set and cleared as needed
#endif
#ifdef _BASE_STATION
#pragma config WDTEN = OFF          // Turn off watchdog timer
#pragma config XINST = OFF          // Turn off extended instruction set
#pragma config OSC = INTOSCPLL      // Use internal oscillator
#pragma config IOL1WAY = OFF        // IOLOCK bit can be set and cleared as needed
#endif

/* ----------- IO Pins -----------
 * RA0 - LED Display Latch Enable (V1) or Display CLK (V2) (PPS)
 * RA1 - LED Display CLK (V1) (PPS)    or Display DIN (V2) (PPS)
 * RA2 - LED Display DIN (V1) (PPS)    or Display Latch Enable (V2)
 * RA3 - LED Display Output Enable
 * RA4 - [CANNOT BE USED (VDDCORE/VCAP)]
 * RA5 - IR Reciever (PPS)
 * RA6 - Oscillator
 * RA7 - Oscillator
 * 
 * RC0 - PWM Output (IR) (PPS, Ports B and C only)
 * RC1 - PWM Output (IR) (PPS, Ports B and C only)
 * RC2 - LED Output (PPS, Ports B and C only)
 * RC3 - I2C SCL
 * RC4 - I2C SDA
 * RC5 - XBee Sleep (PPS)
 * RC6 - UART Debug Output
 * RC7 - UART Debug Input
 *
 * RB0 - XBee CTS (PPS)
 * RB1 - XBee RTS (PPS)
 * RB2 - XBee Tx (PPS)
 * RB3 - XBee Rx (PPS)
 * RB4 - Button Input (Port B Interrupt on Change)
 * RB5 - Button Input (Port B Interrupt on Change)
 * RB6 - Button Input (Port B Interrupt on Change)
 * RB7 - Button Input (Port B Interrupt on Change)
 * ---------------------------- */

#pragma udata msgbuffer
unsigned char msgbuffer[MSGLEN];
#pragma udata

void main(void) {
    XBEE_DATA xbee_data;
    I2C_DATA i2c_data;
    BUFFER_DATA buffer_data;
    int length;
    unsigned char msgtype;
    unsigned char i = 0;
#ifdef _BASE_STATION
    volatile unsigned char IR_receive_flag = 0;
    unsigned char i2c_last_req_size = 0;
    XBEE_ADDRESS_64 last_recv_address = {0};
    XBEE_ADDRESS_16 remote_address_16;
    XBEE_ADDRESS_64 remote_address_64;
#endif
#ifdef _REMOTE
    unsigned int xbee_parent_address_16 = 0;
    enum I2C_STATE i2c_state = I2C_STATE_IDLE;
    enum XBEE_STATE xbee_state = XBEE_STATE_WAITING_TO_JOIN;
    XBEE_ADDRESS_16 parent_address_16;
    XBEE_ADDRESS_64 parent_address_64;
#endif

    // Pointers to allow parsing of xbee data from arbitrary byte array
    XBEE_TX_DATA_PACKET_FRAME *frame_tx_data;
    XBEE_TX_AT_COMMAND_FRAME *frame_tx_at_command;
    
    XBEE_RX_AT_COMMAND_RESPONSE_FRAME *frame_at_cmd_response;
    XBEE_RX_DATA_PACKET_FRAME *frame_data_packet;
    XBEE_RX_DATA_TX_STATUS_FRAME *frame_tx_status;
    XBEE_RX_IO_DATA_SAMPLE_FRAME *frame_io_sample;
    XBEE_RX_EXPLICIT_COMMAND_FRAME *frame_explicit_cmd;
    XBEE_RX_REMOTE_AT_COMMAND_FRAME *frame_remote_at_cmd;
    XBEE_RX_ROUTE_RECORD_FRAME *frame_route_record;
    XBEE_RX_NODE_IDENTIFICATION_INDICATOR_FRAME *frame_node_identification;
    XBEE_RX_MODEM_STATUS_FRAME *frame_modem_status;
    
    /* --------------------- Oscillator Configuration --------------------- */
    OSCTUNEbits.PLLEN = 1;          // Enable 4x PLL
    OSCCONbits.IRCF = 0b111;        // Set INTOSC postscaler to 8MHz
    OSCCONbits.SCS = 0b00; // Use PLL as primary clock source
    /* -------------------------------------------------------------------- */

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;
    
    uart_init(); // Initialize the UART handler code
    xbee_init(&xbee_data); // Initialize the XBee handler code
    i2c_init(&i2c_data); // Initialize the I2C handler code
    buffer_init(&buffer_data);
    //    adc_init();                 // Initialize the ADC
    MQ_init(); // Initialize message queues before enabling any interrupts
    timers_init(); // Initialize timers
    led_driver_init(); // Initialize the driver for the LED display
    port_b_int_init(); // Initialze Port B interrupt handler
#ifdef _BASE_STATION
    intx_init();    // IR receiver input
#endif
    pwm_init(); // Initialize the PWM output driver

    interrupt_enable(); // Enable high-priority interrupts and low-priority interrupts
    interrupt_init(); // Initialize the interrupt priorities
#ifdef _BASE_STATION
    i2c_configure_slave(BASE_STATION_ADDRESS);
    led_driver_num(BASE_STATION_ADDRESS);
#endif
#ifdef _REMOTE
    i2c_configure_master(); // Configure the hardware i2c device as a master
    imu_init();
#endif

    DBG_PRINT_MAIN("\r\nMain: Program Started\r\n");

#ifdef _BASE_STATION
    sleep();
#endif
#ifdef _REMOTE
    // Turn on LED until XBee is connected to network
    pwm_LED_on();
#endif

    // Loop and process recieved messages from interrupts
    while (1) {
        // Call a routine that blocks until either message queues are not empty
        MQ_wait_on_incoming_msg_queues();

        // Process high priority message queue
        length = MQ_recvmsg_ToMainFromHigh(MSGLEN, &msgtype, (void *) msgbuffer);
        if (length < 0) {
            // No message, check the error code to see if it is concern
            if (length != MSG_QUEUE_EMPTY) {
                DBG_PRINT_MAIN("Main: (ERROR) Bad high priority receive, code = %d\r\n", length);
            }
        } else {
            switch (msgtype) {
                    /* --- I2C Message Handlers ----------------------------------*/
#ifdef _BASE_STATION
                case MSGTYPE_OVERRUN:
                    DBG_PRINT_MAIN("Main: (ERROR) UART overrun detected, type = %d\r\n", msgtype);
                    break;
                case MSGTYPE_I2C_DBG:
                    DBG_PRINT_MAIN("Main: I2C Dbg Data Recieved: ");
                    for (i = 0; i < length; i++) {
                        DBG_PRINT_MAIN("%02X ", msgbuffer[i]);
                    }
                    DBG_PRINT_MAIN("\r\n");
                    break;
                case MSGTYPE_I2C_DATA:
                    DBG_PRINT_MAIN("Main: I2C Data Recieved: ");
                    for (i = 0; i < length - 1; i++) {
                        DBG_PRINT_MAIN("%02X ", msgbuffer[i]);
                    }
                    DBG_PRINT_MAIN(" Event Count: %d", msgbuffer[length - 1]);
                    DBG_PRINT_MAIN("\r\n");
                    switch (msgbuffer[0]) {
                        case 0x2:
                            length = 1;
                            // Return size of stored data in buffer
                            if (buffer_data.stored_length > MSGLEN) {
                                msgbuffer[0] = MSGLEN;
                            } else {
                                msgbuffer[0] = buffer_data.stored_length;
                            }
                            i2c_last_req_size = msgbuffer[0];
                            DBG_PRINT_MAIN("Main: (I2C Return 0x2) Returning %d\r\n", msgbuffer[0]);
                            MQ_sendmsg_FromMainToHigh(length, MSGTYPE_I2C_REPLY, (void *) msgbuffer);
                            break;
                        case 0x4:
                            // Return data stored in buffer
                            buffer_read(i2c_last_req_size, msgbuffer);
                            DBG_PRINT_MAIN("Main: (I2C Return 0x4) Returning %d bytes\r\n", i2c_last_req_size);
                            MQ_sendmsg_FromMainToHigh(i2c_last_req_size, MSGTYPE_I2C_REPLY, (void *) msgbuffer);
                            pwm_LED_off();  // Turn off LED if it was on
                            break;
                        case 0x6:
                            // Return status of IR signal input
                            length = 1;
                            msgbuffer[0] = IR_receive_flag;
                            DBG_PRINT_MAIN("Main: Returning IR state %d\r\n", IR_receive_flag);
                            MQ_sendmsg_FromMainToHigh(length, MSGTYPE_I2C_REPLY, (void *) msgbuffer);
                            break;
                        case 0x7:
                            // Send value to specified remote

                            remote_address_64.UPPER_32.char_value[0] = msgbuffer[1];
                            remote_address_64.UPPER_32.char_value[1] = msgbuffer[2];
                            remote_address_64.UPPER_32.char_value[2] = msgbuffer[3];
                            remote_address_64.UPPER_32.char_value[3] = msgbuffer[4];
                            remote_address_64.LOWER_32.char_value[0] = msgbuffer[5];
                            remote_address_64.LOWER_32.char_value[1] = msgbuffer[6];
                            remote_address_64.LOWER_32.char_value[2] = msgbuffer[7];
                            remote_address_64.LOWER_32.char_value[3] = msgbuffer[8];
                            remote_address_16.INT_16.char_value[0] = msgbuffer[9];
                            remote_address_16.INT_16.char_value[1] = msgbuffer[10];
                            DBG_PRINT_MAIN("Main: Sending %X to (64) %08lX %08lX (16) %04X\r\n",
                                    msgbuffer[11],
                                    remote_address_64.UPPER_32.long_value,
                                    remote_address_64.LOWER_32.long_value,
                                    remote_address_16.INT_16.int_value);

                            frame_tx_data = (void *) msgbuffer;
                            frame_tx_data->data[0] = msgbuffer[11];
                            frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;
                            frame_tx_data->frame_id = 0;
                            frame_tx_data->destination_64.UPPER_32.long_value = remote_address_64.UPPER_32.long_value;
                            frame_tx_data->destination_64.LOWER_32.long_value = remote_address_64.LOWER_32.long_value;
                            ConvertEndian64(&frame_tx_data->destination_64);
                            frame_tx_data->destination_16.INT_16.int_value = remote_address_16.INT_16.int_value;
                            ConvertEndian16(&frame_tx_data->destination_16);
                            frame_tx_data->broadcast_radius = 0;
                            frame_tx_data->options = 0x01; // Disable ACK

                            length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;
                            xbee_process_transmit_frame((void *) msgbuffer, length);
                            break;
                        default:
                            DBG_PRINT_MAIN("Main: (ERROR) Unexpected message type recieved: %d\r\n", msgbuffer[0]);
                            break;
                    };
                    break;
#endif
#ifdef _REMOTE
                case MSGTYPE_I2C_MASTER_SEND_COMPLETE:
                    DBG_PRINT_MAIN("Main: I2C Master Send Complete\r\n");
                    break;
                case MSGTYPE_I2C_MASTER_SEND_FAILED:
                    DBG_PRINT_MAIN("Main: (ERROR) I2C Master Send Failed\r\n");
                    break;
                case MSGTYPE_I2C_MASTER_RECV_COMPLETE:
                    DBG_PRINT_MAIN("Main: I2C Master Receive Complete\r\n");
                    DBG_PRINT_MAIN("Main: (I2C Data) ");
                    for (i = 0; i < length; i++) {
                        DBG_PRINT_MAIN("%02X ", msgbuffer[i]);
                    }
                    DBG_PRINT_MAIN("\r\n");
                    
                    // Insert recorded value into buffer
                    if (i2c_state == I2C_STATE_READ_ACC) {
                        buffer_insert_one(RETURNID_ACC);
                        buffer_insert(6, msgbuffer);
                    } else if (i2c_state == I2C_STATE_READ_GYRO) {
                        buffer_insert_one(RETURNID_GYRO);
                        buffer_insert(6, msgbuffer);
                    }

                    if (buffer_free_space() < 7) {
                        wake();
                        // Send data to base station
                        frame_tx_data = (void *) msgbuffer;
                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;
                        frame_tx_data->frame_id = 0;
                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;
                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;
                        ConvertEndian64(&frame_tx_data->destination_64);
                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;
                        ConvertEndian16(&frame_tx_data->destination_16);
                        frame_tx_data->broadcast_radius = 0;
                        frame_tx_data->options = 0x01;  // Disable ACK
                        
                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + buffer_data.stored_length;
                        buffer_read(buffer_data.stored_length, frame_tx_data->data);
                        xbee_process_transmit_frame((void *) msgbuffer, length);
                    }

                    break;
                case MSGTYPE_I2C_MASTER_RECV_FAILED:
                    DBG_PRINT_MAIN("Main: (ERROR) I2C Master Receive Failed\r\n");
                    break;
#endif
                    /* -----------------------------------------------------------*/

                    /* --- XBee Message Handlers ---------------------------------*/
                case MSGTYPE_XBEE_RX_AT_COMMAND_RESPONSE:
                    DBG_PRINT_MAIN("\r\n");
                    frame_at_cmd_response = (void *) msgbuffer;
                    DBG_PRINT_MAIN("Main: XBee AT command response frame\r\n");
                    DBG_PRINT_MAIN("Command: %c%c\r\n", frame_at_cmd_response->command[0], frame_at_cmd_response->command[1]);
                    DBG_PRINT_MAIN("Status: %d\r\n", frame_at_cmd_response->command_status);
                    DBG_PRINT_MAIN("Data: ");
                    for (i = 0; i < length - XBEE_RX_AT_COMMAND_RESPONSE_FRAME_SIZE; i++) {
                        DBG_PRINT_MAIN("%02X ", frame_at_cmd_response->data[i]);
                    }
                    DBG_PRINT_MAIN("\r\n");
#ifdef _REMOTE
                    // Parse response from 'MP' command
                    if (frame_at_cmd_response->command[0] == 'M' &&
                            frame_at_cmd_response->command[1] == 'P') {
                        parent_address_16.INT_16.char_value[0] = frame_at_cmd_response->data[0];
                        parent_address_16.INT_16.char_value[1] = frame_at_cmd_response->data[1];
                        ConvertEndian16(&parent_address_16);
                        xbee_parent_address_16 = parent_address_16.INT_16.int_value;

                        DBG_PRINT_MAIN("Main: XBee parent address (16): %04X\r\n", xbee_parent_address_16);
                        // Set broadcast radius to 1
                        frame_tx_at_command = (void *) msgbuffer;
                        frame_tx_at_command->frame_type = XBEE_TX_AT_COMMAND;
                        frame_tx_at_command->frame_id = 1;
                        frame_tx_at_command->command[0] = 'B';
                        frame_tx_at_command->command[1] = 'H';
                        frame_tx_at_command->data[0] = 1;
                        length = XBEE_TX_AT_COMMAND_FRAME_SIZE + 1;
                        xbee_process_transmit_frame((void *) msgbuffer, length);

                        // Query for parent node info
                        frame_tx_at_command = (void *) msgbuffer;
                        frame_tx_at_command->frame_type = XBEE_TX_AT_COMMAND;
                        frame_tx_at_command->frame_id = 1;
                        frame_tx_at_command->command[0] = 'N';
                        frame_tx_at_command->command[1] = 'D';
                        length = XBEE_TX_AT_COMMAND_FRAME_SIZE;
                        xbee_process_transmit_frame((void *) msgbuffer, length);
                    }
                    // Parse response from 'ND' command
                    else if (frame_at_cmd_response->command[0] == 'N' &&
                            frame_at_cmd_response->command[1] == 'D') {
                        parent_address_64.UPPER_32.char_value[0] = frame_at_cmd_response->data[2];
                        parent_address_64.UPPER_32.char_value[1] = frame_at_cmd_response->data[3];
                        parent_address_64.UPPER_32.char_value[2] = frame_at_cmd_response->data[4];
                        parent_address_64.UPPER_32.char_value[3] = frame_at_cmd_response->data[5];
                        parent_address_64.LOWER_32.char_value[0] = frame_at_cmd_response->data[6];
                        parent_address_64.LOWER_32.char_value[1] = frame_at_cmd_response->data[7];
                        parent_address_64.LOWER_32.char_value[2] = frame_at_cmd_response->data[8];
                        parent_address_64.LOWER_32.char_value[3] = frame_at_cmd_response->data[9];
                        ConvertEndian64(&parent_address_64);

                        DBG_PRINT_MAIN("Main: XBee parent address (64): %08lX %08lX\r\n",
                                parent_address_64.UPPER_32.long_value,
                                parent_address_64.LOWER_32.long_value);
                        
                        xbee_state = XBEE_STATE_JOINED_NETWORK;
                        // Turn off LED after XBee has joined network
                        pwm_LED_off();
                    }
#endif
                    break;
                case MSGTYPE_XBEE_RX_DATA_PACKET:
                    wake();
                    DBG_PRINT_MAIN("Main: XBee data packet frame\r\n");
                    frame_data_packet = (void *) msgbuffer;
                    ConvertEndian64(&frame_data_packet->source_64);
                    ConvertEndian16(&frame_data_packet->source_16);
                    DBG_PRINT_MAIN("Source 64: %08lX-%08lX\r\n",
                            frame_data_packet->source_64.UPPER_32.long_value,
                            frame_data_packet->source_64.LOWER_32.long_value
                            );
                    DBG_PRINT_MAIN("Source 16: %04X\r\n", frame_data_packet->source_16.INT_16.int_value);
                    DBG_PRINT_MAIN("Options: 0x%02X\r\n", frame_data_packet->recieve_options);
                    DBG_PRINT_MAIN("Data: ");
                    for (i = 0; i < length - XBEE_RX_DATA_PACKET_FRAME_SIZE; i++) {
                        DBG_PRINT_MAIN("%02X ", frame_data_packet->data[i]);
                    }
                    DBG_PRINT_MAIN("\r\n");
#ifdef _BASE_STATION

                    if (frame_data_packet->source_64.LOWER_32.long_value != last_recv_address.LOWER_32.long_value ||
                            frame_data_packet->source_64.UPPER_32.long_value != last_recv_address.UPPER_32.long_value) {
                        // Save remote's 64 and 16 bit addresses
                        buffer_insert_one(RETURNID_NEW_REMOTE_SRC);
                        buffer_insert(4, frame_data_packet->source_64.UPPER_32.char_value);
                        buffer_insert(4, frame_data_packet->source_64.LOWER_32.char_value);
                        buffer_insert(2, frame_data_packet->source_16.INT_16.char_value);
                        // Save data
                        buffer_insert(length - XBEE_RX_DATA_PACKET_FRAME_SIZE, frame_data_packet->data);
                        // Save address of sender
                        last_recv_address.UPPER_32.long_value = frame_data_packet->source_64.UPPER_32.long_value;
                        last_recv_address.LOWER_32.long_value = frame_data_packet->source_64.LOWER_32.long_value;
                    } else {
                        // Receive data from same remote that previously sent data
                        buffer_insert(length - XBEE_RX_DATA_PACKET_FRAME_SIZE, frame_data_packet->data);
                    }
                    
                    // Send value of first byte received to LED display
                    led_driver_num(frame_data_packet->data[0]);
#endif
#ifdef _REMOTE
                    led_driver_num(frame_data_packet->data[0]);
#endif
                    sleep();
                    break;
                case MSGTYPE_XBEE_RX_DATA_TX_STATUS:
                    DBG_PRINT_MAIN("Main: XBee TX status frame\r\n");
                    frame_tx_status = (void *) msgbuffer;
                    DBG_PRINT_MAIN("Destination: %02X\r\n", frame_tx_status->destination_16);
                    DBG_PRINT_MAIN("Transmit Retry Count: %02X\r\n", frame_tx_status->transmit_retry_count);
                    DBG_PRINT_MAIN("Delivery Status: %02X\r\n", frame_tx_status->delivery_status);
                    DBG_PRINT_MAIN("Discovery Status: %02X\r\n", frame_tx_status->discovery_status);
                    break;
                case MSGTYPE_XBEE_RX_IO_DATA_SAMPLE:
                    DBG_PRINT_MAIN("Main: XBee IO data sample frame\r\n");
                    frame_io_sample = (void *) msgbuffer;
                    break;
                case MSGTYPE_XBEE_RX_EXPLICIT_COMMAND:
                    DBG_PRINT_MAIN("Main: XBee explicit command frame\r\n");
                    frame_explicit_cmd = (void *) msgbuffer;
                    break;
                case MSGTYPE_XBEE_RX_REMOTE_AT_COMMAND_RESPONSE:
                    DBG_PRINT_MAIN("Main: XBee remote AT command response frame\r\n");
                    frame_remote_at_cmd = (void *) msgbuffer;
                    break;
                case MSGTYPE_XBEE_RX_ROUTE_RECORD:
                    DBG_PRINT_MAIN("Main: XBee route record frame\r\n");
                    frame_route_record = (void *) msgbuffer;
                    break;
                case MSGTYPE_XBEE_RX_NODE_IDENTIFICATION:
                    DBG_PRINT_MAIN("Main: XBee node identification frame\r\n");
                    frame_node_identification = (void *) msgbuffer;
                    ConvertEndian64(&frame_node_identification->source_64);
                    ConvertEndian64(&frame_node_identification->remote_64);
                    ConvertEndian16(&frame_node_identification->source_16);
                    ConvertEndian16(&frame_node_identification->remote_16);
                    ConvertEndian16(&frame_node_identification->parent_16);
                    DBG_PRINT_MAIN("Source 64: %08lX-%08lX\r\n",
                            frame_node_identification->source_64.UPPER_32.long_value,
                            frame_node_identification->source_64.LOWER_32.long_value
                            );
                    DBG_PRINT_MAIN("Source 16: %04X\r\n", frame_node_identification->source_16.INT_16.int_value);
                    DBG_PRINT_MAIN("Remote 64: %08lX-%08lX\r\n",
                            frame_node_identification->remote_64.UPPER_32.long_value,
                            frame_node_identification->remote_64.LOWER_32.long_value
                            );
                    DBG_PRINT_MAIN("Remote 16: %04X\r\n", frame_node_identification->remote_16.INT_16.int_value);
                    DBG_PRINT_MAIN("Parent 16: %04X\r\n", frame_node_identification->parent_16.INT_16.int_value);
                    DBG_PRINT_MAIN("Receive Options: %02X\r\n", frame_node_identification->recieve_options);
                    buffer_insert_one(RETURNID_NEW_REMOTE_CON);
                    buffer_insert(4, frame_node_identification->remote_64.UPPER_32.char_value);
                    buffer_insert(4, frame_node_identification->remote_64.LOWER_32.char_value);
                    buffer_insert(2, frame_node_identification->remote_16.INT_16.char_value);
                    break;
                case MSGTYPE_XBEE_RX_FRAME_MODEM_STATUS:
                    DBG_PRINT_MAIN("Main: XBee modem status frame\r\n");
                    frame_modem_status = (void *) msgbuffer;
                    DBG_PRINT_MAIN("Status: %02X (", frame_modem_status->status);
                    switch(frame_modem_status->status) {
                        case 0:
                            DBG_PRINT_MAIN("Hardware Reset");
#ifdef _REMOTE
                            xbee_state = XBEE_STATE_WAITING_TO_JOIN;
#endif
                            break;
                        case 1:
                            DBG_PRINT_MAIN("Watchdog Timer Reset");
                            break;
                        case 2:
                            DBG_PRINT_MAIN("Joined Network");
#ifdef _REMOTE
                            // Query for parent's address
                            frame_tx_at_command = (void *) msgbuffer;
                            frame_tx_at_command->frame_type = XBEE_TX_AT_COMMAND;
                            frame_tx_at_command->frame_id = 1;
                            frame_tx_at_command->command[0] = 'M';
                            frame_tx_at_command->command[1] = 'P';
                            length = XBEE_TX_AT_COMMAND_FRAME_SIZE;
                            xbee_process_transmit_frame((void *) msgbuffer, length);
#endif
                            break;
                        case 3:
                            DBG_PRINT_MAIN("Disassociated");
                            pwm_LED_on();
                            break;
                        case 6:
                            DBG_PRINT_MAIN("Coordinator Started");
                            break;
                        case 7:
                            DBG_PRINT_MAIN("Network Security Key Updated");
                            break;
                        case 0x11:
                            DBG_PRINT_MAIN("Modem Config Changed While Joining");
                            break;
                    }
                    DBG_PRINT_MAIN(")\r\n");
                    break;
                    /* -----------------------------------------------------------*/
            };
//            continue;
        }

        // Process low priority queue
        length = MQ_recvmsg_ToMainFromLow(MSGLEN, &msgtype, (void *) msgbuffer);
        if (length < 0) {
            // No message, check the error code to see if it is concern
            if (length != MSG_QUEUE_EMPTY) {
                DBG_PRINT_MAIN("Main: (ERROR) Bad low priority receive, code = %d\r\n", length);
            }
        } else {
            switch (msgtype) {
                case MSGTYPE_INT1:
#ifdef _BASE_STATION
                    wake();
                    IR_receive_flag = 1;
                    pwm_LED_on();
//                    timer2_enable();    // Enable timer 2 to turn off LED
                    sleep();
//                    DBG_PRINT_MAIN("Main: INT1 Interrupt\r\n");
#endif
                    break;
                /* --- Port B Interrupt Handlers -----------------------------*/
                case MSGTYPE_PORTB_4_DOWN:
                    DBG_PRINT_MAIN("Main: Port B4 Down\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        wake(); // Wake up all components
                        timer1_enable(); // Set timer to start data polling
                        pwm_LED_on();
                    }
#endif
                    break;
                case MSGTYPE_PORTB_4_UP:
                    DBG_PRINT_MAIN("Main: Port B4 Up\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        wake();
                        timer1_disable(); // Stop data polling timer
                        
                        // Send remaining buffer data to base station
                        frame_tx_data = (void *) msgbuffer;
                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;
                        frame_tx_data->frame_id = 0;
                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;
                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;
                        ConvertEndian64(&frame_tx_data->destination_64);
                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;
                        ConvertEndian16(&frame_tx_data->destination_16);
                        frame_tx_data->broadcast_radius = 0;
                        frame_tx_data->options = 0x01;  // Disable ACK
                        
                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + buffer_data.stored_length;
                        buffer_read(buffer_data.stored_length, frame_tx_data->data);
                        xbee_process_transmit_frame((void *) msgbuffer, length);
                        
                        frame_tx_data->data[0] = RETURNID_DATA_FIN;
                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;
                        xbee_process_transmit_frame((void *) msgbuffer, length);

                        i2c_state = I2C_STATE_IDLE;
                        pwm_LED_off();
                        sleep();
                    }
#endif
                    break;
                case MSGTYPE_PORTB_5_DOWN:
                    DBG_PRINT_MAIN("Main: Port B5 Down\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        wake(); // Wake up all components
                        timer3_enable(); // Enable PWM timer
                        frame_tx_data = (void *) msgbuffer;
                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;
                        frame_tx_data->frame_id = 0;
                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;
                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;
                        ConvertEndian64(&frame_tx_data->destination_64);
                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;
                        ConvertEndian16(&frame_tx_data->destination_16);
                        frame_tx_data->broadcast_radius = 0;
                        frame_tx_data->options = 0x01; // Disable ACK
                        frame_tx_data->data[0] = RETURNID_CONN;

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;
                        xbee_process_transmit_frame((void *) msgbuffer, length);
                    }
#endif
                    break;
                case MSGTYPE_PORTB_5_UP:
                    DBG_PRINT_MAIN("Main: Port B5 Up\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        timer3_disable(); // Disable PWM timer
                        sleep();
                    }
#endif
                    break;
                case MSGTYPE_PORTB_6_DOWN:
                    DBG_PRINT_MAIN("Main: Port B6 Down\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        wake(); // Wake up all components
                        frame_tx_data = (void *) msgbuffer;
                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;
                        frame_tx_data->frame_id = 0;
                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;
                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;
                        ConvertEndian64(&frame_tx_data->destination_64);
                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;
                        ConvertEndian16(&frame_tx_data->destination_16);
                        frame_tx_data->broadcast_radius = 0;
                        frame_tx_data->options = 0x01; // Disable ACK
                        frame_tx_data->data[0] = RETURNID_BTN2;

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;
                        xbee_process_transmit_frame((void *) msgbuffer, length);
                    }
#endif
                    break;
                case MSGTYPE_PORTB_6_UP:
                    DBG_PRINT_MAIN("Main: Port B6 Up\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        sleep();
                    }
#endif
                    break;
                case MSGTYPE_PORTB_7_DOWN:
                    DBG_PRINT_MAIN("Main: Port B7 Down\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        wake(); // Wake up all components
                        frame_tx_data = (void *) msgbuffer;
                        frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;
                        frame_tx_data->frame_id = 0;
                        frame_tx_data->destination_64.UPPER_32.long_value = parent_address_64.UPPER_32.long_value;
                        frame_tx_data->destination_64.LOWER_32.long_value = parent_address_64.LOWER_32.long_value;
                        ConvertEndian64(&frame_tx_data->destination_64);
                        frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address_16;
                        ConvertEndian16(&frame_tx_data->destination_16);
                        frame_tx_data->broadcast_radius = 0;
                        frame_tx_data->options = 0x01; // Disable ACK
                        frame_tx_data->data[0] = RETURNID_BTN1;

                        length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;
                        xbee_process_transmit_frame((void *) msgbuffer, length);;
                    }
#endif
                    break;
                case MSGTYPE_PORTB_7_UP:
                    DBG_PRINT_MAIN("Main: Port B7 Up\r\n");
#ifdef _REMOTE
                    if (xbee_state == XBEE_STATE_JOINED_NETWORK) {
                        sleep();
                    }
#endif
                    break;
                    /* -----------------------------------------------------------*/
                    /* --- Timer Interrupt Handlers ------------------------------*/
                case MSGTYPE_TIMER0:
                    DBG_PRINT_MAIN("Main: Timer 0 Interrupt\r\n");
#ifdef _BASE_STATION
                    pwm_LED_off();
                    IR_receive_flag = 0;    // Reset IR receive flag
                    INTCON3bits.INT1IE = 1; // Turn on INT1 interrupt
#endif
                    sleep_enable();
                    break;
                case MSGTYPE_TIMER1:
                    DBG_PRINT_MAIN("Main: Timer 1 Interrupt\r\n");
#ifdef _REMOTE
//                    /* XBee Demo */
//                    frame_tx_data = (void *) msgbuffer;
//                    frame_tx_data->frame_type = XBEE_TX_DATA_PACKET;
//                    frame_tx_data->frame_id = 0;
//                    frame_tx_data->destination_64.UPPER_32.long_value = 0x00000000;
//                    frame_tx_data->destination_64.LOWER_32.long_value = 0x00000000;
//                    ConvertEndian64(&frame_tx_data->destination_64);
//                    frame_tx_data->destination_16.INT_16.int_value = xbee_parent_address;
//                    ConvertEndian16(&frame_tx_data->destination_16);
//                    frame_tx_data->broadcast_radius = 1;
//                    frame_tx_data->options = 0x01; // Disable ACK
//                    frame_tx_data->data[0] = counter;
//
//                    led_driver_num(counter);
//                    counter++;
//                    if (counter == 100)
//                        counter = 0;
//
//                    length = XBEE_TX_DATA_PACKET_FRAME_SIZE + 1;
//                    xbee_process_transmit_frame((void *) msgbuffer, length);

                    /* Read values from accelerometer and gyroscope */
                    if (i2c_state == I2C_STATE_READ_GYRO) {
                        imu_read_acc();
                        i2c_state = I2C_STATE_READ_ACC;
                    } else if (i2c_state == I2C_STATE_READ_ACC) {
                        imu_read_gyro();
                        i2c_state = I2C_STATE_READ_GYRO;
                    } else {    // I2C_STATE_IDLE
                        imu_read_acc();
                        i2c_state = I2C_STATE_READ_ACC;
                    }
#endif
                    break;
                case MSGTYPE_TIMER2:
                    DBG_PRINT_MAIN("Main: Timer 2 Interrupt\r\n");
#ifdef _BASE_STATION
                    IR_receive_flag = 0;
                    pwm_LED_off();
                    INTCON3bits.INT1IE = 1; // Turn on INT1 interrupt
#endif
                    break;
//                case MSGTYPE_ADC_NEWVALUE:
//                    // Get the value in the ADC
//                    adc_last_value = *((unsigned int*) msgbuffer);
//                    adc_last_value_shifted = adc_last_value >> 4;
//                    DBG_PRINT_MAIN("Main: ADC Value = %d\r\n", adc_last_value);
//
//                    adc_start();
//                    break;
                default:
                    DBG_PRINT_MAIN("Main: (ERROR) Unexpected msg in low queue, length = %d, type = %d\r\n", length, msgtype);
                    for (i = 0; i < length; i++) {
                        DBG_PRINT_MAIN("%X ", msgbuffer[i]);
                    }
                    DBG_PRINT_MAIN("\r\n");
                    break;
            };
            continue;
        }
    }
}