static void cmdGetImuLoop(unsigned char status, unsigned char length, unsigned char *frame) {
unsigned int count;
unsigned long tic;
unsigned char *tic_char;
Payload pld;
LED_RED = 1;
count = frame[0] + (frame[1] << 8);
tic_char = (unsigned char*) &tic;
swatchReset();
tic = swatchTic();
while (count) {
pld = payCreateEmpty(16); // data length = 16
paySetData(pld, 4, tic_char);
payAppendData(pld, 4, 6, xlReadXYZ());
payAppendData(pld, 10, 6, gyroReadXYZ());
paySetStatus(pld, status);
paySetType(pld, CMD_GET_IMU_DATA);
radioSendPayload(macGetDestAddr(), pld);
count--;
payDelete(pld);
delay_ms(4);
tic = swatchTic();
}
LED_RED = 0;
}
void pqPush(PayQueue pq, Payload pld) {
Payload pay;
if (queueIsFull(pq)) {
pay = (Payload)queuePop(pq);
payDelete(pay);
}
queueAppend(pq, pld);
}
int main ( void )
{
fun_queue = queueInit(FUN_Q_LEN);
rx_pay_queue = pqInit(12); //replace 12 with a #define const later
test_function tf;
/* Initialization */
SetupClock();
SwitchClocks();
SetupPorts();
SetupInterrupts();
SetupI2C();
SetupADC();
SetupTimer1();
SetupPWM();
SetupTimer2();
gyroSetup();
xlSetup();
dfmemSetup();
WordVal pan_id = {RADIO_PAN_ID};
WordVal src_addr = {RADIO_SRC_ADDR};
WordVal dest_addr = {RADIO_DEST_ADDR};
radioInit(src_addr, pan_id, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetDestAddr(dest_addr);
radioSetChannel(RADIO_MY_CHAN);
char j;
for(j=0; j<3; j++){
LED_2 = ON;
delay_ms(500);
LED_2 = OFF;
delay_ms(500);
}
LED_2 = ON;
EnableIntT2;
while(1){
while(!queueIsEmpty(fun_queue))
{
rx_payload = pqPop(rx_pay_queue);
tf = (test_function)queuePop(fun_queue);
(*tf)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload));
payDelete(rx_payload);
}
}
return 0;
}
void cmdHandleRadioRxBuffer(void) {
Payload pld;
unsigned char command, status;
if ((pld = radioReceivePayload()) != NULL) {
status = payGetStatus(pld);
command = payGetType(pld);
//Due to bugs, command may be a surprious value; check explicitly
if (command <= MAX_CMD_FUNC) {
cmd_func[command](status, pld->data_length, payGetData(pld));
}
payDelete(pld);
}
return;
}
//read data from the UART, and call the proper function based on the Xbee code
void __attribute__((__interrupt__, no_auto_psv)) _U1RXInterrupt(void)
{
static unsigned char uart_rx_state = UART_RX_WAITING;
static unsigned char uart_rx_cnt = 0;
static Payload uart_pld;
static WordVal uart_pld_len;
static byte uart_checksum;
static unsigned char packet_type = 0;
static unsigned char test;
unsigned char c;
if(U1STAbits.OERR)
{
U1STAbits.OERR = 0;
}
c = ReadUART1();
if (uart_rx_state == UART_RX_WAITING && c == RX_START)
{
uart_rx_state = UART_RX_ON;
packet_type = 0;
uart_rx_cnt = 1;
uart_checksum = 0x00;
}else if (uart_rx_state == UART_RX_ON)
{
switch (uart_rx_cnt)
{
//XBee interface uses two bytes for payload length, despite the
//fact that packets can't be longer than 128 bytes. The high byte
//is extracted, but never used here.
case LEN_HB_POS:
uart_pld_len.byte.HB = c;
uart_rx_cnt++;
break;
case LEN_LB_POS:
uart_pld_len.byte.LB = c;
//We create a payload structure to store the data incoming from the uart
uart_pld = payCreateEmpty(uart_pld_len.byte.LB-PAYLOAD_HEADER_LENGTH);
test = uart_pld_len.byte.LB;
uart_rx_cnt++;
break;
case API_ID_POS:
//Currently, we're only supporting the 16-bit TX/RX API,
//and the AT command mode
packet_type = c;
uart_checksum += c;
uart_rx_cnt++;
break;
default:
if (uart_rx_cnt == (uart_pld_len.byte.LB + RX_DATA_OFFSET-1))
{
if (uart_checksum + c == 0xFF) //We have a legit packet
{
//Check for type of packet and call relevant function
switch (packet_type)
{
case AT_COMMAND_MODE:
xbeeHandleAt(uart_pld);
break;
case TX_16BIT:
xbeeHandleTx(uart_pld);
break;
default:
//do nothing, but probably should send an error
break;
}
payDelete(uart_pld);
}else //Start over
{
payDelete(uart_pld);
}
uart_rx_state = UART_RX_WAITING;
}else
{
uart_pld->pld_data[uart_rx_cnt-RX_DATA_OFFSET] = c;
uart_checksum += c;
uart_rx_cnt++;
}
break;
}
}
_U1RXIF = 0;
}
