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; }