I am implementing a Double-Sided Two-Way Ranging (DS-TWR) system using DW3000 UWB transceivers, based on the Qorvo/Decawave examples. My Responder device successfully receives the initial “poll” message from the Initiator, but it hangs indefinitely when it tries to send the “response” message.
The Problem
The code execution freezes and never returns from the dwt_starttx function call. This occurs in the Responder’s main loop after a valid poll has been received.
Relevant Responder Code Snippet:
// ... after receiving a poll and reading the RX timestamp ...
/* 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_setdelayedtrxtime(resp_tx_time);
// ... set RX after TX delay, etc. ...
/* 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);
dwt_writetxfctrl(sizeof(tx_resp_msg)+FCS_LEN, 0, 1);
/* The program hangs on the following line and never returns */
ret = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
if (ret == DWT_ERROR) {
continue;
}
// ... code never reaches here ...
Debugging Information
By stepping into the dwt_starttx driver function, I’ve pinpointed the exact cause of the hang. The code is stuck in a loop waiting for a status bit to clear, which never happens. The problematic check is:
// Inside dwt_starttx()
// This condition is perpetually false, causing an infinite loop (or timeout).
uint8_t checkTxOK = dwt_read8bitoffsetreg(dw, SYS_STATUS_ID, 3U);
if ((checkTxOK & (uint8_t)(SYS_STATUS_HPDWARN_BIT_MASK >> 24UL)) == 0U)
The HPDWARN bit (Half Period Delay Warning) in the SYS_STATUS register is set and never clears. The DW3000 datasheet states this warning occurs if the scheduled transmission time is too close to the current time, or has already passed. This suggests an issue with my timing calculations.
My Questions
- What are the common reasons for the
HPDWARNflag to be persistently set when scheduling a delayed transmission? - Looking at my Responder’s configuration and timing logic, is there a flaw in how I calculate
resp_tx_timebased on the poll’s reception timestamp (poll_rx_ts)? - Are the constants
POLL_RX_TO_RESP_TX_DLY_UUS(900 us) andUUS_TO_DWT_TIMEcorrectly applied?
Full Code for Context
Below is the complete, self-contained code for both the Responder and Initiator roles for full reproducibility.
/**
* @brief Initializes and runs the UWB peripheral in Responder mode.
*/
static void bsp_init_uwb_responder(void)
{
struct dwt_probe_s *dw3000_probe_interf;
/* Initialize the UWB */
gpio_toggle(GPIO_LED_GREEN);
dw3000_hw_reset();
dw3000_hw_init();
dw3000_probe_interf = deca_get_probe_interface();
dwt_probe(dw3000_probe_interf);
if (dwt_initialise(DWT_DW_INIT) == DWT_ERROR) {
while (1) { /* spin */ };
}
/* Check device ID is correct (0xDECA0302) */
volatile uint32_t dev_id = dwt_readdevid();
if ((int)dev_id != DWT_DW3000_DEV_ID)
{
/* Handle the error */
}
/* Default communication configuration. We use STS with SDC DW mode. */
static dwt_config_t config = {
5, /* Channel number. */
DWT_PLEN_64, /* Preamble length. Used in TX only. */
DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
9, /* TX preamble code. Used in TX only. */
9, /* RX preamble code. Used in RX only. */
1, /* 0 to use standard 8 symbol SFD,
* 1 to use non-standard 8 symbol,
* 2 for non-standard 16 symbol SFD and
* 3 for 4z 8 symbol SDF type */
DWT_BR_6M8, /* Data rate. */
DWT_PHRMODE_STD, /* PHY header mode. */
DWT_PHRRATE_STD, /* PHY header rate. */
(65 + 8 - 8), /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
DWT_STS_MODE_1 | DWT_STS_MODE_SDC, /* STS mode 1 with SDC see NOTE on SDC below*/
DWT_STS_LEN_64, /* STS length see allowed values in Enum dwt_sts_lengths_e */
DWT_PDOA_M0 /* PDOA mode off */
};
/* have some delay after each range (e.g. so LDC can be updated (on ARM eval
* boards), needs to be slightly less than RNG_DELAY_MS in the initiator
* example */
#define DELAY_MS 980
/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#define TX_ANT_DLY 16385
#define RX_ANT_DLY 16385
/* 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
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8_t rx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21};
static uint8_t tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0};
static uint8_t 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};
/* 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
/* Frame sequence number, incremented after each transmission. */
static uint8_t 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_t rx_buffer[RX_BUF_LEN];
/* Hold copy of status register state here for reference so that it can be
* examined at a debug breakpoint. */
static uint32_t status_reg = 0;
/* 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 DW3000's delayed TX function. This includes the
* frame length of approximately 180 us with above configuration. */
#define POLL_RX_TO_RESP_TX_DLY_UUS 900
/* This is the delay from the end of the frame transmission to the enable of
* the receiver, as programmed for the DW3000's wait for response feature. */
#define RESP_TX_TO_FINAL_RX_DLY_UUS 670
/* Receive final timeout. See NOTE 5 below. */
#define FINAL_RX_TIMEOUT_UUS 300
/* Preamble timeout, in multiple of PAC size. See NOTE 6 below. */
#define PRE_TIMEOUT 5
/* Speed of light in air, in metres per second. */
#define SPEED_OF_LIGHT 299702547
/* Timestamps of frames transmission/reception. */
static uint8_t poll_rx_ts_bytes[5];
static uint8_t resp_tx_ts_bytes[5];
static uint8_t final_rx_ts_bytes[5];
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;
/* 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;
/* TX Power Configuration Settings
* Values for the PG_DELAY and TX_POWER registers reflect the bandwidth and power of the spectrum at the current
* temperature. These values can be calibrated prior to taking reference measurements. */
dwt_txconfig_t txconfig_options =
{
0x34, /* PG delay. */
0xfdfdfdfd, /* TX power. */
0x0 /* PG count. */
};
/* Need to make sure DW IC is in IDLE_RC before proceeding */
while (!dwt_checkidlerc())
{
/* Add a timeout */
};
/* Configure DW IC. See NOTE 13 below. */
if (dwt_configure(&config))
{
while (1) { };
}
/* Configure the TX spectrum parameters (power, PG delay and PG count). */
dwt_configuretxrf(&txconfig_options);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Loop forever responding to ranging requests. */
while (1) {
gpio_toggle(GPIO_LED_BLUE);
/* turn off preamble timeout as the responder does not know when the
* poll is coming. */
dwt_setpreambledetecttimeout(0);
/* Clear reception timeout to start next ranging process. */
dwt_setrxtimeout(0);
/* Activate reception immediately. */
dwt_rxenable(DWT_START_RX_IMMEDIATE);
/* Poll for reception of a frame or error/timeout. See NOTE 8 below. */
while (!((status_reg = dwt_read_reg(SYS_STATUS_ID)) &
(SYS_STATUS_RXFCG_BIT_MASK |
SYS_STATUS_ALL_RX_TO |
SYS_STATUS_ALL_RX_ERR)))
{ /* spin */ };
if (status_reg & SYS_STATUS_RXFCG_BIT_MASK) {
uint32_t frame_len;
int16_t stsqual;
/* Clear good RX frame event in the DW3000 status register. */
dwt_write_reg(SYS_STATUS_ID, SYS_STATUS_RXFCG_BIT_MASK);
/* As STS mode is used, we only consider frames that are received
* with good STS quality. If STS is good this will be true >= 0*/
if (dwt_readstsquality(&stsqual, (int32_t)NULL)) {
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read_reg(RX_FINFO_ID) & RX_FINFO_STD_RXFLEN_MASK;
if (frame_len <= RX_BUF_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) {
uint32_t resp_tx_time;
int ret;
/* Retrieve poll transmission and response reception timestamp.
* Read the 40-bit TX and RX timestamp into the byte array. */
dwt_readrxtimestamp(poll_rx_ts_bytes, 0);
poll_rx_ts = 0;
for (int i = 4; i >= 0; i--) {
poll_rx_ts = (poll_rx_ts << 8) | poll_rx_ts_bytes[i];
}
/* 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_setdelayedtrxtime(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)+FCS_LEN, 0, 1); /* Zero offset in TX buffer, ranging. */
/* Set preamble timeout for expected final frame from the
* initiator. See NOTE 6 below. */
dwt_setpreambledetecttimeout(PRE_TIMEOUT);
ret = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
/* 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;
}
/* Poll for reception of expected "final" frame or
* error/timeout. See NOTE 8 below. */
while (!((status_reg = dwt_read_reg(SYS_STATUS_ID)) &
(SYS_STATUS_RXFCG_BIT_MASK |
SYS_STATUS_ALL_RX_TO |
SYS_STATUS_ALL_RX_ERR)))
{ /* spin */ };
/* Increment frame sequence number after transmission
* of the response message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG_BIT_MASK) {
/* Clear good RX frame event and TX frame sent in
* the DW3000 status register. */
dwt_write_reg(SYS_STATUS_ID, SYS_STATUS_RXFCG_BIT_MASK |
SYS_STATUS_TXFRS_BIT_MASK);
/* As STS mode is used, we only consider frames that
* are received with good STS quality.
* If STS is good this will be true >= 0 */
if (dwt_readstsquality(&stsqual, (int32_t)NULL)) {
/* A frame has been received, read it into
* the local buffer. */
frame_len = dwt_read_reg(RX_FINFO_ID) & RX_FINFO_STD_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_t poll_tx_ts_32, resp_rx_ts_32, final_tx_ts_32;
uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32_local;
double Ra, Rb, Da, Db;
int64_t tof_dtu;
/* Retrieve poll transmission and response reception timestamp.
* Read the 40-bit TX and RX timestamp into the byte array. */
dwt_readtxtimestamp(resp_tx_ts_bytes);
dwt_readrxtimestamp(final_rx_ts_bytes, 0);
resp_tx_ts = 0;
for (int i = 4; i >= 0; i--) {
resp_tx_ts = (resp_tx_ts << 8) | resp_tx_ts_bytes[i];
}
final_rx_ts = 0;
for (int i = 4; i >= 0; i--) {
final_rx_ts = (final_rx_ts << 8) | final_rx_ts_bytes[i];
}
/* Get timestamps embedded in the final message. */
uint8_t i;
poll_tx_ts_32 = 0;
for (i = 0; i < 4; i++)
{
poll_tx_ts_32 += ((uint32_t)rx_buffer[FINAL_MSG_POLL_TX_TS_IDX + i] << (i * 8));
}
resp_rx_ts_32 = 0;
for (i = 0; i < 4; i++)
{
resp_rx_ts_32 += ((uint32_t)rx_buffer[FINAL_MSG_RESP_RX_TS_IDX + i] << (i * 8));
}
final_tx_ts_32 = 0;
for (i = 0; i < 4; i++)
{
final_tx_ts_32 += ((uint32_t)rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX + i] << (i * 8));
}
/* Compute time of flight. 32-bit subtractions
* give correct answers even if clock has wrapped.
* See NOTE 12 below. */
poll_rx_ts_32 = (uint32_t)poll_rx_ts;
resp_tx_ts_32 = (uint32_t)resp_tx_ts;
final_rx_ts_32_local = (uint32_t)final_rx_ts;
Ra = (double)(resp_rx_ts_32 - poll_tx_ts_32);
Rb = (double)(final_rx_ts_32_local - resp_tx_ts_32);
Da = (double)(final_tx_ts_32 - resp_rx_ts_32);
Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);
tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
}
}
}
else {
/* Clear RX error/timeout events in the DW3000 status register. */
dwt_write_reg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
}
}
}
}
else {
/* Clear RX error/timeout events in the DW3000 status register. */
dwt_write_reg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO |
SYS_STATUS_ALL_RX_ERR);
}
/* add some delay before next ranging exchange */
HAL_Delay(DELAY_MS);
}
}
/**
* @brief Initializes and runs the UWB peripheral in Initiator mode.
*/
static void bsp_init_uwb_initiator(void)
{
struct dwt_probe_s *dw3000_probe_interf;
/* Initialize the UWB */
gpio_toggle(GPIO_LED_GREEN);
dw3000_hw_reset();
dw3000_hw_init();
dw3000_probe_interf = deca_get_probe_interface();
dwt_probe(dw3000_probe_interf);
if (dwt_initialise(DWT_DW_INIT) == DWT_ERROR) {
while (1) { /* spin */ };
}
/* Check device ID is correct (0xDECA0302) */
volatile uint32_t dev_id = dwt_readdevid();
if ((int)dev_id != DWT_DW3000_DEV_ID)
{
/* Handle the error */
}
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 1000
/* Default communication configuration. We use STS with SDC DW mode. */
static dwt_config_t config = {
5, /* Channel number. */
DWT_PLEN_64, /* Preamble length. Used in TX only. */
DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
9, /* TX preamble code. Used in TX only. */
9, /* RX preamble code. Used in RX only. */
1, /* 0 to use standard 8 symbol SFD,
* 1 to use non-standard 8 symbol,
* 2 for non-standard 16 symbol SFD and
* 3 for 4z 8 symbol SDF type */
DWT_BR_6M8, /* Data rate. */
DWT_PHRMODE_STD, /* PHY header mode. */
DWT_PHRRATE_STD, /* PHY header rate. */
(65 + 8 - 8), /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
DWT_STS_MODE_1 | DWT_STS_MODE_SDC, /* STS mode 1 with SDC see NOTE on SDC below*/
DWT_STS_LEN_64,/* STS length see allowed values in Enum dwt_sts_lengths_e */
DWT_PDOA_M0 /* PDOA mode off */
};
/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#define TX_ANT_DLY 16385
#define RX_ANT_DLY 16385
/* 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
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8_t tx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21};
static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0};
static uint8_t 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};
/* 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
/* Frame sequence number, incremented after each transmission. */
static uint8_t 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_t rx_buffer[RX_BUF_LEN];
/* Hold copy of status register state here for reference so that it can be
* examined at a debug breakpoint. */
static uint32_t status_reg = 0;
/* 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 DW3000's wait for response feature. */
#define POLL_TX_TO_RESP_RX_DLY_UUS 690
/* This is the delay from Frame RX timestamp to TX reply timestamp used for
* calculating/setting the DW3000's delayed TX function. This includes the
* frame length of approximately 180 us with above configuration. */
#define RESP_RX_TO_FINAL_TX_DLY_UUS 880
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 1200
/* Preamble timeout, in multiple of PAC size. See NOTE 6 below. */
#define PRE_TIMEOUT 5
/* Time-stamps of frames transmission/reception, expressed
* in device time units. */
static uint8_t poll_tx_ts_bytes[5];
static uint8_t resp_rx_ts_bytes[5];
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
static uint32_t final_tx_ts;
/* TX Power Configuration Settings
* Values for the PG_DELAY and TX_POWER registers reflect the bandwidth and power of the spectrum at the current
* temperature. These values can be calibrated prior to taking reference measurements. */
dwt_txconfig_t txconfig_options =
{
0x34, /* PG delay. */
0xfdfdfdfd, /* TX power. */
0x0 /* PG count. */
};
/* Need to make sure DW IC is in IDLE_RC before proceeding */
while (!dwt_checkidlerc())
{
/* Add a timeout */
};
if (dwt_initialise(DWT_DW_INIT) == DWT_ERROR) {
while (1) { /* spin */ };
}
/* Configure DW IC. See NOTE 13 below. */
if (dwt_configure(&config))
{
while (1) { };
}
/* Configure the TX spectrum parameters (power, PG delay and PG count). */
dwt_configuretxrf(&txconfig_options);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Set expected response's delay and timeout. See NOTE 4, 5 and 6 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);
dwt_setpreambledetecttimeout(PRE_TIMEOUT);
/* Loop forever initiating ranging exchanges. */
while (1)
{
gpio_toggle(GPIO_LED_BLUE);
/* Write frame data to DW3000 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)+FCS_LEN, 0, 1); /* Zero offset in TX buffer, ranging. */
/* clear all events */
dwt_write_reg(SYS_STATUS_ID, 0xFFFFFFFF);
/* 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_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
/* We assume that the transmission is achieved correctly, poll for
* reception of a frame or error/timeout. See NOTE 9 below. */
while (!((status_reg = dwt_read_reg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG_BIT_MASK |
SYS_STATUS_ALL_RX_TO |
SYS_STATUS_ALL_RX_ERR)))
{ /* spin */ };
/* Increment frame sequence number after transmission of the poll
* message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG_BIT_MASK)
{
uint32_t frame_len;
int16_t stsqual;
/* Clear good RX frame event and TX frame sent in the DW3000 status register. */
dwt_write_reg(SYS_STATUS_ID, SYS_STATUS_RXFCG_BIT_MASK | SYS_STATUS_TXFRS_BIT_MASK);
/* As STS is used, we only consider frames that are received
* with good STS quality */
if (dwt_readstsquality(&stsqual, (int32_t)NULL))
{
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read_reg(RX_FINFO_ID) & RX_FINFO_STD_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 STS-SDC 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_t final_tx_time;
int ret;
/* Retrieve poll transmission and response reception timestamp.
* Read the 40-bit TX and RX timestamp into the byte array. */
dwt_readtxtimestamp(poll_tx_ts_bytes);
dwt_readrxtimestamp(resp_rx_ts_bytes, 0);
poll_tx_ts = 0;
for (int i = 4; i >= 0; i--) {
poll_tx_ts = (poll_tx_ts << 8) | poll_tx_ts_bytes[i];
}
resp_rx_ts = 0;
for (int i = 4; i >= 0; i--) {
resp_rx_ts = (resp_rx_ts << 8) | resp_rx_ts_bytes[i];
}
/* 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_setdelayedtrxtime(final_tx_time);
/* Final TX timestamp is the transmission time we
* programmed plus the TX antenna delay. */
final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFEUL)) << 8) + TX_ANT_DLY;
/* Write all timestamps in the final message.
* See NOTE 11 below. */
uint64_t temp_ts;
temp_ts = poll_tx_ts;
for (int i = 0; i < 5; i++) // Assumes a 5-byte (40-bit) timestamp length
{
tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX + i] = (uint8_t)temp_ts;
temp_ts >>= 8;
}
temp_ts = resp_rx_ts;
for (int i = 0; i < 5; i++) // Assumes a 5-byte (40-bit) timestamp length
{
tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX + i] = (uint8_t)temp_ts;
temp_ts >>= 8;
}
temp_ts = final_tx_ts;
for (int i = 0; i < 5; i++) // Assumes a 5-byte (40-bit) timestamp length
{
tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX + i] = (uint8_t)temp_ts;
temp_ts >>= 8;
}
/* 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)+FCS_LEN, 0, 1); /* Zero offset in TX buffer, ranging. */
ret = dwt_starttx(DWT_START_TX_DELAYED);
/* If dwt_starttx() returns an error, abandon this ranging
* exchange and proceed to the next one. See NOTE 12 below. */
if (ret == DWT_SUCCESS) {
/* Poll DW3000 until TX frame sent event set. See NOTE 9 below. */
while (!(dwt_read_reg(SYS_STATUS_ID) & SYS_STATUS_TXFRS_BIT_MASK))
{ /* spin */ };
/* Clear TXFRS event. */
dwt_write_reg(SYS_STATUS_ID, SYS_STATUS_TXFRS_BIT_MASK);
/* Increment frame sequence number after transmission
* of the final message (modulo 256). */
frame_seq_nb++;
}
}
}
}
else {
/* Clear RX error/timeout events in the DW3000
*status register. */
dwt_write_reg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
}
/* Execute a delay between ranging exchanges. */
HAL_Delay(RNG_DELAY_MS);
}
}