// ==== PRIVATE FUNCTIONS ===================================================== static void clksyncSendRequest(SyncStatus sync) { MacPacket packet; Payload pld; unsigned long s0; packet = radioRequestPacket(4); if(packet == NULL) { return; } macSetDestAddr(packet, sync->master_addr); macSetDestPan(packet, sync->master_pan); pld = macGetPayload(packet); paySetType(pld, CMD_CLOCK_UPDATE_REQUEST); while(!radioTxQueueEmpty()) { radioProcess(); } s0 = sclockGetGlobalTicks(); pld = macGetPayload(packet); paySetData(pld, 4, (unsigned char*) &s0); while(!radioEnqueueTxPacket(packet)) { radioProcess(); } radioProcess(); sync->requests++; if(sync->requests > MAX_REQUEST_ATTEMPTS) { sync->state = STATE_REQUEST_TIMEOUT; } }
void clksyncHandleRequest(MacPacket packet) { Payload pld; MacPacket response; unsigned long* frame; unsigned long s0, m1, m2; pld = macGetPayload(packet); frame = (unsigned long*) payGetData(pld); s0 = frame[0]; // Read requester time of flight m1 = packet->timestamp + sclockGetOffsetTicks(); // Read local time of reception response = radioRequestPacket(12); // Sending 3 longs if(response == NULL) { return; } macSetDestAddr(response, macGetSrcAddr(packet)); macSetDestPan(response, macGetSrcPan(packet)); pld = macGetPayload(response); // Create response packet paySetType(pld, CMD_CLOCK_UPDATE_RESPONSE); paySetData(pld, 4, (unsigned char*) &s0); payAppendData(pld, 4, 4, (unsigned char*) & m1); // Empty TX queue to minimize time of flight error while(!radioTxQueueEmpty()) { radioProcess(); } m2 = sclockGetGlobalTicks(); // Get approximate time of flight payAppendData(pld, 8, 4, (unsigned char*) & m2); while(!radioEnqueueTxPacket(response)) { radioProcess(); } radioProcess(); }
int main (void) { unsigned int i; /* Initialization */ SetupClock(); SetupPorts(); batSetup(); cmdSetup(); mcSetup(); SetupADC(); SwitchClocks(); sclockSetup(); radioInit(TXPQ_MAX_SIZE, RXPQ_MAX_SIZE); radioSetChannel(MY_CHAN); radioSetSrcPanID(PAN_ID); radioSetSrcAddr(SRC_ADDR); dfmemSetup(); camSetup(); cambuffSetup(); gyroSetup(); cmdResetSettings(); for (i = 0; i < 6; i++) { LED_GREEN = ~LED_GREEN; delay_ms(50); LED_RED = ~LED_RED; delay_ms(50); LED_ORANGE = ~LED_ORANGE; delay_ms(50); } LED_GREEN = 0; LED_RED = 0; LED_ORANGE = 0; /* Program */ while (1) { cmdHandleRadioRxBuffer(); radioProcess(); } }
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; }
void __attribute__((interrupt, no_auto_psv)) _T6Interrupt(void) { radioProcess(); _T6IF = 0; }
int main(void) { //wakeTime = 0; //dcCounter = 0; // Processor Initialization SetupClock(); SwitchClocks(); SetupPorts(); sclockSetup(); LED_1 = 0; LED_2 = 0; LED_3 = 0; cmdSetup(); radioInit(RADIO_TXPQ_MAX_SIZE, RADIO_RXPQ_MAX_SIZE); radioSetChannel(RADIO_CHANNEL); radioSetSrcPanID(RADIO_PAN_ID); radioSetSrcAddr(RADIO_SRC_ADDR); dfmemSetup(); uint64_t id = dfmemGetUnqiueID(); telemSetup(); //Timer 5, HW priority 4 mpuSetup(); imuSetup(); //Timer 4, HW priority 3 tiHSetup(); adcSetup(); //AMS Encoders //encSetup(); //"Open Loop" vibration & jitter generator, AP 2014 //olVibeSetup(); legCtrlSetup(); //Timer 1, HW priority 5 steeringSetup(); //Timer 5, HW priority 4 //Tail control is a special case //tailCtrlSetup(); //Camera is untested with current code base, AP 12/6/2012 //ovcamSetup(); LED_RED = 1; //Red is use an "alive" indicator LED_GREEN = 0; LED_YELLOW = 0; //Radio startup verification //if (phyGetState() == 0x16) { // LED_GREEN = 1; //} //Sleeping and low power options //_VREGS = 1; //gyroSleep(); /////FUNCTION TEST, NOT FOR PRODUCTION //olVibeStart(); //////////////////// while (1) { cmdHandleRadioRxBuffer(); radioProcess(); } }