static void cmdDirUpdateResponse(MacPacket packet) { typedef struct { unsigned long long UUID; unsigned long timestamp; unsigned int address; }UpdateEntry; Payload pld; unsigned int i, num_entries; DirEntry entry; UpdateEntry *update; pld = macGetPayload(packet); update = (UpdateEntry*) payGetData(pld); num_entries = payGetDataLength(pld)/sizeof(UpdateEntry); for(i = 0; i < num_entries; i++) { entry = dirQueryID(update[i].UUID); // Retrieve entry if(entry == NULL) { // If not seen, create entry = dirAddNew(); if(entry == NULL) { continue; } // Check for creation failure // Skip updates older than current info } else if(update[i].timestamp < entry->timestamp){ continue; } entry->uuid = update[i].UUID; entry->address = update[i].address; entry->pan_id = netGetLocalPanID(); entry->timestamp = update[i].timestamp; } }
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; }
/*----------------------------------------------------------------------------- * AUX functions -----------------------------------------------------------------------------*/ static void cmdEcho(MacPacket packet) { Payload pld = macGetPayload(packet); unsigned char status = payGetStatus(pld); unsigned char* frame = payGetData(pld); unsigned int length = payGetDataLength(pld); unsigned int srcAddr = macGetSrcAddr(packet); MacPacket response; response = radioRequestPacket(length); if(response == NULL) { return; } macSetDestAddr(response, srcAddr); pld = response->payload; paySetData(pld, length, frame); paySetStatus(pld, status); paySetType(pld, CMD_ECHO); while(!radioEnqueueTxPacket(response)); }
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; }