Hi! I am using double side two way ranging using interrupts and getting distance value of -2706m. And in single side two way ranging the distance value I have got above 39000m that was not correct. I am using Deca-driver library for ranging measurement. Can you help me for accurate ranging measurements. I am using dwm1000 module for this purpose.
This is the code for responder with interrupt
__IO uint8_t rec_ok = 0;
__IO uint8_t rec_err = 0;
uint32_t status1 = 0;
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 1000
static DWT_ConfigTypeDef dwtConfig = {
.LoadCode = DW_LOAD_UCODE,
.Channel = DW_CHANNEL_2,
.PulseRepFreq = DW_PRF_64M,
.TxPreambLen = DW_TX_PLEN_1024,
.PreambleAcqChunk = DW_PAC_32,
.TxCode = 9,
.RxCode = 9,
.NonStandardSFD = ENABLE,
.DataRate = DW_DATARATE_110K,
.PhrMode = DW_PHR_MODE_STD,
.SFDTimeout = (1024 + 64 - 32) // TxPreambLen + 1 + SFD length - PAC
};
/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#define TX_ANT_DLY 16436
#define RX_ANT_DLY 16436
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 rx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8 tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8 rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
/* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */
#define ALL_MSG_COMMON_LEN 10
/* Index to access some of the fields in the frames involved in the process. */
#define ALL_MSG_SN_IDX 2
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4
/* Frame sequence number, incremented after each transmission. */
static uint8 frame_seq_nb = 0;
/* Buffer to store received messages.
* Its size is adjusted to longest frame that this example code is supposed to handle. */
#define RX_BUF_LEN 24
static uint8 rx_buffer[RX_BUF_LEN];
/* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
* 1 uus = 512 / 499.2 �s and 1 �s = 499.2 * 128 dtu. */
#define UUS_TO_DWT_TIME 65536
/* Delay between frames, in UWB microseconds. See NOTE 4 below. */
/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the
* frame length of approximately 2.46 ms with above configuration. */
#define POLL_RX_TO_RESP_TX_DLY_UUS 2750
/* 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. */
#define RESP_TX_TO_FINAL_RX_DLY_UUS 500
/* Receive final timeout. See NOTE 5 below. */
#define FINAL_RX_TIMEOUT_UUS 3300
/* Preamble timeout, in multiple of PAC size. See NOTE 6 below. */
#define PRE_TIMEOUT 8
/* Timestamps of frames transmission/reception.
* As they are 40-bit wide, we need to define a 64-bit int type to handle them. */
static uint64 poll_rx_ts;
static uint64 resp_tx_ts;
static uint64 final_rx_ts;
/* Speed of light in air, in metres per second. */
#define SPEED_OF_LIGHT 299702547
/* Hold copies of computed time of flight and distance here for reference so that it can be examined at a debug breakpoint. */
static double tof;
static double distance;
/* String used to display measured distance on LCD screen (16 characters maximum). */
char dist_str[16] = {0};
/* Declaration of static functions. */
static uint64 get_tx_timestamp_u64(void);
static uint64 get_rx_timestamp_u64(void);
static void final_msg_get_ts(const uint8 *ts_field, uint32 *ts);
pFunc DW1000_ExtiCallback = NULL;
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
if(GPIO_Pin == DW1000_IRQn_Pin)
{
DW1000_ExtiCallback();
}
}
static void rx_ok_cb( const dwt_cb_data_t *cb_data )
{
for (int i = 0 ; i < RX_BUF_LEN; i++ )
{
rx_buffer[i] = 0;
}
if (cb_data->datalength <= RX_BUF_LEN) {
DWT_ReadRxData(rx_buffer, cb_data->datalength, 0);
rec_ok = 1;
}
}
static void rx_err_cb( const dwt_cb_data_t *cb_data )
{
status1 = DWT_GetSysStatus();
rec_err = 1;
DWT_RxReset();
}
int double_sided_interrupt_responder(void)
{
DWT_SetSpeedLow();
DWT_Initialise(&dwtConfig);
DWT_SetSpeedHigh();
DWT_Configure(&dwtConfig);
DWT_SetCallbacks(NULL, &rx_ok_cb, NULL, &rx_err_cb);
DWT_SetInterrupt(DWT_INT_RFCG| DWT_INT_RFTO | DWT_INT_RXPTO | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFSL | DWT_INT_SFDT, 1);
DWT_SetRxAntennaDelay(RX_ANT_DLY);
DWT_SetTxAntennaDelay(TX_ANT_DLY);
DWT_RxEnable(DWT_RX_IMMEDIATE);
while (1) {
if(rec_err)
{
rec_err = 0;
DWT_RxEnable(DWT_RX_IMMEDIATE);
}
if(rec_ok)
{
rec_ok = 0;
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) {
uint32_t resp_tx_time;
int ret;
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();
/* Set send time for response. See NOTE 9 below. */
resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
DWT_SetDelayedTxRxTime(resp_tx_time);
/* Set expected delay and timeout for final message reception. See NOTE 4 and 5 below. */
DWT_SetRxAfterTxDelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
// DWT_SetRxTimeout(FINAL_RX_TIMEOUT_UUS);
/* Write and send the response message. See NOTE 10 below.*/
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
DWT_WriteTxData(sizeof(tx_resp_msg), tx_resp_msg, 0); /* Zero offset in TX buffer. */
DWT_WriteTxFCtrl(sizeof(tx_resp_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
ret = DWT_StartTx(DWT_RX_IMMEDIATE);
/* If dwt_starttx() returns an error, abandon this ranging exchange and proceed to the next one. See NOTE 11 below. */
if (ret == DWT_ERROR)
{
continue;
}
while(!(DWT_GetSysStatus() & SYS_STATUS_TXFRS));
DWT_SetSysStatus(SYS_STATUS_TXFRS);
DWT_RxEnable(DWT_RX_IMMEDIATE);
}
while(!(rec_ok | rec_err))
{
};
rec_ok = 0;
rec_err = 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 12 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;
DWT_RxEnable(DWT_RX_IMMEDIATE);
}
}
// HAL_Delay(RNG_DELAY_MS);
}
}
/*! ------------------------------------------------------------------------------------------------------------------
* @fn get_tx_timestamp_u64()
*
* @brief Get the TX time-stamp in a 64-bit variable.
* /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX!
*
* @param none
*
* @return 64-bit value of the read time-stamp.
*/
static uint64 get_tx_timestamp_u64(void)
{
uint8 ts_tab[5];
uint64 ts = 0;
int i;
DWT_ReadTxTimestamp(ts_tab);
for (i = 4; i >= 0; i--)
{
ts <<= 8;
ts |= ts_tab[i];
}
return ts;
}
/*! ------------------------------------------------------------------------------------------------------------------
* @fn get_rx_timestamp_u64()
*
* @brief Get the RX time-stamp in a 64-bit variable.
* /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX!
*
* @param none
*
* @return 64-bit value of the read time-stamp.
*/
static uint64 get_rx_timestamp_u64(void)
{
uint8 ts_tab[5];
uint64 ts = 0;
int i;
DWT_ReadRxTimestamp(ts_tab);
for (i = 4; i >= 0; i--)
{
ts <<= 8;
ts |= ts_tab[i];
}
return ts;
}
/*! ------------------------------------------------------------------------------------------------------------------
* @fn final_msg_get_ts()
*
* @brief Read a given timestamp value from the final message. In the timestamp fields of the final message, the least
* significant byte is at the lower address.
*
* @param ts_field pointer on the first byte of the timestamp field to read
* ts timestamp value
*
* @return none
*/
static void final_msg_get_ts(const uint8 *ts_field, uint32 *ts)
{
int i;
*ts = 0;
for (i = 0; i < FINAL_MSG_TS_LEN; i++)
{
*ts += ts_field[i] << (i * 8);
}
}
This is code for initator
/*
* double_sided_interrupt_initator.c
*
* Created on: Jan 13, 2024
* Author: Admin
*/
#include "double_sided_interrupt_initator.h"
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 1000
__IO uint8_t rec_ok = 0;
__IO uint8_t rec_err = 0;
uint32_t status1 = 0;
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
static DWT_ConfigTypeDef dwtConfig = {
.LoadCode = DW_LOAD_UCODE,
.Channel = DW_CHANNEL_2,
.PulseRepFreq = DW_PRF_64M,
.TxPreambLen = DW_TX_PLEN_1024,
.PreambleAcqChunk = DW_PAC_32,
.TxCode = 9,
.RxCode = 9,
.NonStandardSFD = ENABLE,
.DataRate = DW_DATARATE_110K,
.PhrMode = DW_PHR_MODE_STD,
.SFDTimeout = (1025 + 64 - 32) // TxPreambLen + 1 + SFD length - PAC
};
/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#define TX_ANT_DLY 16436
#define RX_ANT_DLY 16436
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 tx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8 rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8 tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
/* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */
#define ALL_MSG_COMMON_LEN 10
/* Indexes to access some of the fields in the frames defined above. */
#define ALL_MSG_SN_IDX 2
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4
/* Frame sequence number, incremented after each transmission. */
static uint8 frame_seq_nb = 0;
/* Buffer to store received response message.
* Its size is adjusted to longest frame that this example code is supposed to handle. */
#define RX_BUF_LEN 20
static uint8 rx_buffer[RX_BUF_LEN];
/* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
* 1 uus = 512 / 499.2 �s and 1 �s = 499.2 * 128 dtu. */
#define UUS_TO_DWT_TIME 65536
/* Delay between frames, in UWB microseconds. See NOTE 4 below. */
/* 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. */
#define POLL_TX_TO_RESP_RX_DLY_UUS 300
/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the
* frame length of approximately 2.66 ms with above configuration. */
#define RESP_RX_TO_FINAL_TX_DLY_UUS 3100
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 2700
/* Preamble timeout, in multiple of PAC size. See NOTE 6 below. */
#define PRE_TIMEOUT 8
/* Time-stamps of frames transmission/reception, expressed in device time units.
* As they are 40-bit wide, we need to define a 64-bit int type to handle them. */
static uint64 poll_tx_ts;
static uint64 resp_rx_ts;
static uint64 final_tx_ts;
/* Declaration of static functions. */
static uint64 get_tx_timestamp_u64(void);
static uint64 get_rx_timestamp_u64(void);
static void final_msg_set_ts(uint8 *ts_field, uint64 ts);
pFunc DW1000_ExtiCallback = NULL;
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
if(GPIO_Pin == DW1000_IRQn_Pin)
{
DW1000_ExtiCallback();
}
}
static void rx_ok_cb( const dwt_cb_data_t *cb_data )
{
/* Clear good RX frame event in the DW1000 status register. */
DWT_SetSysStatus(SYS_STATUS_RXFCG);
for (int i = 0 ; i < RX_BUF_LEN; i++ )
{
rx_buffer[i] = 0;
}
if (cb_data->datalength <= RX_BUF_LEN) {
DWT_ReadRxData(rx_buffer, cb_data->datalength, 0);
rec_ok = 1;
}
}
static void rx_err_cb( const dwt_cb_data_t *cb_data )
{
status1 = DWT_GetSysStatus();
rec_err = 1;
DWT_RxReset();
}
/*! ------------------------------------------------------------------------------------------------------------------
* @fn main()
*
* @brief Application entry point.
*
* @param none
*
* @return none
*/
int double_sided_interrupt_initator(void)
{
DWT_SetSpeedLow();
DWT_Initialise(&dwtConfig);
DWT_SetSpeedHigh();
DWT_Configure(&dwtConfig);
DWT_SetCallbacks(NULL, &rx_ok_cb, NULL, &rx_err_cb);
DWT_SetInterrupt(DWT_INT_RFCG| DWT_INT_RFTO | DWT_INT_RXPTO | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFSL | DWT_INT_SFDT, 1);
DWT_SetRxAntennaDelay(RX_ANT_DLY);
DWT_SetTxAntennaDelay(TX_ANT_DLY);
/* Write frame data to DW1000 and prepare transmission. See NOTE 8 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
DWT_WriteTxData(sizeof(tx_poll_msg), tx_poll_msg, 0); /* Zero offset in TX buffer. */
DWT_WriteTxFCtrl(sizeof(tx_poll_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
/* 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. */
DWT_StartTx(DWT_TX_IMMEDIATE);
while(!(DWT_GetSysStatus() & SYS_STATUS_TXFRS));
/* Activate reception immediately. */
DWT_SetSysStatus(SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the poll message (modulo 256). */
frame_seq_nb++;
DWT_RxEnable(DWT_RX_IMMEDIATE);
while (1) {
if(rec_err)
{
/* Write frame data to DW1000 and prepare transmission. See NOTE 8 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
DWT_WriteTxData(sizeof(tx_poll_msg), tx_poll_msg, 0); /* Zero offset in TX buffer. */
DWT_WriteTxFCtrl(sizeof(tx_poll_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
/* 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. */
DWT_StartTx(DWT_TX_IMMEDIATE);
while(!(DWT_GetSysStatus() & SYS_STATUS_TXFRS));
/* Activate reception immediately. */
DWT_SetSysStatus(SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the poll message (modulo 256). */
frame_seq_nb++;
DWT_RxEnable(DWT_RX_IMMEDIATE);
rec_err = 0;
}
if(rec_ok)
{
rec_ok = 0;
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0) {
uint32_t final_tx_time;
int ret;
/* 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 10 below. */
final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
DWT_SetRxAfterTxDelay(final_tx_time);
/* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFEUL)) << 8) + TX_ANT_DLY;
/* Write all timestamps in the final message. See NOTE 11 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 8 below. */
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
DWT_WriteTxData(sizeof(tx_final_msg), tx_final_msg, 0); /* Zero offset in TX buffer. */
DWT_WriteTxFCtrl(sizeof(tx_final_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
ret = DWT_StartTx(DWT_TX_IMMEDIATE);
while(!(DWT_GetSysStatus() & SYS_STATUS_TXFRS));
/* Activate reception immediately. */
DWT_SetSysStatus(SYS_STATUS_TXFRS);
HAL_Delay(RNG_DELAY_MS);
frame_seq_nb++;
/* Write frame data to DW1000 and prepare transmission. See NOTE 8 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
DWT_WriteTxData(sizeof(tx_poll_msg), tx_poll_msg, 0); /* Zero offset in TX buffer. */
DWT_WriteTxFCtrl(sizeof(tx_poll_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
/* 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. */
DWT_StartTx(DWT_TX_IMMEDIATE);
while(!(DWT_GetSysStatus() & SYS_STATUS_TXFRS));
/* Activate reception immediately. */
DWT_SetSysStatus(SYS_STATUS_TXFRS);
DWT_RxEnable(DWT_RX_IMMEDIATE);
}
}
}
}
/*! ------------------------------------------------------------------------------------------------------------------
* @fn get_tx_timestamp_u64()
*
* @brief Get the TX time-stamp in a 64-bit variable.
* /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX!
*
* @param none
*
* @return 64-bit value of the read time-stamp.
*/
static uint64 get_tx_timestamp_u64(void)
{
uint8_t ts_tab[5];
uint64 ts = 0;
int i;
DWT_ReadTxTimestamp(ts_tab);
for (i = 4; i >= 0; i--)
{
ts <<= 8;
ts |= ts_tab[i];
}
return ts;
}
/*! ------------------------------------------------------------------------------------------------------------------
* @fn get_rx_timestamp_u64()
*
* @brief Get the RX time-stamp in a 64-bit variable.
* /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX!
*
* @param none
*
* @return 64-bit value of the read time-stamp.
*/
static uint64 get_rx_timestamp_u64(void)
{
uint8_t ts_tab[5];
uint64 ts = 0;
int i;
DWT_ReadRxTimestamp(ts_tab);
for (i = 4; i >= 0; i--)
{
ts <<= 8;
ts |= ts_tab[i];
}
return ts;
}
/*! ------------------------------------------------------------------------------------------------------------------
* @fn final_msg_set_ts()
*
* @brief Fill a given timestamp field in the final message with the given value. In the timestamp fields of the final
* message, the least significant byte is at the lower address.
*
* @param ts_field pointer on the first byte of the timestamp field to fill
* ts timestamp value
*
* @return none
*/
static void final_msg_set_ts(uint8 *ts_field, uint64 ts)
{
int i;
for (i = 0; i < FINAL_MSG_TS_LEN; i++)
{
ts_field[i] = (uint8_t) ts;
ts >>= 8;
}
}
