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);
}
}
}
