static int range(cph_deca_anchor_range_t * range) { int result = CPH_OK; dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); // Setup POLL frame to request to range with anchor tx_poll_msg.header.dest = range->shortid; cph_deca_load_frame((cph_deca_msg_header_t*)&tx_poll_msg, sizeof(tx_poll_msg)); status_reg = cph_deca_send_response_expected(); if (status_reg & SYS_STATUS_RXFCG) { uint32 frame_len; cph_deca_msg_header_t * rx_header; // A frame has been received, read it into the local buffer. rx_header = cph_deca_read_frame(rx_buffer, &frame_len); if (rx_header) { // If valid response, calculate distance if (rx_header->functionCode == 0xE1) { 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. poll_tx_ts = dwt_readtxtimestamplo32(); resp_rx_ts = dwt_readrxtimestamplo32(); // Get timestamps embedded in response message. poll_rx_ts = ((cph_deca_msg_range_response_t*) (rx_header))->requestRxTs; resp_tx_ts = ((cph_deca_msg_range_response_t*) (rx_header))->responseTxTs; // 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; range->range = tof * SPEED_OF_LIGHT; range->range_avg -= (range->range_avg / RANGE_SAMPLES_AVG); range->range_avg += (range->range / RANGE_SAMPLES_AVG); } else { result = CPH_BAD_FRAME; } } else { result = CPH_BAD_LENGTH; } } else { // Clear RX error events in the DW1000 status register. dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); result = CPH_ERROR; } return result; }
/*! ------------------------------------------------------------------------------------------------------------------ * @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); } } }