// Called at the beginning of each listening window for transmitting to // the tag. static void ranging_listening_window_task () { // Check if we are done transmitting to the tag. // Ideally we never get here, as an ack from the tag will cause us to stop // cycling through listening windows and put us back into a ready state. if (_ranging_listening_window_num == NUM_RANGING_CHANNELS) { // Go back to IDLE _state = ASTATE_IDLE; // Stop the timer for the window timer_stop(_anchor_timer); // Restart being an anchor oneway_anchor_start(); } else { // Setup the channel and antenna settings oneway_set_ranging_listening_window_settings(ANCHOR, _ranging_listening_window_num, pp_anc_final_pkt.final_antenna); // Prepare the outgoing packet to send back to the // tag with our TOAs. pp_anc_final_pkt.ieee154_header_unicast.seqNum++; const uint16_t frame_len = sizeof(struct pp_anc_final); // const uint16_t frame_len = sizeof(struct pp_anc_final) - (sizeof(uint64_t)*NUM_RANGING_BROADCASTS); dwt_writetxfctrl(frame_len, 0); // Pick a slot to respond in. Generate a random number and mod it // by the number of slots uint8_t slot_num = ranval(&_prng_state) % (_ranging_operation_config.anchor_reply_window_in_us / _ranging_operation_config.anchor_reply_slot_time_in_us); // Come up with the time to send this packet back to the // tag based on the slot we picked. uint32_t delay_time = dwt_readsystimestamphi32() + DW_DELAY_FROM_US(ANC_FINAL_INITIAL_DELAY_HACK_VALUE + (slot_num*_ranging_operation_config.anchor_reply_slot_time_in_us)); delay_time &= 0xFFFFFFFE; // Record the outgoing time in the packet. Do not take calibration into // account here, as that is done on all of the RX timestamps. pp_anc_final_pkt.dw_time_sent = (((uint64_t) delay_time) << 8); // Set the packet to be transmitted later. dwt_setdelayedtrxtime(delay_time); // Send the response packet // TODO: handle if starttx errors. I'm not sure what to do about it, // other than just wait for the next slot. dwt_starttx(DWT_START_TX_DELAYED); dwt_settxantennadelay(DW1000_ANTENNA_DELAY_TX); dwt_writetxdata(frame_len, (uint8_t*) &pp_anc_final_pkt, 0); _ranging_listening_window_num++; } }
// Called at the beginning of each listening window for transmitting to // the tag. static void ranging_listening_window_task () { // Check if we are done transmitting to the tag. // Ideally we never get here, as an ack from the tag will cause us to stop // cycling through listening windows and put us back into a ready state. if (_ranging_listening_window_num == NUM_RANGING_CHANNELS) { // Go back to IDLE _state = ASTATE_IDLE; // Stop the timer for the window timer_stop(_ranging_broadcast_timer); } else { // Setup the channel and antenna settings dw1000_set_ranging_listening_window_settings(ANCHOR, _ranging_listening_window_num, pp_anc_final_pkt.final_antenna, FALSE); // Prepare the outgoing packet to send back to the // tag with our TOAs. pp_anc_final_pkt.ieee154_header_unicast.seqNum++; const uint16_t frame_len = sizeof(struct pp_anc_final); dwt_writetxfctrl(frame_len, 0); // Pick a slot to respond in. Generate a random number and mod it // by the number of slots uint8_t slot_num = ranval(&_prng_state) % (_ranging_operation_config.anchor_reply_window_in_us / _ranging_operation_config.anchor_reply_slot_time_in_us); // Come up with the time to send this packet back to the // tag based on the slot we picked. uint32_t delay_time = dwt_readsystimestamphi32() + DW_DELAY_FROM_US(ANC_FINAL_INITIAL_DELAY_HACK_VALUE + (slot_num*_ranging_operation_config.anchor_reply_slot_time_in_us)); delay_time &= 0xFFFFFFFE; pp_anc_final_pkt.dw_time_sent = delay_time; dwt_setdelayedtrxtime(delay_time); // Send the response packet int err = dwt_starttx(DWT_START_TX_DELAYED); dwt_settxantennadelay(DW1000_ANTENNA_DELAY_TX); dwt_writetxdata(frame_len, (uint8_t*) &pp_anc_final_pkt, 0); _ranging_listening_window_num++; } }
int instancesenddlypacket(instance_data_t *inst, int delayedTx) { int result = 0; dwt_writetxfctrl(inst->psduLength, 0); if(delayedTx == DWT_START_TX_DELAYED) { dwt_setdelayedtrxtime(inst->delayedReplyTime) ; //should be high 32-bits of delayed TX TS } //begin delayed TX of frame if (dwt_starttx(delayedTx | inst->wait4ack)) // delayed start was too late { result = 1; //late/error inst->lateTX++; } else { inst->timeofTx = portGetTickCnt(); inst->monitor = 1; } return result; // state changes }
bool DWM1000_Anchor::dispatch(Msg& msg) { PT_BEGIN() PT_WAIT_UNTIL(msg.is(0, SIG_INIT)); init(); while (true) { WAIT_POLL: { dwt_setrxtimeout(0); /* Clear reception timeout to start next ranging process. */ dwt_rxenable(0); /* Activate reception immediately. */ // dwt_setinterrupt(DWT_INT_RFCG, 1); // enable RXD interrupt while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */ timeout(1000);/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */ clearInterrupt(); PT_YIELD_UNTIL(timeout() || isInterruptDetected()); status_reg = _status_reg; LOG<< HEX << " status reg.:" << status_reg << " ,interrupts : " << interruptCount << FLUSH; status_reg = dwt_read32bitreg(SYS_STATUS_ID); LOG<< HEX << " IRQ pin : " << digitalRead(D2) << " status_reg DWM1000 " << status_reg << FLUSH;// PULL LOW if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)) break; } } ///____________________________________________________________________________ if (status_reg & SYS_STATUS_RXFCG) { LOG<< " $ "<<FLUSH; uint32 frame_len; dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); /* Clear good RX frame event in the DW1000 status register. */ /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023; if (frame_len <= RX_BUFFER_LEN) { dwt_readrxdata(rx_buffer, frame_len, 0); } /* Check that the frame is a poll sent by "DS TWR initiator" example. * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */ rx_buffer[ALL_MSG_SN_IDX] = 0; if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) { LOG<< " $$ "<<FLUSH; uint32 resp_tx_time; poll_rx_ts = get_rx_timestamp_u64(); /* Retrieve poll reception timestamp. */ /* Set send time for response. See NOTE 8 below. */ resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8; dwt_setdelayedtrxtime(resp_tx_time); /* Set expected delay and timeout for final message reception. */ dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS); dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS); /* Write and send the response message. See NOTE 9 below.*/ tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb; dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0); dwt_writetxfctrl(sizeof(tx_resp_msg), 0); dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED); /* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout. * See NOTE 7 below. */ // while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */ timeout(10); dwt_setinterrupt(DWT_INT_RFCG, 1);// enable clearInterrupt(); // PT_YIELD_UNTIL(timeout() || isInterruptDetected()); status_reg = dwt_read32bitreg(SYS_STATUS_ID); // status_reg = _status_reg; LOG<< HEX << " status reg2:" << status_reg << FLUSH; // if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)) // break; // } // while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) // { }; /* Increment frame sequence number after transmission of the response message (modulo 256). */ frame_seq_nb++; if (status_reg & SYS_STATUS_RXFCG) { LOG<< " $$$ "<<FLUSH; /* Clear good RX frame event and TX frame sent in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg( RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; if (frame_len <= RX_BUF_LEN) { dwt_readrxdata(rx_buffer, frame_len, 0); } /* Check that the frame is a final message sent by "DS TWR initiator" example. * As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */ rx_buffer[ALL_MSG_SN_IDX] = 0; if (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN) == 0) { uint32 poll_tx_ts, resp_rx_ts, final_tx_ts; uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32; double Ra, Rb, Da, Db; int64 tof_dtu; /* Retrieve response transmission and final reception timestamps. */ resp_tx_ts = get_tx_timestamp_u64(); final_rx_ts = get_rx_timestamp_u64(); /* Get timestamps embedded in the final message. */ final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts); final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts); final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts); /* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */ poll_rx_ts_32 = (uint32) poll_rx_ts; resp_tx_ts_32 = (uint32) resp_tx_ts; final_rx_ts_32 = (uint32) final_rx_ts; Ra = (double) (resp_rx_ts - poll_tx_ts); Rb = (double) (final_rx_ts_32 - resp_tx_ts_32); Da = (double) (final_tx_ts - resp_rx_ts); Db = (double) (resp_tx_ts_32 - poll_rx_ts_32); tof_dtu = (int64) ((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db)); tof = tof_dtu * DWT_TIME_UNITS; distance = tof * SPEED_OF_LIGHT; /* Display computed distance on LCD. */ // char dist_str[20]; // sprintf(dist_str,"%3.2f", distance); // lcd_display_str(dist_str); LOG<< " distance : " << (float)distance << "m. " << FLUSH; } } else { /* Clear RX error events in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); } } } else {
// ------------------------------------------------------------------------------------------------------------------- // // the main instance state machine (for Tag instance mode only!) // // ------------------------------------------------------------------------------------------------------------------- // int testapprun_tf(instance_data_t *inst, int message) { switch (inst->testAppState) { case TA_INIT : // printf("TA_INIT") ; switch (inst->mode) { case TAG: { int mode = 0; dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data and ack frames; inst->frameFilteringEnabled = 1 ; dwt_setpanid(inst->panid); dwt_seteui(inst->eui64); inst->msg_f.panID[0] = (inst->panid) & 0xff; inst->msg_f.panID[1] = inst->panid >> 8; #if (DR_DISCOVERY == 1) inst->mode = TAG_TDOA ; inst->testAppState = TA_TXBLINK_WAIT_SEND; memcpy(&inst->blinkmsg.tagID[0], &inst->eui64[0], BLINK_FRAME_SOURCE_ADDRESS); #else inst->testAppState = TA_TXPOLL_WAIT_SEND; #endif //can use RX auto re-enable when not logging/plotting errored frames inst->rxautoreenable = 1; dwt_setautorxreenable(inst->rxautoreenable); //not necessary to auto RX re-enable as the receiver is on for a short time (Tag knows when the response is coming) //disable double buffer for a Tag - not needed.... dwt_setdblrxbuffmode(0); //enable/disable double RX buffer //NOTE - Auto ACK only works if frame filtering is enabled! dwt_enableautoack(ACK_RESPONSE_TIME); //wait for ACK_RESPONSE_TIME symbols (e.g. 5) before replying with the ACK 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 mode |= DWT_LOADLDO; if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet mode |= DWT_LOADOPSET; //NOTE: on the EVK1000 the DEEPSLEEP is not actually putting the DW1000 into full DEEPSLEEP mode as XTAL is kept on dwt_configuresleep(mode, DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings) } break; default: break; } break; // end case TA_INIT case TA_SLEEP_DONE : { event_data_t* dw_event = instance_getevent(20); //clear the event from the queue // waiting for timout from application to wakup IC if (dw_event->type != DWT_SIG_RX_TIMEOUT) { inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout break; } inst->done = INST_NOT_DONE_YET; inst->instToSleep = 0; inst->testAppState = inst->nextState; inst->nextState = 0; //clear inst->canprintinfo = 0; //wake up from DEEP SLEEP { //wake up device from low power mode //NOTE - in the ARM code just drop chip select for 200us port_SPIx_clear_chip_select(); //CS low instance_data[0].dwIDLE = 0; //reset setup_DW1000RSTnIRQ(1); //enable RSTn IRQ Sleep(1); //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise port_SPIx_set_chip_select(); //CS high #if (DW_IDLE_CHK==1) //Wait (sleep) to give DW1000 time to get to IDLE state Sleep(5); //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(inst->txantennaDelay) ; //set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM /*if((inst->mode == TAG) || (inst->mode == TAG_TDOA)) { dwt_setpanid(inst->panid); dwt_seteui(inst->eui64); }*/ #elif (DW_IDLE_CHK==2) //Use RSTn pin to notify the micro that DW1000 is in IDLE //need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable while(instance_data[0].dwIDLE == 0); //wait for DW1000 to go to IDLE state RSTn pin to go high if(dwt_read32bitreg(0x0) != 0xDECA0130) { //error? int x = 0; } setup_DW1000RSTnIRQ(0); //disable RSTn IRQ #else //need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable while(dwt_read32bitreg(0x0) != 0xDECA0130); //Sleep(2); #endif dwt_entersleepaftertx(0); dwt_setinterrupt(DWT_INT_TFRS, 1); //re-enable the TX/RX interrupts } } break; case TA_TXE_WAIT : //either go to sleep or proceed to TX a message // printf("TA_TXE_WAIT") ; //if we are scheduled to go to sleep before next sending then sleep first. if(((inst->nextState == TA_TXPOLL_WAIT_SEND) || (inst->nextState == TA_TXBLINK_WAIT_SEND)) && (inst->instToSleep) //go to sleep before sending the next poll ) { //the app should put chip into low power state and wake up in 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 TagTimeoutTimer (instancetimer) inst->testAppState = TA_SLEEP_DONE; if(inst->nextState == TA_TXBLINK_WAIT_SEND) inst->canprintinfo = 1; //put device into low power mode dwt_entersleep(); //go to sleep } 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_TXBLINK_WAIT_SEND : { //blink frames with IEEE EUI-64 tag ID inst->blinkmsg.frameCtrl = 0xC5 ; inst->blinkmsg.seqNum = inst->frame_sn++; dwt_writetxdata((BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC), (uint8 *) (&inst->blinkmsg), 0) ; // write the frame data dwt_writetxfctrl((BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC), 0); //response will be sent after 500us (thus delay the receiver turn on by 290sym ~ 299us) //use delayed rx on (wait4resp timer) - this value is applied when the TX frame is done/sent, so this value can be written after TX is started dwt_setrxaftertxdelay(inst->rnginitW4Rdelay_sy); //units are ~us - wait for wait4respTIM before RX on (delay RX) dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //always using immediate TX dwt_setrxtimeout(inst->fwtoTimeB_sy); //units are us - wait for BLINKRX_FWTO_TIME after RX on before timing out #if (DW_IDLE_CHK==2) //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(inst->txantennaDelay) ; #endif inst->sentSN = inst->blinkmsg.seqNum; inst->wait4ack = DWT_RESPONSE_EXPECTED; //Poll is coming soon after... inst->instToSleep = 1; inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation inst->previousState = TA_TXBLINK_WAIT_SEND ; inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below) } break ; // end case TA_TXBLINK_WAIT_SEND case TA_TXPOLL_WAIT_SEND : { #if (DR_DISCOVERY == 1) //NOTE the anchor address is set after receiving the ranging initialisation message inst->instToSleep = 1; //go to Sleep after this poll #else //set destination address if(destaddress(inst)) { break; } //copy anchor address to short message structure inst->msg_f.destAddr[0] = inst->msg.destAddr[0]; inst->msg_f.destAddr[1] = inst->msg.destAddr[1]; #endif inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_POLLF; inst->psduLength = TAG_POLL_F_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC + EXTRA_LENGTH; //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6) inst->msg_f.frameCtrl[0] = 0x41 /*PID comp*/; //short address for both inst->msg_f.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/; inst->msg_f.seqNum = inst->frame_sn++; inst->wait4ack = DWT_RESPONSE_EXPECTED; //Response is coming after 275 us... //500 -> 485, 800 -> 765 dwt_writetxfctrl(inst->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(inst->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(inst->psduLength, (uint8 *) &inst->msg_f, 0) ; // write the poll frame data //start TX of frame dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack); #if (DW_IDLE_CHK==2) //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(inst->txantennaDelay) ; #endif inst->sentSN = inst->msg_f.seqNum; //write the final function code inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_FINALF; //increment the sequence number for the final message inst->msg_f.seqNum = inst->frame_sn; 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 below) inst->responseRxNum = 0; } break; case TA_TX_WAIT_CONF : //printf("TA_TX_WAIT_CONF") ; { //uint8 temp[5]; event_data_t* dw_event = instance_getevent(5); //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 == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK) { //we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed inst->gotTO = 1; } if(dw_event->type == SIG_RX_ACK) { inst->wait4ack = 0 ; //clear the flag as the ACK has been received inst_processackmsg(inst, dw_event->msgu.rxackmsg.seqNum); //printf("RX ACK in TA_TX_WAIT_CONF... wait for TX confirm before changing state\n"); } inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; break; } inst->done = INST_NOT_DONE_YET; if(inst->previousState == TA_TXFINAL_WAIT_SEND) //tag will do immediate receive when waiting for report (as anchor sends it without delay) { #if (DR_DISCOVERY == 1) //in Discovery mode anchor is not sending the report to tag go to sleep inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //kick off the TagTimeoutTimer (instance timer) to initiate wakeup inst->nextState = TA_TXPOLL_WAIT_SEND; inst->testAppState = TA_TXE_WAIT; //we are going manually to sleep - change to TA_TXE_WAIT state #else //wait for report when non-Discovery mode if(inst->wait4ack == 0) dwt_rxenable(0) ; // turn receiver on, #endif break; } else if (inst->gotTO) //timeout { inst_processrxtimeout(inst); inst->gotTO = 0; } else { if(inst->previousState == TA_TXPOLL_WAIT_SEND) { // write the final's frame control and address tx data (add CRC as the function will write length - 2) dwt_writetxdata((FRAME_CRTL_AND_ADDRESS_S + 1 + FRAME_CRC), (uint8 *) &inst->msg_f, FINAL_MSG_OFFSET) ; // write the final frame data dwt_entersleepaftertx(1); dwt_setinterrupt(DWT_INT_TFRS, 0); //disable all the interrupts (wont be able to enter sleep if interrupts are pending) inst->tagPollTxTime32l = dw_event->timeStamp32l; inst->relpyAddress[0] = inst->msg_f.destAddr[0]; inst->relpyAddress[1] = inst->msg_f.destAddr[1]; inst->canprintinfo = 2; } if(inst->previousState == TA_TXRANGINGINIT_WAIT_SEND) //set frame control for the response message { dwt_writetxfctrl((ANCH_RESPONSE_F_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC + EXTRA_LENGTH), RESPONSE_MSG_OFFSET); } inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame //turn RX on if(inst->wait4ack == 0) dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1 inst->wait4ack = 0 ; //dwt_readfromdevice(0x19, 0, 5, temp); //sprintf((char*)&usbdata[20], "T2R%d %02x%02x%02x%02x%02x ", count, temp[4], temp[3], temp[2], temp[1], temp[0]); //send_usbmessage(&usbdata[20], 16); //count=0; } } break ; // end case TA_TX_WAIT_CONF case TA_RXE_WAIT : //printf("TA_RXE_WAIT") ; { // - with "fast" ranging - we only get here after frame timeout... //turn RX on instancerxon(inst, 0, 0) ; // turn RX on, with/without delay 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 //printf("TA_RX_WAIT_DATA") ; switch (message) { case SIG_RX_BLINK : { instance_getevent(6); //get and clear this event //else //not initiating ranging - continue to receive { inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame //turn RX on dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1 inst->done = INST_NOT_DONE_YET; } } break; case SIG_RX_ACK : { event_data_t* dw_event = instance_getevent(7); //get and clear this event inst_processackmsg(inst, dw_event->msgu.rxackmsg.seqNum); //else we did not expect this ACK turn the RX on again //only enable receiver when not using double buffering inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame //turn RX on dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1 inst->done = INST_NOT_DONE_YET; } break; case DWT_SIG_RX_OKAY : { event_data_t* dw_event = instance_getevent(8); //get and clear this event uint8 srcAddr[8] = {0,0,0,0,0,0,0,0}; int fcode = 0; int fn_code = 0; int srclen = 0; int fctrladdr_len; uint8 *messageData; inst->stoptimer = 0; //clear the flag, as we have received a message // 16 or 64 bit addresses switch(dw_event->msgu.frame[1]) { case 0xCC: // memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[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); 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); 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); 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->ackexpected) && (inst->ackTO)) //ACK frame was expected but we got a good frame - treat as ACK timeout { //printf("got good frame instead of ACK in DWT_SIG_RX_OKAY - pretend TO\n"); inst_processrxtimeout(inst); message = 0; //clear the message as we have processed the event } else { inst->ackexpected = 0; //clear this as we got good frame (but as not using ACK TO) we prob missed the ACK - check if it has been addressed to us fcode = fn_code; //tag has address filtering so if it received a frame it must be addressed to it switch(fcode) { case RTLS_DEMO_MSG_RNG_INIT: { if(inst->mode == TAG_TDOA) //only start ranging with someone if not ranging already { //double delay = rxrngmsg->messageData[RES_T1] + (rxrngmsg->messageData[RES_T2] << 8); //in ms inst->testAppState = TA_TXPOLL_WAIT_SEND ; // send next poll //remember the anchor address inst->msg_f.destAddr[0] = srcAddr[0]; inst->msg_f.destAddr[1] = srcAddr[1]; inst->msg_f.sourceAddr[0] = messageData[RES_R1]; inst->msg_f.sourceAddr[1] = messageData[RES_R1+1]; inst->tagShortAdd = (uint16)messageData[RES_R1] + ((uint16)messageData[RES_R2] << 8) ; dwt_setaddress16(inst->tagShortAdd); //instancesetreplydelay(delay); // inst->mode = TAG ; inst->rxTimeouts = 0; //reset timeout count inst->instToSleep = 0; //don't go to sleep - start ranging instead and then sleep after 1 range is done inst->done = INST_NOT_DONE_YET; } } break; //RTLS_DEMO_MSG_RNG_INITF case RTLS_DEMO_MSG_ANCH_RESPF: { #if (TWSYMRANGE == 1) //need to write the delayed time before starting transmission inst->delayedReplyTime32 = ((uint32)dw_event->timeStamp32h + (uint32)inst->fixedFastReplyDelay32h) ; dwt_setdelayedtrxtime(inst->delayedReplyTime32) ; dwt_writetxfctrl((TAG_FINAL_F_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC), FINAL_MSG_OFFSET); if(dwt_starttx(DWT_START_TX_DELAYED)) { //error - TX FAILED // initiate the re-transmission inst->testAppState = TA_TXE_WAIT ; inst->nextState = TA_TXPOLL_WAIT_SEND; dwt_entersleepaftertx(0); inst->wait4ack = 0; //clear the flag as the TX has inst->lateTX++; break; //exit this switch case... } else { rtd_t rtd; //calculate the difference between response rx and final tx //here we just need to subtract the low 32 bits as the response delay is < 32bits (actually it is < 26 bits) rtd.diffRmP = (uint32)dw_event->timeStamp32l - (uint32)inst->tagPollTxTime32l ; //calculate difference between final tx and response rx rtd.diffFmR = (uint32)inst->txantennaDelay + ((uint32)inst->fixedFastReplyDelay32h << 8) - ((uint32)dw_event->timeStamp32l & 0x1FF); //write the rest of the message (the two response time differences (low 32 bits) dwt_writetxdata((TAG_FINAL_F_MSG_LEN - 1 + FRAME_CRC), (uint8 *) &rtd, (FINAL_MSG_OFFSET+FRAME_CRTL_AND_ADDRESS_S+1)) ; // write the frame data inst->sentSN = inst->msg_f.seqNum; inst->previousState = TA_TXFINAL_WAIT_SEND; //if Tag is not waiting for report - it will go to sleep automatically after the final is sent inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //kick off the TagTimeoutTimer (instancetimer) to initiate wakeup inst->testAppState = TA_SLEEP_DONE; //we are going automatically to sleep so no TX confirm interrupt (next state will be TA_SLEEP_DONE) inst->canprintinfo = 1; inst->txmsgcount ++; inst->frame_sn++ ; //increment as final is sent } inst->respPSC = (dwt_read16bitoffsetreg(0x10, 2) >> 4); inst->wait4ack = 0; //no response inst->ackexpected = !ACK_REQUESTED ; //used to ignore unexpected ACK frames //inst->rxu.anchorRespRxTime = inst->rxu.rxTimeStamp ; //Response's Rx time inst->nextState = TA_TXPOLL_WAIT_SEND; #else if(inst->responseRxNum == 0) // this is first response { dwt_setrxtimeout(5000); //~5ms //turn RX on dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1 inst->anchResp1RxTime32l = dw_event->timeStamp32l; inst->responseRxNum++; } else // we have two responses and can calculate ToF { //the first response will be sent time X after reception of the poll, but as the tx time is snapped to 8ns //we need to account for the low 9 bits of poll rx time in the RTD calculation uint32 pollrxlowbits = (uint32)messageData[1] + (uint32)(messageData[2] << 8); //RTD = (RxResp1 - TxPoll) - (RxResp2 - RxResp1) //ToF = RTD/2 = RxResp1 - 0.5 * (TxPoll + RxResp2) inst->tof32 = ((uint32)inst->anchResp1RxTime32l - (uint32)inst->tagPollTxTime32l + pollrxlowbits) - ((uint32)dw_event->timeStamp32l - (uint32)inst->anchResp1RxTime32l); inst->tof32 <<= 1; //to make it compatible with reportTOF() which expects ToF*4 reportTOF_f(inst); inst->newrange = 1; inst->testAppState = TA_TXE_WAIT ; inst->nextState = TA_TXPOLL_WAIT_SEND; } #endif } break; //RTLS_DEMO_MSG_ANCH_RESPF case RTLS_DEMO_MSG_ANCH_TOFRF: { inst->tof32 = messageData[TOFR]; inst->tof32 += (uint32)messageData[TOFR+1] << 8; inst->tof32 += (uint32)messageData[TOFR+2] << 16; inst->tof32 += (uint32)messageData[TOFR+3] << 24; if(dw_event->msgu.rxmsg_ss.seqNum != inst->lastReportSN) { reportTOF_f(inst); inst->newrange = 1; inst->lastReportSN = dw_event->msgu.rxmsg_ss.seqNum; inst->newrangetagaddress = srcAddr[0] + ((uint16) srcAddr[1] << 8); inst->newrangeancaddress = inst->eui64[0] + ((uint16) inst->eui64[1] << 8); } inst->testAppState = TA_TXE_WAIT; inst->nextState = TA_TXPOLL_WAIT_SEND ; // send next poll } break; //RTLS_DEMO_MSG_ANCH_TOFRF default: { //only enable receiver when not using double buffering inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame //turn RX on dwt_rxenable(0) ; // turn receiver on, immediate = 0/delayed = 1 } break; } //end switch (rxmsg->functionCode) 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(9); //get and clear the ACK sent event } } } break ; case DWT_SIG_RX_TIMEOUT : //printf("PD_DATA_TIMEOUT") ; instance_getevent(26); //get and clear this event inst_processrxtimeout(inst); 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: //no event - wait in receive... { //stay in Rx (fall-through from above state) //if(DWT_SIG_TX_AA_DONE == message) printf("Got SIG_TX_AA_DONE in RX wait - ignore\n"); if(inst->done == INST_NOT_DONE_YET) inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; } break; default : { //printf("\nERROR - Unexpected message %d ??\n", message) ; //assert(0) ; // Unexpected Primitive what is going on ? } break ; } break ; // end case TA_RX_WAIT_DATA default: //printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ; break; } // end switch on testAppState return inst->done; } // end testapprun()
// Triggered when we receive a packet void app_dw1000_rxcallback (const dwt_callback_data_t *rxd) { // First grab a copy of local time when this arrived rtimer_clock_t rt_timestamp = RTIMER_NOW(); DEBUG_B6_HIGH; if (rxd->event == DWT_SIG_RX_OKAY) { leds_toggle(LEDS_BLUE); uint8_t packet_type; uint64_t dw_timestamp; uint8_t recv_pkt_buf[RX_PKT_BUF_LEN]; // Get the dw_timestamp first uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0}; dwt_readrxtimestamp(txTimeStamp); dw_timestamp = ((uint64_t) (*((uint32_t*) txTimeStamp))) | (((uint64_t) txTimeStamp[4]) << 32); // Get the packet dwt_readrxdata(recv_pkt_buf, MIN(RX_PKT_BUF_LEN, rxd->datalength), 0); packet_type = recv_pkt_buf[offsetof(struct pp_tag_poll, message_type)]; global_round_num = recv_pkt_buf[offsetof(struct pp_tag_poll, roundNum)]; if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_POLL) { struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf; DEBUG_P("TAG_POLL rx: %u\r\n", pkt->subsequence); if (global_subseq_num == pkt->subsequence) { // Configurations matched, record arrival time pp_anc_final_pkt.TOAs[global_subseq_num] = dw_timestamp; } else { // Tag/anchor weren't on same settings, so we // drop this sample and catch the anchor up global_subseq_num = pkt->subsequence; } if (!global_round_active) { DEBUG_B4_LOW; DEBUG_B5_LOW; memset(antenna_statistics, 0, sizeof(antenna_statistics)); global_round_active = true; start_of_new_subseq = true; substate_timer_fired = true; /* subseq_start_time = rt_timestamp - US_TO_RT(TAG_SQ_START_TO_POLL_SFD_HIGH_US); rtimer_clock_t set_to = subseq_start_time + US_TO_RT(POLL_TO_SS_US+SS_TO_SQ_US); */ rtimer_clock_t set_to = rt_timestamp + US_TO_RT( POLL_TO_SS_US + SS_TO_SQ_US - TAG_SQ_START_TO_POLL_SFD_HIGH_US - ANC_MYSTERY_STARTUP_DELAY_US); rtimer_set(&subsequence_timer, set_to, 1, (rtimer_callback_t)subsequence_task, NULL); } //Keep a running total of the number of packets seen from each antenna antenna_statistics[subseq_num_to_anchor_sel(global_subseq_num)]++; } else if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_FINAL) { if (!global_round_active) { // The first packet we happened to receive was // an ANC_FINAL. We have nothing interesting to // reply with, so don't. But we *do* need to set // receive mode again so that a new poll will be // caught dwt_rxenable(0); return; } //We're likely in RX mode, so we need to exit before transmission dwt_forcetrxoff(); pp_anc_final_pkt.TOAs[NUM_MEASUREMENTS] = dw_timestamp; pp_anc_final_pkt.header.seqNum++; const uint16 frame_len = sizeof(struct pp_anc_final); dwt_writetxfctrl(frame_len, 0); //Schedule this transmission for our scheduled time slot DEBUG_B6_LOW; uint32_t temp = dwt_readsystimestamphi32(); uint32_t delay_time = temp + DW_DELAY_FROM_US( ANC_FINAL_INITIAL_DELAY_HACK_VALUE + (ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1)) ); /* I don't understand what exactly is going on * here. The chip seems to want way longer for * this preamble than others -- maybe something * to do with the large payload? From empirical * measurements, the 300 base delay is about the * minimum (250 next tested as not working) DW_DELAY_FROM_US( REVISED_DELAY_FROM_PKT_LEN_US(frame_len) + (2*ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1)) ); */ delay_time &= 0xFFFFFFFE; pp_anc_final_pkt.dw_time_sent = delay_time; dwt_setdelayedtrxtime(delay_time); int err = dwt_starttx(DWT_START_TX_DELAYED); dwt_settxantennadelay(TX_ANTENNA_DELAY); dwt_writetxdata(frame_len, (uint8_t*) &pp_anc_final_pkt, 0); DEBUG_B6_HIGH; #ifdef DW_DEBUG // No printing until after all dwt timing op's struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf; DEBUG_P("TAG_FINAL rx: %u\r\n", pkt->subsequence); #endif if (err) { #ifdef DW_DEBUG uint32_t now = dwt_readsystimestamphi32(); DEBUG_P("Could not send anchor response\r\n"); DEBUG_P(" -- sched time %lu, now %lu (diff %lu)\r\n", delay_time, now, now-delay_time); leds_on(LEDS_RED); #endif } else { DEBUG_P("Send ANC_FINAL\r\n"); leds_off(LEDS_RED); } } else { DEBUG_P("*** ERR: RX Unknown packet type: 0x%X\r\n", packet_type); } } else {
bool DWM1000_Tag::dispatch(Msg& msg) { PT_BEGIN() PT_WAIT_UNTIL(msg.is(0, SIG_INIT)); init(); POLL_SEND: { while (true) { timeout(1000); // delay between POLL PT_YIELD_UNTIL(timeout()); /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */ tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb; dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0); dwt_writetxfctrl(sizeof(tx_poll_msg), 0); /* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay * set by dwt_setrxaftertxdelay() has elapsed. */ LOG<< " Start TXF " << FLUSH; dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);// SEND POLL MSG dwt_setinterrupt(DWT_INT_TFRS, 0); dwt_setinterrupt(DWT_INT_RFCG, 1); // enable clearInterrupt(); _timeoutCounter = 0; /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */ timeout(10); PT_YIELD_UNTIL(timeout() || isInterruptDetected()); // WAIT RESP MSG if (isInterruptDetected()) LOG<< " INTERRUPT DETECTED " << FLUSH; status_reg = dwt_read32bitreg(SYS_STATUS_ID); LOG<< HEX <<" SYS_STATUS " << status_reg << FLUSH; if (status_reg == 0xDEADDEAD) { init(); } else if (status_reg & SYS_STATUS_RXFCG) goto RESP_RECEIVED; else if (status_reg & SYS_STATUS_ALL_RX_ERR) { if (status_reg & SYS_STATUS_RXRFTO) INFO(" RX Timeout"); else INFO(" RX error "); dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); /* Clear RX error events in the DW1000 status register. */ } } } RESP_RECEIVED: { LOG<< " Received " <<FLUSH; frame_seq_nb++; /* Increment frame sequence number after transmission of the poll message (modulo 256). */ uint32 frame_len; /* Clear good RX frame event and TX frame sent in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); /* A frame has been received, read iCHANGEt into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; if (frame_len <= RX_BUF_LEN) { dwt_readrxdata(rx_buffer, frame_len, 0); } /* Check that the frame is the expected response from the companion "DS TWR responder" example. * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */ rx_buffer[ALL_MSG_SN_IDX] = 0; if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0) { // CHECK RESP MSG uint32 final_tx_time; /* Retrieve poll transmission and response reception timestamp. */ poll_tx_ts = get_tx_timestamp_u64(); resp_rx_ts = get_rx_timestamp_u64(); /* Compute final message transmission time. See NOTE 9 below. */ final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8; dwt_setdelayedtrxtime(final_tx_time); /* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */ final_tx_ts = (((uint64) (final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY; /* Write all timestamps in the final message. See NOTE 10 below. */ final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts); final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts); final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts); /* Write and send final message. See NOTE 7 below. */ tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb; dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0); dwt_writetxfctrl(sizeof(tx_final_msg), 0); dwt_starttx(DWT_START_TX_DELAYED); // SEND FINAL MSG /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */ timeout(10); PT_YIELD_UNTIL((dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS) || timeout());; /* Clear TXFRS event. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS); /* Increment frame sequence number after transmission of the final message (modulo 256). */ frame_seq_nb++; } else {
/*! ------------------------------------------------------------------------------------------------------------------ * @fn main() * * @brief Application entry point. * * @param none * * @return none */ int ssTwrResp(void) { /* Reset and initialise DW1000. * For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum * performance. */ int i; int status; reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */ //spi_set_rate_low(); dwt_initialise(DWT_LOADUCODE); //spi_set_rate_high(); /* Configure DW1000. See NOTE 5 below. */ dwt_configure(&config); uint32_t otpVal[0x20]; dwt_otpread(0,otpVal,0x20); printf("OTP 6: 0x%x\r\n",otpVal[6]); printf("OTP 7: 0x%x\r\n",otpVal[7]); printf("OTP x16: 0x%x\r\n",otpVal[0x16]); printf("OTP x17: 0x%x\r\n",otpVal[0x17]); /* Apply default antenna delay value. See NOTE 2 below. */ printf("antenna delays: default TX: %d, default RX: %d, evk 16m: %d, evk 64m: %d\r\n",TX_ANT_DLY,RX_ANT_DLY,DWT_RF_DELAY_16M,DWT_RF_DELAY_64M); tx_delay = TX_ANT_DLY; rx_delay = RX_ANT_DLY; dwt_setrxantennadelay(rx_delay); dwt_settxantennadelay(tx_delay); btn = buttons(); printf("%s entering main loop\r\n",__FUNCTION__); /* Loop forever responding to ranging requests. */ while (1) { /* Activate reception immediately. */ dwt_rxenable(0); /* Poll for reception of a frame or error/timeout. See NOTE 6 below. */ while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) { } ; //printf("Waiting. status reg 0x%x\r\n",status_reg); }; //printf("Status reg now 0x%x\r\n",status_reg); if (status_reg & SYS_STATUS_RXFCG) { uint32 frame_len; //printf("Check RX\r\n"); /* Clear good RX frame event in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); /* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023; //printf("Frame length %d\r\n",frame_len); if (frame_len <= RX_BUFFER_LEN) { dwt_readrxdata(rx_buffer, frame_len, 0); } /* for (i=0;i<frame_len;i++) { printf("%x ",rx_buffer[i]); } printf("\r\n"); */ /* Check that the frame is a poll sent by "SS TWR initiator" example. * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */ rx_buffer[ALL_MSG_SN_IDX] = 0; if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) { uint32 resp_tx_time; //printf("Poll MSG\r\n"); /* Retrieve poll reception timestamp. */ poll_rx_ts = get_rx_timestamp_u64(); //printf("RX timestamp: %lld\r\n",poll_rx_ts); /* Compute final message transmission time. See NOTE 7 below. */ resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8; dwt_setdelayedtrxtime(resp_tx_time); //printf("TX time: %d\r\n",resp_tx_time); /* Response TX timestamp is the transmission time we programmed plus the antenna delay. */ resp_tx_ts = (((uint64)(resp_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY; //printf("TX timestamp: %lld\r\n",resp_tx_ts); /* Write all timestamps in the final message. See NOTE 8 below. */ resp_msg_set_ts(&tx_resp_msg[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts); resp_msg_set_ts(&tx_resp_msg[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts); /* Write and send the response message. See NOTE 9 below. */ tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb; status = dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0); if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__); status = dwt_writetxfctrl(sizeof(tx_resp_msg), 0); if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__); status = dwt_starttx(DWT_START_TX_DELAYED); if (DWT_SUCCESS != status) { printf("API error line %d\r\n",__LINE__); printf("RX timestamp: %llu\r\n",poll_rx_ts); printf("TX time: %llu\r\n",((uint64)resp_tx_time) << 8); printf("TX timestamp: %llu\r\n",resp_tx_ts); } // poll only if starttx was OK if (DWT_SUCCESS == status) { /* Poll DW1000 until TX frame sent event set. See NOTE 6 below. */ u32 tx_stat; while (!(tx_stat = dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)) { }; //printf("Waiting. status reg 0x%x\r\n",tx_stat); } //printf("After Poll: status reg now 0x%x\r\n",tx_stat); } /* Clear TXFRS event. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS); /* Increment frame sequence number after transmission of the poll message (modulo 256). */ frame_seq_nb++; } }