void __attribute__((interrupt, no_auto_psv)) _T2Interrupt(void) {
MacPacket rx_packet;
Payload rx_payload;
if (!radioRxQueueEmpty())
{
// Check for unprocessed packet
rx_packet = radioDequeueRxPacket();
if(rx_packet == NULL) return;
// Retrieve payload
rx_payload = macGetPayload(rx_packet);
// Switch on packet type
Test* test = (Test*) malloc(sizeof(Test));
if(!test) return;
test->packet = rx_packet;
switch(payGetType(rx_payload))
{
case RADIO_TEST:
test->tf = &test_radio;
queuePush(fun_queue, test);
break;
case GYRO_TEST:
test->tf = &test_gyro;
queuePush(fun_queue, test);
break;
case HALL_TEST:
test->tf = &test_hall;
queuePush(fun_queue, test);
break;
case ACCEL_TEST:
test->tf = &test_accel;
queuePush(fun_queue, test);
break;
case DFLASH_TEST:
test->tf = &test_dflash;
queuePush(fun_queue, test);
break;
case MOTOR_TEST:
test->tf = &test_motor;
queuePush(fun_queue, test);
break;
case SMA_TEST:
test->tf = &test_sma;
queuePush(fun_queue, test);
break;
default: // temporary to check out what is happening to packets
test->tf = &test_radio;
queuePush(fun_queue, test);
break;
}
}
_T2IF = 0;
}
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 cmdPushFunc(MacPacket rx_packet) {
Payload rx_payload;
unsigned char command;
rx_payload = macGetPayload(rx_packet);
if(rx_payload != NULL) {
command = payGetType(rx_payload);
if(command < MAX_CMD_FUNC && cmd_func[command] != NULL) {
rx_payload->test = cmd_func[command];
carrayAddTail(fun_queue, rx_packet);
} else {
cmdError(); // halt on error - could also just ignore....
}
}
}
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;
}
void cmdProcessBuffer(void) {
MacPacket packet;
Payload pld;
unsigned char command;
// Check for unprocessed packet
//packet = radioDequeueRxPacket();
packet = carrayPopTail(input_queue);
if(packet == NULL) {
return;
} else {
Nop();
Nop();
}
pld = macGetPayload(packet);
command = payGetType(pld);
if(command < MAX_CMD_FUNC_SIZE) {
cmd_func[command](packet);
}
radioReturnPacket(packet);
}
int main() {
// Processor Initialization
SetupClock();
SwitchClocks();
SetupPorts();
sclockSetup();
LED_1 = 1;
LED_2 = 1;
LED_3 = 1;
// Message Passing
fun_queue = carrayCreate(FUN_Q_LEN);
cmdSetup();
// Radio setup
radioInit(RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL);
radioSetSrcAddr(RADIO_SRC_ADDR);
radioSetSrcPanID(RADIO_PAN_ID);
uart_tx_packet = NULL;
uart_tx_flag = 0;
//uartInit(&cmdPushFunc);
tactileInit();
// Need delay for encoders to be ready
delay_ms(100);
amsEncoderSetup();
mpuSetup();
tiHSetup();
dfmemSetup();
telemSetup();
adcSetup();
pidSetup();
LED_1 = 0;
LED_3 = 1;
while(1){
// Send outgoing radio packets
radioProcess();
/*
// Send outgoing uart packets
if(uart_tx_flag) {
uartSendPacket(uart_tx_packet);
uart_tx_flag = 0;
}*/
checkTactileBuffer();
// move received packets to function queue
while (!radioRxQueueEmpty()) {
// Check for unprocessed packet
rx_packet = radioDequeueRxPacket();
if(rx_packet != NULL) {
cmdPushFunc(rx_packet);
}
}
// process commands from function queue
while(!carrayIsEmpty(fun_queue)) {
rx_packet = carrayPopHead(fun_queue);
unsigned int rx_src_addr = rx_packet->src_addr.val;
if(rx_packet != NULL) {
rx_payload = macGetPayload(rx_packet);
if(rx_payload != NULL) {
rx_function = (test_function)(rx_payload->test);
if(rx_function != NULL) {
LED_2 = ~LED_2;
(rx_function)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload), rx_src_addr);
}
}
ppoolReturnFullPacket(rx_packet);
}
}
}
return 0;
}
