Example #1
0
cph_deca_msg_header_t * cph_deca_read_frame(uint8_t * rx_buffer, uint32_t *frame_len) {

	/* 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;
	if (*frame_len <= CPH_MAX_MSG_SIZE) {
		dwt_readrxdata(rx_buffer, *frame_len, 0);

#if defined(DEBUG_PRINT_RXFRAME)
		printf("FRM: ");
		for (int i=0;i<*frame_len;i++)
			printf("%02X ", rx_buffer[i]);
		printf("\r\n");
#endif

		return (cph_deca_msg_header_t*)rx_buffer;
	}
	else {
		TRACE("Bad length: %04X\r\n", *frame_len);
		return 0;
	}

}
Example #2
0
uint32_t cph_deca_wait_for_rx_finished_signal(int timeout_ms, volatile uint8_t * signal) {
	uint32_t status_reg;
	uint32_t start_ms = cph_get_millis();
	uint32_t elapsed = 0;
	while (cph_deca_isr_is_detected() == false) {
		if (*signal == 0xFF)
			return 0;
		elapsed = cph_get_millis() - start_ms;
		if (elapsed > timeout_ms)
			return 0;
	}
	status_reg = dwt_read32bitreg(SYS_STATUS_ID);
	return status_reg;
}
Example #3
0
void toSendMsg1(uint8 niz[], int size){

	uint8 niz1[size];
	int rx_id;
	uint8 transmission_delay;


	memset(dataseq2,0,sizeof(dataseq1));
	sprintf((char*)&dataseq2[0],"%lx %lx send",

	niz1[1],niz1[2]);//,niz1[2],sizeof(niz1), size);
	writetoLCD(1, 0, 0x2);
	writetoLCD(40,1,dataseq2);
	memset(dataseq2,0,sizeof(dataseq2));



	 rx_id=niz[2];
	 if(rx_id==1){
	 for (int i=0;i<size;i++){
	      niz1[i]=niz[i];
	}
	 niz1[1]=rx_seq_num;
	 niz1[2]=3;					// ID of the end node/ second one in the line.


	                               /* Write frame data to DW1000 and prepare transmission. See NOTE 3 below.*/
	dwt_writetxdata(size, niz1, 0);
	dwt_writetxfctrl(size, 0);

	                                      /* Start transmission. */
	dwt_starttx(DWT_START_TX_IMMEDIATE);

	                                      /* Poll DW1000 until TX frame sent event set. See NOTE 4 below.
	                                       * STATUS register is 5 bytes long but, as the event we are looking at is in the first byte of the register, we can use this simplest API
	                                       * function to access it.*/
	while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
	{ };

	/* Clear TX frame sent event. */
	dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);

	/* Execute a delay between transmissions. */
	deca_sleep(TX_DELAY_MS);

	/* Increment the blink frame sequence number (modulo 256). */
	rx_seq_num++;

	 }
}
Example #4
0
cph_deca_msg_header_t * cph_deca_read_frame(uint8_t * rx_buffer, uint32_t *frame_len) {

	/* 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;
	if (*frame_len <= CPH_MAX_MSG_SIZE) {
		dwt_readrxdata(rx_buffer, *frame_len, 0);
		return (cph_deca_msg_header_t*)rx_buffer;
	}
	else {
		return 0;
	}

}
Example #5
0
static void print_status(void) {
	uint32 status = 0;
	uint32 status1 = 0;

	status = dwt_read32bitreg(SYS_STATUS_ID);
	status1 = dwt_read32bitoffsetreg(SYS_STATUS_ID, 1);            // read status register
	dwt_readfromdevice(RX_BUFFER_ID, 0, 2, buffer);
	printf("MAIN status %02X %08X, byte 0 %02X%02X\r\n", status1 >> 24, status, buffer[0], buffer[1]);

	memset(buffer, 0xaa, FS_CTRL_LEN);
	dwt_readfromdevice(FS_CTRL_ID, 0, FS_CTRL_LEN, buffer);
	printf("FS_PLLCFG %08X  FS_PLLTUNE %02X  FS_XTALT %02X\r\n", *((uint32_t*) &buffer[0x07]), (uint8_t) (buffer[0x0b]),
			(uint8_t) (buffer[0x0e]));

	status = dwt_read32bitoffsetreg(RF_CONF_ID, RF_STATUS_OFFSET);
	printf("RF_STATUS %08X\r\n\r\n", status);
}
Example #6
0
void toSendMsg(uint8 niz[], int size){   // for relaing node, that does not change anything

     	uint8 niz1[size];
     	int rx_id;
     	uint8 transmission_delay;


     	memset(dataseq2,0,sizeof(dataseq1));
     	sprintf((char*)&dataseq2[0],"%lx send",

     	niz1[2]);//,niz1[2],sizeof(niz1), size);
     	writetoLCD(1, 0, 0x2);
     	writetoLCD(40,1,dataseq2);
     	memset(dataseq2,0,sizeof(dataseq2));



     	 rx_id=niz[2];

     	 for (int i=0;i<size;i++){
     	      niz1[i]=niz[i];
     	}



     	/* Write frame data to DW1000 and prepare transmission. See NOTE 3 below.*/
     	dwt_writetxdata(size, niz1, 0);
     	dwt_writetxfctrl(size, 0);

     	/* Start transmission. */
     	dwt_starttx(DWT_START_TX_IMMEDIATE);

     	/* Poll DW1000 until TX frame sent event set. See NOTE 4 below.
     	                                       * STATUS register is 5 bytes long but, as the event we are looking at is in the first byte of the register, we can use this simplest API
     	                                       * function to access it.*/
        while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
     	{ };

     	 /* Clear TX frame sent event. */
     	dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);

     	/* Execute a delay between transmissions. */
     	deca_sleep(TX_DELAY_MS);
     	 }
Example #7
0
/**
 * Application entry point.
 */
int simpleTx(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. */
    reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
    //spi_set_rate_low();
    dwt_initialise(DWT_LOADNONE);
    //spi_set_rate_high();

    /* Configure DW1000. See NOTE 2 below. */
    dwt_configure(&config);

    /* Loop forever sending frames periodically. */
    while(1)
    {
        /* Write frame data to DW1000 and prepare transmission. See NOTE 3 below.*/
        dwt_writetxdata(sizeof(tx_msg), tx_msg, 0);
        dwt_writetxfctrl(sizeof(tx_msg), 0);

        /* Start transmission. */
        dwt_starttx(DWT_START_TX_IMMEDIATE);

        /* Poll DW1000 until TX frame sent event set. See NOTE 4 below.
         * STATUS register is 5 bytes long but, as the event we are looking at is in the first byte of the register, we can use this simplest API
         * function to access it.*/
        while (!(status_reg = dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
        { };
        printf("Status reg now 0x%x\r\n",status_reg);

        /* Clear TX frame sent event. */
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);

        /* Execute a delay between transmissions. */
        deca_sleep(TX_DELAY_MS);

        // toggle led
		ledToggle();

        /* Increment the blink frame sequence number (modulo 256). */
        tx_msg[BLINK_FRAME_SN_IDX]++;
    }
}
Example #8
0
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 {
Example #9
0
IRAM void DWM1000_Anchor::my_dwt_isr() {
	interrupt_detected = true;
	_status_reg = dwt_read32bitreg(SYS_STATUS_ID);
	dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR | SYS_STATUS_RXFCG);
	interruptCount++;
}
Example #10
0
// -------------------------------------------------------------------------------------------------------------------
//
// 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()
Example #11
0
/**
 * Application entry point.
 */
int rxWait(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;

    reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
    //spi_set_rate_low();
    dwt_initialise(DWT_LOADNONE);
    //spi_set_rate_high();

    /* Configure DW1000. See NOTE 2 below. */
    dwt_configure(&config);

    /* Loop forever sending and receiving frames periodically. */
    while (1)
    {
        /* Activate reception immediately. See NOTE 3 below. */
        dwt_rxenable(0);

        /* Poll until a frame is properly received or an error occurs. See NOTE 4 below.
         * STATUS register is 5 bytes long but, as the events we are looking at are in the lower bytes of the register, we can use this simplest API
         * function to access it. */
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
        { };
        printf("Status reg now 0x%x\r\n",status_reg);

        if (status_reg & 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;
            if (frame_len <= FRAME_LEN_MAX)
            {
                dwt_readrxdata(rx_buffer, frame_len, 0);
            }

            /* Clear good RX frame event in the DW1000 status register. */
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);

            for (i=0;i<frame_len;i++) {
            	printf("%x ",rx_buffer[i]);
            }
            printf("\r\n");

            /* Validate the frame is the one expected as sent by "TX then wait for a response" example. */
            if ((frame_len == 14) && (rx_buffer[0] == 0xC5) && (rx_buffer[10] == 0x43) && (rx_buffer[11] == 0x2))
            {
                int i;

                /* Copy source address of blink in response destination address. */
                for (i = 0; i < 8; i++)
                {
                    tx_msg[DATA_FRAME_DEST_IDX + i] = rx_buffer[BLINK_FRAME_SRC_IDX + i];
                }

                /* Write response frame data to DW1000 and prepare transmission. See NOTE 5 below.*/
                dwt_writetxdata(sizeof(tx_msg), tx_msg, 0);
                dwt_writetxfctrl(sizeof(tx_msg), 0);

                /* Send the response. */
                dwt_starttx(DWT_START_TX_IMMEDIATE);

                /* Poll DW1000 until TX frame sent event set. */
                while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
                { };

                /* Clear TX frame sent event. */
                dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);

                /* Increment the data frame sequence number (modulo 256). */
                tx_msg[DATA_FRAME_SN_IDX]++;
            }
        }
        else
        {
            /* Clear RX error events in the DW1000 status register. */
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
            printf("Some RX errors ...\r\n");
        }
    }
}
Example #12
0
void listener_run(void) {

	uint32_t announce_coord_ts = 0;
	uint32_t elapsed = 0;
	uint32_t last_ts = 0;
	uint32_t count = 0;

	irq_init();
	pio_disable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);



	// Setup DW1000
	dwt_txconfig_t txconfig;

	// Setup DECAWAVE
	reset_DW1000();
	spi_set_rate_low();
	dwt_initialise(DWT_LOADUCODE);
	spi_set_rate_high();

	dwt_configure(&cph_config->dwt_config);

	dwt_setpanid(0x4350);
	dwt_setaddress16(0x1234);

	// Clear CLKPLL_LL
	dwt_write32bitreg(SYS_STATUS_ID, 0x02000000);

	uint32_t id = dwt_readdevid();
	printf("Device ID: %08X\r\n", id);


#if 1

	dwt_setcallbacks(0, rxcallback);
	dwt_setinterrupt(
			DWT_INT_TFRS | DWT_INT_RFCG
					| (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/),
			1);

	pio_enable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);

	dwt_rxenable(0);

	while (1) {

		elapsed = cph_get_millis() - last_ts;
		if (elapsed > 5000) {
			printf("alive %d\r\n", count++);
			last_ts = cph_get_millis();
		}

		if (trx_signal == SIGNAL_RCV) {
			printf("[RCV] %d - ", frame_len);
			for (int i = 0; i < frame_len; i++) {
				printf("%02X ", rx_buffer[i]);
			}
			printf("\r\n");
			trx_signal = SIGNAL_EMPTY;
			dwt_rxenable(0);
		}
		else if(trx_signal == SIGNAL_ERR) {
			printf("ERROR: %08X\r\n", error_status_reg);
			trx_signal = SIGNAL_EMPTY;
			dwt_rxenable(0);
		}
		else if(trx_signal == SIGNAL_ERR_LEN) {
			printf("ERROR LENGTH: %08X\r\n", error_status_reg);
			trx_signal = SIGNAL_EMPTY;
			dwt_rxenable(0);
		}
	}

#else

	while (1) {

		/* Activate reception immediately. */
		dwt_rxenable(0);

		while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) {
		};

		if (status_reg & SYS_STATUS_RXFCG) {
			uint32 frame_len;

			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);

			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
			if (frame_len <= MAXRXSIXZE) {
				dwt_readrxdata(rx_buffer, frame_len, 0);
			} else {
				frame_len = 0;
			}

			if (frame_len > 0) {
				printf("[RCV] ");
				for (int i = 0; i < frame_len; i++) {
					printf("%02X ", rx_buffer[i]);
				}
				printf("\r\n");
			} else {
				printf("ERROR: frame_len == %d\r\n", frame_len);
			}

		} else {

			printf("ERROR: dwt_rxenable has status of %08X\r\n", status_reg);
			/* Clear RX error events in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR | SYS_STATUS_CLKPLL_LL);
		}
	}
#endif

}
Example #13
0
// -------------------------------------------------------------------------------------------------------------------
//
// 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
Example #14
0
/*! ------------------------------------------------------------------------------------------------------------------
 * @fn main()
 *
 * @brief Application entry point.
 *
 * @param  none
 *
 * @return none
 */
int ssTwrInit(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 status;

	reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
    //spi_set_rate_low();
    status = dwt_initialise(DWT_LOADUCODE);
    if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);
    //spi_set_rate_high();

    /* Configure DW1000. See NOTE 6 below. */
    status = dwt_configure(&config);
    if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);

    // read otp
    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);

    /* Set expected response's delay and timeout. See NOTE 1 and 5 below.
     * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);

    btn = buttons();

    printf("%s entering main loop\r\n",__FUNCTION__);

    /* Loop forever initiating ranging exchanges. */
    while (1)
    {
        /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
        tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
        status = dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
        if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);
        status = dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
        if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);
        status = dwt_writetxfctrl(sizeof(tx_poll_msg), 0);
        if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);

        /* 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. */
        status = dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
        if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);

        /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 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 (SYS_STATUS_RXRFTO & status_reg)
            printf("RX timeout\r\n");

        /* Increment frame sequence number after transmission of the poll message (modulo 256). */
        frame_seq_nb++;

        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_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 "SS 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)
            {
                uint32 poll_tx_ts, resp_rx_ts, poll_rx_ts, resp_tx_ts;
                int32 rtd_init, rtd_resp;

                /* Retrieve poll transmission and response reception timestamps. See NOTE 9 below. */
                poll_tx_ts = dwt_readtxtimestamplo32();
                resp_rx_ts = dwt_readrxtimestamplo32();

                /* Get timestamps embedded in response message. */
                resp_msg_get_ts(&rx_buffer[RESP_MSG_POLL_RX_TS_IDX], &poll_rx_ts);
                resp_msg_get_ts(&rx_buffer[RESP_MSG_RESP_TX_TS_IDX], &resp_tx_ts);

                /* Compute time of flight and distance. */
                rtd_init = resp_rx_ts - poll_tx_ts;
                rtd_resp = resp_tx_ts - poll_rx_ts;

                tof = ((rtd_init - rtd_resp) / 2.0) * DWT_TIME_UNITS;
                distance = tof * SPEED_OF_LIGHT;

                /* Display computed distance on LCD. */
                //sprintf(dist_str, "DIST: %3.2f m", distance);
                sprintf(dist_str, "%3.2f", distance);
                printf("%s\r\n",dist_str);
                //lcd_display_str(dist_str);

            }
        }
        else
        {
            /* Clear RX error events in the DW1000 status register. */
            printf("Errors occured. Clearing up\r\n");
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
        }

        /* Execute a delay between ranging exchanges. */
        //printf("Delay %dms\r\n",RNG_DELAY_MS);
        deca_sleep(RNG_DELAY_MS);

        // toggle led
		ledToggle();
		// update antenna
		if (buttons() & 1) {
		    tx_delay -= 10; // >>= 1;
		    rx_delay -= 10; //>>= 1;
		    dwt_setrxantennadelay(rx_delay);
		    dwt_settxantennadelay(tx_delay);
		    printf("Decreased antenna delay to 0x%x\r\n",tx_delay);
		}
		if (buttons() & 2) {
			tx_delay += 10;
			rx_delay += 10;
		    dwt_setrxantennadelay(rx_delay);
		    dwt_settxantennadelay(tx_delay);
		    printf("Increased antenna delay to 0x%x\r\n",tx_delay);
		}

    }
}
Example #15
0
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 {
Example #16
0
/*! ------------------------------------------------------------------------------------------------------------------
 * @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++;
            }
        }