void deca_sleep(unsigned int time_ms) { /* This assumes that the tick has a period of exactly one millisecond. See CLOCKS_PER_SEC define. */ unsigned long end = portGetTickCount() + time_ms; while ((signed long)(portGetTickCount() - end) <= 0) ; }
void sleep_mode (data_t *data) { //Set Timer data->timer = portGetTickCount() + data->tagSleepTime_ms; //set timeout time data->timer_en = 1; //start timer data->stoptimer = 0 ; //clear the flag - timer can run if instancetimer_en set (set above) data->previous_state = data->current_state; data->current_state = INIT_STATE; }
void rx_fail(data_t *data) { //Log the problem and depending on it change STATE APPROPRIATLY data->timer = portGetTickCount() + data->tagSleepTime_ms; //set timeout time data->timer_en = 1; //start timer data->stoptimer = 0 ; //clear the flag - timer can run if instancetimer_en set (set above) data->previous_state = data->current_state; data->current_state = INIT_STATE; }
/* Return a clock that ticks at CLOCKS_PER_SEC Hz. */ clock_t _times_r(void *reent, struct tms * tp) { clock_t timeval = portGetTickCount(); //time32_incr; if (tp) { tp->tms_utime = timeval; /* user time */ tp->tms_stime = 0; /* system time */ tp->tms_cutime = 0; /* user time, children */ tp->tms_cstime = 0; /* system time, children */ } return timeval; };
void send_final (data_t *data) { //We have received a message handle it // Prepare answer //send message exactly X time after we received it //Set Timer data->timer = portGetTickCount() + data->txTimeouts; //set timeout time data->timer_en = 1; //start timer data->stoptimer = 0 ; //clear the flag - timer can run if instancetimer_en set (set above) data->previous_state = data->current_state; data->current_state = TX_FINAL_STATE; }
void send_poll (data_t *data) { //Prepare message data->msg.seqNum = data->frame_sn++; data->msg.panID[0] = (data->panid) & 0xff; data->msg.panID[1] = data->panid >> 8; data->msg.fctCode = SDSTWR_FCT_CODE_POLL; //No late information as it's a poll data->psduLength = HEADER_LENGTH + MESSAGE_DATA_POLL + CRC_LENGTH; dwt_writetxfctrl(data->psduLength, 0); //if the response is expected there is a 1ms timeout to stop RX if no response (ACK or other frame) coming dwt_setrxtimeout(data->fwtoTime_sy); //units are us - wait for 215us after RX on //use delayed rx on (wait4resp timer) //dwt_setrxaftertxdelay(inst->fixedReplyDelay_sy); //units are ~us - wait for wait4respTIM before RX on (delay RX) dwt_writetxdata(data->psduLength, (uint8 *) &data->msg, 0) ; // write the poll frame data //start TX of frame dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //this is platform dependent - only program if DW EVK/EVB dwt_setleds(1); //MP bug - TX antenna delay needs reprogramming as it is not preserved dwt_settxantennadelay(data->txantennaDelay) ; //Send Message as soon as possible data->timer = portGetTickCount() + data->txTimeouts; //set timeout time data->timer_en = 1; //start timer data->stoptimer = 0 ; //clear the flag - timer can run if instancetimer_en set (set above) data->previous_state = data->current_state; data->current_state = TX_POLL_STATE; /* set new state, if necessary */ }
// ------------------------------------------------------------------------------------------------------------------- // // the main instance state machine (all the instance modes Tag, Anchor or Listener use the same statemachine....) // // ------------------------------------------------------------------------------------------------------------------- // int testapprun(instance_data_t *inst, int message) { switch (inst->testAppState) { case TA_INIT : #if defined(DEBUG) printf("TA_INIT") ; #endif switch (inst->mode) { case TAG: { uint16 sleep_mode = 0; dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ack frames; dwt_setpanid(inst->panID); memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); dwt_seteui(inst->eui64); //set source address inst->newRangeTagAddress = inst->instanceAddress16 ; dwt_setaddress16(inst->instanceAddress16); //Start off by Sleeping 1st -> set instToSleep to TRUE inst->nextState = TA_TXPOLL_WAIT_SEND; inst->testAppState = TA_TXE_WAIT; inst->instToSleep = TRUE ; inst->rangeNum = 0; inst->tagSleepCorrection = 0; sleep_mode = (DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV); if((dwt_getldotune() != 0)) //if we need to use LDO tune value from OTP kick it after sleep sleep_mode |= DWT_LOADLDO; if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet sleep_mode |= DWT_LOADOPSET; #if (DEEP_SLEEP == 1) dwt_configuresleep(sleep_mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings) #endif instanceconfigframeheader16(inst); inst->instanceWakeTime = portGetTickCount(); } break; case ANCHOR: { memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); dwt_seteui(inst->eui64); dwt_setpanid(inst->panID); //set source address inst->shortAdd_idx = (inst->instanceAddress16 & 0x3) ; dwt_setaddress16(inst->instanceAddress16); //if address = 0x8000 if(inst->instanceAddress16 == GATEWAY_ANCHOR_ADDR) { inst->gatewayAnchor = TRUE; } dwt_enableframefilter(DWT_FF_NOTYPE_EN); //allow data, ack frames; // First time anchor listens we don't do a delayed RX dwt_setrxaftertxdelay(0); //change to next state - wait to receive a message inst->testAppState = TA_RXE_WAIT ; dwt_setrxtimeout(0); dwt_setpreambledetecttimeout(0); instanceconfigframeheader16(inst); } break; case LISTENER: { dwt_enableframefilter(DWT_FF_NOTYPE_EN); //disable frame filtering dwt_setrxaftertxdelay(0); //no delay of turning on of RX dwt_setrxtimeout(0); dwt_setpreambledetecttimeout(0); //change to next state - wait to receive a message inst->testAppState = TA_RXE_WAIT ; } break ; // end case TA_INIT default: break; } break; // end case TA_INIT case TA_SLEEP_DONE : { event_data_t* dw_event = instance_getevent(10); //clear the event from the queue // waiting for timout from application to wakup IC if (dw_event->type != DWT_SIG_RX_TIMEOUT) { // if no pause and no wake-up timeout continu waiting for the sleep to be done. inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout break; } inst->done = INST_NOT_DONE_YET; inst->instToSleep = FALSE ; inst->testAppState = inst->nextState; inst->nextState = 0; //clear inst->instanceWakeTime = portGetTickCount(); // Record the time count when we wake-up #if (DEEP_SLEEP == 1) { uint32 x = 0; //wake up device from low power mode //NOTE - in the ARM code just drop chip select for 200us //led_on(LED_PC9); port_SPIx_clear_chip_select(); //CS low instance_data[0].dwIDLE = 0; //reset DW1000 IDLE flag setup_DW1000RSTnIRQ(1); //enable RSTn IRQ Sleep(2); //200 us to wake up - need 2 as Sleep(1) is ~ 175 us //then wait 5ms for DW1000 XTAL to stabilise - instead of wait we wait for RSTn to go high //Sleep(5); Sleep(83); //need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable //when RSTn goes high the DW1000 is in INIT, it will enter IDLE after PLL lock (in 5 us) /*while(instance_data[0].dwIDLE == 0) // this variable will be sent in the IRQ (process_dwRSTn_irq) { //wait for DW1000 to go to IDLE state RSTn pin to go high x++; }*/ setup_DW1000RSTnIRQ(0); //disable RSTn IRQ port_SPIx_set_chip_select(); //CS high //!!! NOTE it takes ~35us for the DW1000 to download AON and lock the PLL and be in IDLE state //do some dummy reads of the dev ID register to make sure DW1000 is in IDLE before setting LEDs x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) /*if(x != DWT_DEVICE_ID) { x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz) }*/ //led_off(LED_PC9); //this is platform dependent - only program if DW EVK/EVB dwt_setleds(1); //MP bug - TX antenna delay needs reprogramming as it is not preserved (only RX) dwt_settxantennadelay(inst->txAntennaDelay) ; //set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM dwt_seteui(inst->eui64); } #else Sleep(3); //to approximate match the time spent in the #if above #endif instancesetantennadelays(); //this will update the antenna delay if it has changed instancesettxpower(); //configure TX power if it has changed } break; case TA_TXE_WAIT : //either go to sleep or proceed to TX a message #if defined(DEBUG) printf("TA_TXE_WAIT\n") ; #endif //if we are scheduled to go to sleep before next transmission then sleep first. if((inst->nextState == TA_TXPOLL_WAIT_SEND) && (inst->instToSleep) //go to sleep before sending the next poll/ starting new ranging exchange ) { inst->rangeNum++; //increment the range number before going to sleep //the app should put chip into low power state and wake up after tagSleepTime_ms time... //the app could go to *_IDLE state and wait for uP to wake it up... inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the Sleep timer countdown inst->testAppState = TA_SLEEP_DONE; { #if (DEEP_SLEEP == 1) //put device into low power mode dwt_entersleep(); //go to sleep #endif //DW1000 gone to sleep - report the received range inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask); inst->rxResponseMaskReport = inst->rxResponseMask; inst->rxResponseMask = 0; inst->newRangeTime = portGetTickCount() ; } } else //proceed to configuration and transmission of a frame { inst->testAppState = inst->nextState; inst->nextState = 0; //clear } break ; // end case TA_TXE_WAIT case TA_TXPOLL_WAIT_SEND : { #if defined(DEBUG) printf("TA_TXPOLL_WAIT_SEND\n") ; #endif inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; //copy new range number inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_POLL : RTLS_DEMO_MSG_ANCH_POLL; //message function code (specifies if message is a poll, response or other...) inst->psduLength = (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); inst->msg_f.seqNum = inst->frameSN++; //copy sequence number and then increment inst->msg_f.sourceAddr[0] = inst->eui64[0]; //copy the address inst->msg_f.sourceAddr[1] = inst->eui64[1]; //copy the address inst->msg_f.destAddr[0] = 0xff; //set the destination address (broadcast == 0xffff) inst->msg_f.destAddr[1] = 0xff; //set the destination address (broadcast == 0xffff) dwt_writetxdata(inst->psduLength, (uint8 *) &inst->msg_f, 0) ; // write the frame data //set the delayed rx on time (the response message will be sent after this delay (from A0)) dwt_setrxaftertxdelay((uint32)RX_RESPONSE1_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX) if(inst->mode == TAG) { inst->rxResps[inst->rangeNum] = 0; //reset the number of received responses inst->responseTO = MAX_ANCHOR_LIST_SIZE; //expecting 4 responses dwt_setrxtimeout((uint16)inst->fwtoTime_sy * MAX_ANCHOR_LIST_SIZE); //configure the RX FWTO } else { inst->rxResps[inst->rangeNumAnc] = 0; //reset number of responses inst->responseTO = NUM_EXPECTED_RESPONSES_ANC0; //2 responses A1, A2 dwt_setrxtimeout((uint16)inst->fwtoTime_sy * (NUM_EXPECTED_RESPONSES_ANC0)); //units are } inst->rxResponseMask = 0; //reset/clear the mask of received responses when tx poll inst->rxResponseMaskAnc = 0; inst->wait4ack = DWT_RESPONSE_EXPECTED; //response is expected - automatically enable the receiver dwt_writetxfctrl(inst->psduLength, 0); //write frame control dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //transmit the frame inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation inst->previousState = TA_TXPOLL_WAIT_SEND ; inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above) } break; case TA_TXFINAL_WAIT_SEND : { //the final has the same range number as the poll (part of the same ranging exchange) inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; //the mask is sent so the anchors know whether the response RX time is valid inst->msg_f.messageData[VRESP] = (inst->mode == TAG) ? inst->rxResponseMask : inst->rxResponseMaskAnc; inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_FINAL : RTLS_DEMO_MSG_ANCH_FINAL; //message function code (specifies if message is a poll, response or other...) inst->psduLength = (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); inst->msg_f.seqNum = inst->frameSN++; dwt_writetxdata(inst->psduLength, (uint8 *) &inst->msg_f, 0) ; // write the frame data inst->wait4ack = 0; //clear the flag not using wait for response as this message ends the ranging exchange if(instancesenddlypacket(inst, DWT_START_TX_DELAYED)) { // initiate the re-transmission if(inst->mode == TAG) { inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time inst->nextState = TA_TXPOLL_WAIT_SEND ; } else { //A0 - failed to send Final //A1 - failed to send Final //go back to RX and behave as anchor instance_backtoanchor(inst); } break; //exit this switch case... } else { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXFINAL_WAIT_SEND; } if(inst->mode == TAG) { inst->instToSleep = TRUE ; } inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above) } break; case TA_TX_WAIT_CONF : #if defined(DEBUG) printf("TA_TX_WAIT_CONF %d m%d states %08x %08x\n", inst->previousState, message, dwt_read32bitreg(0x19), dwt_read32bitreg(0x0f)) ; #endif { event_data_t* dw_event = instance_getevent(11); //get and clear this event //NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK //this happens because if polling the ISR the RX event will be processed 1st and then the TX event //thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it. if(dw_event->type != DWT_SIG_TX_DONE) //wait for TX done confirmation { if(dw_event->type != 0) { if(dw_event->type == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK) { #if defined(DEBUG) printf("RX timeout in TA_TX_WAIT_CONF (%d)\n", inst->previousState); #endif //we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed inst->gotTO = 1; } else { inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; } } inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; break; } inst->done = INST_NOT_DONE_YET; if(inst->previousState == TA_TXFINAL_WAIT_SEND) { if(inst->mode == TAG) { inst->testAppState = TA_TXE_WAIT ; inst->nextState = TA_TXPOLL_WAIT_SEND ; break; } else { instance_backtoanchor(inst); } } else if (inst->gotTO == 1) //timeout { #if defined(DEBUG) printf("got TO in TA_TX_WAIT_CONF\n"); #endif inst_processrxtimeout(inst); inst->gotTO = 0; inst->wait4ack = 0 ; //clear this break; } else { inst->txu.txTimeStamp = dw_event->timeStamp; if(inst->previousState == TA_TXPOLL_WAIT_SEND) { uint64 tagCalculatedFinalTxTime ; // Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime if(inst->mode == TAG) { tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelay) & MASK_TXDTS; } else //for anchor make the final half the delay ..... (this is ok, as A0 awaits 2 responses) { tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelayAnc) & MASK_TXDTS; } inst->delayedReplyTime = tagCalculatedFinalTxTime >> 8; //high 32-bits // Calculate Time Final message will be sent and write this field of Final message // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by // zeroing its low 9 bits, and then having the TX antenna delay added // getting antenna delay from the device and add it to the Calculated TX Time tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay; tagCalculatedFinalTxTime &= MASK_40BIT; // Write Calculated TX time field of Final message memcpy(&(inst->msg_f.messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5); // Write Poll TX time field of Final message memcpy(&(inst->msg_f.messageData[PTXT]), (uint8 *)&inst->txu.tagPollTxTime, 5); //change the w4r for the second and remaining anchors to 50 us //dwt_setrxaftertxdelay((uint32)RX_RESPONSEX_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX) } if(inst->previousState == TA_TXRESPONSE_SENT_TORX) { inst->previousState = TA_TXRESPONSE_WAIT_SEND ; } inst->testAppState = TA_RXE_WAIT ; // After sending, tag expects response/report, anchor waits to receive a final/new poll message = 0; //fall into the next case (turn on the RX) } } //break ; // end case TA_TX_WAIT_CONF case TA_RXE_WAIT : #if defined(DEBUG) printf("TA_RXE_WAIT\n") ; #endif { if(inst->wait4ack == 0) //if this is set the RX will turn on automatically after TX { //turn RX on dwt_rxenable(DWT_START_RX_IMMEDIATE) ; // turn RX on, without delay } else { inst->wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto } if (inst->mode != LISTENER) { inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO } inst->testAppState = TA_RX_WAIT_DATA; // let this state handle it // end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately. if(message == 0) break; } case TA_RX_WAIT_DATA : // Wait RX data #if defined(DEBUG) printf("TA_RX_WAIT_DATA %d\n", message) ; #endif switch (message) { //if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used case DWT_SIG_RX_OKAY : { event_data_t* dw_event = instance_getevent(15); //get and clear this event uint8 srcAddr[8] = {0,0,0,0,0,0,0,0}; uint8 dstAddr[8] = {0,0,0,0,0,0,0,0}; int fcode = 0; int fn_code = 0; //int srclen = 0; //int fctrladdr_len; uint8 tof_idx = 0; uint8 *messageData; inst->stopTimer = 0; //clear the flag, as we have received a message // handle 16 and 64 bit source and destination addresses switch(dw_event->msgu.frame[1] & 0xCC) { case 0xCC: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ll.destAddr[0]), ADDR_BYTE_SIZE_L); fn_code = dw_event->msgu.rxmsg_ll.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_ll.messageData[0]; //srclen = ADDR_BYTE_SIZE_L; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_L; break; case 0xC8: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_sl.sourceAddr[0]), ADDR_BYTE_SIZE_L); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_sl.destAddr[0]), ADDR_BYTE_SIZE_S); fn_code = dw_event->msgu.rxmsg_sl.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_sl.messageData[0]; //srclen = ADDR_BYTE_SIZE_L; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS; break; case 0x8C: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ls.destAddr[0]), ADDR_BYTE_SIZE_L); fn_code = dw_event->msgu.rxmsg_ls.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_ls.messageData[0]; //srclen = ADDR_BYTE_SIZE_S; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS; break; case 0x88: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S); memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ss.destAddr[0]), ADDR_BYTE_SIZE_S); fn_code = dw_event->msgu.rxmsg_ss.messageData[FCODE]; messageData = &dw_event->msgu.rxmsg_ss.messageData[0]; //srclen = ADDR_BYTE_SIZE_S; //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_S; break; } if((inst->instToSleep == FALSE) && (inst->mode == LISTENER))//update received data, and go back to receiving frames { //do something with message data (e.g. could extract any ToFs and print them) inst->testAppState = TA_RXE_WAIT ; // wait for next frame dwt_setrxaftertxdelay(0); } else { //process ranging messages fcode = fn_code; tof_idx = srcAddr[0] & 0x3 ; switch(fcode) { case RTLS_DEMO_MSG_ANCH_POLL: case RTLS_DEMO_MSG_TAG_POLL: { inst->tagPollRxTime = dw_event->timeStamp ; //save Poll's Rx time if(fcode == RTLS_DEMO_MSG_TAG_POLL) //got poll from Tag { inst->rangeNumA[srcAddr[0]&0x7] = messageData[POLL_RNUM]; //when anchor receives a poll, we need to remember the new range number } else //got poll from Anchor (initiator) { inst->rangeNumAAnc[tof_idx] = messageData[POLL_RNUM]; //when anchor receives poll from another anchor - save the range number } if (A1_ANCHOR_ADDR == inst->instanceAddress16) //this is A1 { if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) //poll is from A0 { //configure the time A1 will poll A2 (it should be in half slot time from now) inst->a1SlotTime = dw_event->uTimeStamp + (inst->slotPeriod); //inst->instanceTimerEn = 1; - THIS IS ENABLED BELOW AFTER FINAL // - means that if final is not received then A1 will not range to A2 } } //the response has been sent - await TX done event if(dw_event->type_pend == DWT_SIG_TX_PENDING) { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXRESPONSE_SENT_POLLRX ; //wait for TX confirmation of sent response } //already re-enabled the receiver else if (dw_event->type_pend == DWT_SIG_RX_PENDING) { //stay in RX wait for next frame... //RX is already enabled... inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame } else //the DW1000 is idle (re-enable from the application level) { //stay in RX wait for next frame... inst->testAppState = TA_RXE_WAIT ; // wait for next frame } } break; //RTLS_DEMO_MSG_TAG_POLL case RTLS_DEMO_MSG_ANCH_RESP2: case RTLS_DEMO_MSG_ANCH_RESP: { uint8 currentRangeNum = (messageData[TOFRN] + 1); //current = previous + 1 if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) //if response from gateway then use the correction factor { if(inst->mode == TAG) { // casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods inst->tagSleepCorrection = (int16) (((uint16) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]); inst->tagSleepRnd = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore } } //the response has been sent - await TX done event if(dw_event->type_pend == DWT_SIG_TX_PENDING) //anchor received response from anchor ID - 1 so is sending it's response now back to tag { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXRESPONSE_SENT_RESPRX ; //wait for TX confirmation of sent response } //already re-enabled the receiver else if(dw_event->type_pend == DWT_SIG_RX_PENDING) { // stay in TA_RX_WAIT_DATA - receiver is already enabled. } //DW1000 idle - send the final else //if(dw_event->type_pend == DWT_SIG_DW_IDLE) { if(((TAG == inst->mode) && (inst->rxResponseMask & 0x1)) //if A0's response received send the final || ((A1_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x4)) || ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x2)) ) //if A1's response received { inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final } else //go to sleep { if(TAG == inst->mode) { inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time inst->nextState = TA_TXPOLL_WAIT_SEND ; inst->instToSleep = TRUE; } else { instance_backtoanchor(inst); } } } /*else { //stay in RX wait for next frame... inst->testAppState = TA_RXE_WAIT ; // wait for next frame }*/ if(fcode == RTLS_DEMO_MSG_ANCH_RESP) //tag to anchor mode { if(currentRangeNum == inst->rangeNum) //these are the previous ranges... { //copy the ToF and put into array (array holds last 4 ToFs) memcpy(&inst->tofArray[(srcAddr[0]&0x3)], &(messageData[TOFR]), 4); //check if the ToF is valid, this makes sure we only report valid ToFs //e.g. consider the case of reception of response from anchor a1 (we are anchor a2) //if a1 got a Poll with previous Range number but got no Final, then the response will have //the correct range number but the range will be INVALID_TOF if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF) { inst->rxResponseMask |= (0x1 << (srcAddr[0]&0x3)); } } else { if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF) { inst->tofArray[(srcAddr[0]&0x3)] = INVALID_TOF; } } } else //anchor to anchor (only gateway processes anchor to anchor ToFs) { //report the correct set of ranges (ranges from anchors A1, A2 need to match owns range number) if((inst->gatewayAnchor)&&(currentRangeNum == inst->rangeNumAnc)) //these are the previous ranges... { inst->rangeNumAAnc[0] = inst->rangeNumAnc ; //once A0 receives A2's response then it can report the 3 ToFs. if(inst->rxResps[inst->rangeNumAnc] == 3) //if(A2_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) { //copy the ToF and put into array, the array should have 3 ToFs A0-A1, A0-A2 and A1-A2 memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); //calculate all anchor - anchor ranges... and report inst->newRange = instance_calcranges(&inst->tofArrayAnc[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_A2A, &inst->rxResponseMaskAnc); inst->rxResponseMaskReport = inst->rxResponseMaskAnc; inst->rxResponseMaskAnc = 0; inst->newRangeTime = dw_event->uTimeStamp ; } else { //copy the ToF and put into array (array holds last 4 ToFs) memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); } } } } break; //RTLS_DEMO_MSG_ANCH_RESP case RTLS_DEMO_MSG_ANCH_FINAL: case RTLS_DEMO_MSG_TAG_FINAL: { int64 Rb, Da, Ra, Db ; uint64 tagFinalTxTime = 0; uint64 tagFinalRxTime = 0; uint64 tagPollTxTime = 0; uint64 anchorRespRxTime = 0; uint64 tof = INVALID_TOF; double RaRbxDaDb = 0; double RbyDb = 0; double RayDa = 0; uint8 validResp = messageData[VRESP]; uint8 index = RRXT0 + 5*(inst->shortAdd_idx); if((RTLS_DEMO_MSG_TAG_FINAL == fcode) && (inst->rangeNumA[srcAddr[0]&0x7] != messageData[POLL_RNUM])) //Final's range number needs to match Poll's or else discard this message { inst->testAppState = TA_RXE_WAIT ; // wait for next frame break; } if((RTLS_DEMO_MSG_ANCH_FINAL == fcode) && (((inst->rangeNumAAnc[tof_idx] != messageData[POLL_RNUM]) //Final's range number needs to match Poll's or else discard this message || inst->gatewayAnchor) //gateway can ignore the Final (from A1 to A2 exchange) || (A3_ANCHOR_ADDR == inst->instanceAddress16))) //A3 does not care about Final from A1 or A0 { inst->testAppState = TA_RXE_WAIT ; // wait for next frame break; } if (A1_ANCHOR_ADDR == inst->instanceAddress16) //this is A1 { if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8)))) //final is from A0 { //ENABLE TIMER ONLY IF FINAL RECEIVED inst->instanceTimerEn = 1; } } //output data over USB... inst->newRangeAncAddress = inst->instanceAddress16; //if we got the final, maybe the tag did not get our response, so //we can use other anchors responses/ToF if there are any.. and output.. //but we cannot calculate new range if(((validResp & (0x1<<(inst->shortAdd_idx))) != 0)) { // time of arrival of Final message tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time /* #if defined(DEBUG) printf("FinalRx Timestamp: %4.15e\n", convertdevicetimetosecu(dw_event.timeStamp)); #endif */ inst->delayedReplyTime = 0 ; // times measured at Tag extracted from the message buffer // extract 40bit times memcpy(&tagPollTxTime, &(messageData[PTXT]), 5); memcpy(&anchorRespRxTime, &(messageData[index]), 5); memcpy(&tagFinalTxTime, &(messageData[FTXT]), 5); // poll response round trip delay time is calculated as // (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime) Ra = (int64)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT); Db = (int64)((inst->txu.anchorRespTxTime - inst->tagPollRxTime) & MASK_40BIT); // response final round trip delay time is calculated as // (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime) Rb = (int64)((tagFinalRxTime - inst->txu.anchorRespTxTime) & MASK_40BIT); Da = (int64)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT); RaRbxDaDb = (((double)Ra))*(((double)Rb)) - (((double)Da))*(((double)Db)); RbyDb = ((double)Rb + (double)Db); RayDa = ((double)Ra + (double)Da); tof = (int32) ( RaRbxDaDb/(RbyDb + RayDa) ); } //tag to anchor ranging if(RTLS_DEMO_MSG_TAG_FINAL == fcode) { inst->newRangeTagAddress = srcAddr[0] + ((uint16) srcAddr[1] << 8); //time-of-flight inst->tof[inst->newRangeTagAddress & 0x7] = tof; //calculate all tag - anchor ranges... and report inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask); inst->rxResponseMaskReport = inst->rxResponseMask; //copy the valid mask to report inst->rxResponseMask = 0; //we have our range - update the own mask entry... if(tof != INVALID_TOF) //check the last ToF entry is valid and copy into the current array { setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array inst->rxResponseMask = (0x1 << inst->shortAdd_idx); inst->tofArray[inst->shortAdd_idx] = tof; } inst->newRangeTime = dw_event->uTimeStamp ; } else //anchor to anchor ranging { inst->newRangeTagAddress = srcAddr[0] + ((uint16) srcAddr[1] << 8); //time-of-flight inst->tofAnc[tof_idx] = tof; } //reset the response count if(inst->rxResps[inst->rxRespsIdx] >= 0) { inst->rxResps[inst->rxRespsIdx] = -1 * inst->rxResps[inst->rxRespsIdx]; if(inst->rxResps[inst->rxRespsIdx] == 0) //as A0 will have this as 0 when ranging to A1 inst->rxResps[inst->rxRespsIdx] = -1 ; } instancesetantennadelays(); //this will update the antenna delay if it has changed instancesettxpower(); // configure TX power if it has changed inst->testAppState = TA_RXE_WAIT ; // wait for next frame } break; //RTLS_DEMO_MSG_TAG_FINAL default: { //only enable receiver when not using double buffering inst->testAppState = TA_RXE_WAIT ; // wait for next frame dwt_setrxaftertxdelay(0); } break; } //end switch (fcode) if(dw_event->msgu.frame[0] & 0x20) { //as we only pass the received frame with the ACK request bit set after the ACK has been sent instance_getevent(16); //get and clear the ACK sent event } } //end else } break ; //end of DWT_SIG_RX_OKAY case DWT_SIG_RX_TIMEOUT : { event_data_t* dw_event = instance_getevent(17); //get and clear this event #if defined(DEBUG) printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ; #endif //Anchor can time out and then need to send response - so will be in TX pending if(dw_event->type_pend == DWT_SIG_TX_PENDING) { inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation inst->previousState = TA_TXRESPONSE_SENT_TORX ; //wait for TX confirmation of sent response } else if(dw_event->type_pend == DWT_SIG_DW_IDLE) //if timed out and back in receive then don't process as timeout { inst_processrxtimeout(inst); } //else if RX_PENDING then wait for next RX event... message = 0; //clear the message as we have processed the event } break ; case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response case 0: default : { if(message) // == DWT_SIG_TX_DONE) { inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; } if(inst->done == INST_NOT_DONE_YET) inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; } break; } break ; // end case TA_RX_WAIT_DATA default: #if defined(DEBUG) printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ; #endif break; } // end switch on testAppState