Hi,
I’m currently doing my master thesis in computer science and I’m trying to create a system to test the accuracy of UWB ranging.
I’m trying to perform double sided two-way ranging using the ESP32 Dw3000 from makerfabs. - Eventually the goal is to perform asymmetric DB TWR with multiple anchors.
The code base is written in c++ inspired by QORVOS own example code of ‘ds_twr_responder’ and ‘ds_twr_initiator’.
I’m using the Arduino IDE and the ESP32 Dw3000 library from here.
However I’m having some issues with the code, as the final message from the initator to the responder gives me an error, which I cannot decode.
When the responder polls for the final message it reads the system status register for errors, which includes checking the following subregisters: \
All RX error masks:
- SYS_STATUS_RXPHE
- SYS_STATUS_RXFCE
- SYS_STATUS_RXFSL
- SYS_STATUS_RXSTO
- SYS_STATUS_ARFE
- SYS_STATUS_CIAERR
User defined RX error masks:
- SYS_STATUS_RXFTO_BIT_MASK
- SYS_STATUS_RXPTO_BIT_MASK
and the RXFCG subregister.
Printing the system status register gives me the following output:
“131312” which is the decimal representation of the hex value 200F0 and the binary representation of 100000000011110000.
I know it should be possible to decode which subregister the error is coming from, however I have a hard time figuring it out.\
The code for the intiator and responder is attached below.
Does anyone have any idea what the error could be and how to fix it?
Additionally, I’m very interested in getting in contact with anybody having experience with the ESP32 Dw3000 from makerfabs.
Initator code:
#include "dw3000.h"
#define APP_NAME "SS TWR INIT v1.0"
// connection pins
const uint8_t PIN_RST = 27; // reset pin
const uint8_t PIN_IRQ = 34; // irq pin
const uint8_t PIN_SS = 4; // spi select pin
/* Default communication configuration. We use default non-STS DW mode. */
static dwt_config_t config = {
5, /* Channel number. */
DWT_PLEN_128, /* 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. */
(129 + 8 - 8), /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
DWT_STS_MODE_OFF, /* STS disabled */
DWT_STS_LEN_64, /* STS length see allowed values in Enum dwt_sts_lengths_e */
DWT_PDOA_M0 /* PDOA mode off */
};
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 1000
/* Default antenna delay values for 64 MHz PRF. See NOTE 2 below. */
#define TX_ANT_DLY 16385
#define RX_ANT_DLY 16385
/* Frames used in the ranging process. See NOTE 3 below. */
static uint8_t tx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'C', 'C', 'D', 'D', 0x21, 0, 0};
static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'D', 'D', 'C', 'C', 0x10, 0x02, 0, 0};
static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'C', 'C', 'D', 'D', 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 3 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 RESP_MSG_POLL_RX_TS_IDX 10
#define RESP_MSG_RESP_TX_TS_IDX 14
#define POLL_MSG_POLL_TX_TS_IDX 10
#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 RESP_MSG_TS_LEN 4
/* 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 25
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;
static uint32_t poll_tx_ts, resp_rx_ts;
/* Delay between frames, in UWB microseconds. See NOTE 1 below. */
#ifdef RPI_BUILD
#define POLL_TX_TO_RESP_RX_DLY_UUS 240
#endif // RPI_BUILD
#ifdef STM32F429xx
#define POLL_TX_TO_RESP_RX_DLY_UUS 240
#endif // STM32F429xx
#ifdef NRF52840_XXAA
#define POLL_TX_TO_RESP_RX_DLY_UUS 240
#endif // NRF52840_XXAA
/* Receive response timeout. See NOTE 5 below. */
#ifdef RPI_BUILD
#define RESP_RX_TIMEOUT_UUS 270
#endif // RPI_BUILD
#ifdef STM32F429xx
#define RESP_RX_TIMEOUT_UUS 210
#endif // STM32F429xx
#ifdef NRF52840_XXAA
#define RESP_RX_TIMEOUT_UUS 400
#endif // NRF52840_XXAA
#define POLL_TX_TO_RESP_RX_DLY_UUS 240
#define RESP_RX_TO_FINAL_TX_DLY_UUS 300
#define RESP_RX_TIMEOUT_UUS 400
/* 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, t_round_1, t_reply;
static double distance;
/* 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. See NOTE 2 below. */
extern dwt_txconfig_t txconfig_options;
void setup()
{
UART_init();
test_run_info((unsigned char *)APP_NAME);
Serial.begin(115200);
/* Configure SPI rate, DW3000 supports up to 38 MHz */
/* Reset DW IC */
spiBegin(PIN_IRQ, PIN_RST);
spiSelect(PIN_SS);
delay(2); // Time needed for DW3000 to start up (transition from INIT_RC to IDLE_RC, or could wait for SPIRDY event)
while (!dwt_checkidlerc()) // Need to make sure DW IC is in IDLE_RC before proceeding
{
UART_puts("IDLE FAILED\r\n");
while (1)
;
}
if (dwt_initialise(DWT_DW_INIT) == DWT_ERROR)
{
UART_puts("INIT FAILED\r\n");
while (1)
;
}
// Enabling LEDs here for debug so that for each TX the D1 LED will flash on DW3000 red eval-shield boards.
dwt_setleds(DWT_LEDS_ENABLE | DWT_LEDS_INIT_BLINK);
/* Configure DW IC. See NOTE 6 below. */
if (dwt_configure(&config)) // if the dwt_configure returns DWT_ERROR either the PLL or RX calibration has failed the host should reset the device
{
UART_puts("CONFIG FAILED\r\n");
while (1)
;
}
/* Configure the TX spectrum parameters (power, PG delay and PG count) */
dwt_configuretxrf(&txconfig_options);
/* Apply default antenna delay value. See NOTE 2 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* 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);
/* Next can enable TX/RX states output on GPIOs 5 and 6 to help debug, and also TX/RX LEDs
* Note, in real low power applications the LEDs should not be used. */
dwt_setlnapamode(DWT_LNA_ENABLE | DWT_PA_ENABLE);
}
void loop()
{
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS_BIT_MASK);
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_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 8 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG_BIT_MASK | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{
};
/* 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;
/* Clear good RX frame event & TX frame sent in the DW IC status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG_BIT_MASK);
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS_BIT_MASK);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RXFLEN_MASK;
if (frame_len <= sizeof(rx_buffer))
{
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_t final_tx_ts, final_tx_time;
int ret;
float clockOffsetRatio;
/* Retrieve poll transmission and response reception timestamps. See NOTE 9 below. */
poll_tx_ts = dwt_readtxtimestamplo32();
resp_rx_ts = dwt_readrxtimestamplo32();
/* Read carrier integrator value and calculate clock offset ratio. See NOTE 11 below. */
clockOffsetRatio = ((float)dwt_readclockoffset()) / (uint32_t)(1 << 26);
/* Compute final message transmission time. See NOTE 7 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 antenna delay. */
final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFEUL)) << 8) + TX_ANT_DLY;
/* Write resp_rx_ts & final_tx_ts in the final message. See NOTE 8 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 the response message. See NOTE 9 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_START_TX_DELAYED);
if (ret == DWT_SUCCESS)
{
test_run_info((unsigned char *)"Succes");
/* Poll DW IC until TX frame sent event set. See NOTE 6 below. */
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS_BIT_MASK))
{
};
Serial.println("final message: ");
for (int i = 0; i < sizeof(tx_final_msg); i++)
{
Serial.print("[");
Serial.print(i);
Serial.print("] : ");
Serial.print(tx_final_msg[i]);
Serial.println(" ");
}
Serial.println(" ");
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS_BIT_MASK);
/* Increment frame sequence number after transmission of the poll message (modulo 256). */
frame_seq_nb++;
}
}
}
else
{
/* Clear RX error/timeout events in the DW IC status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_TXFRS_BIT_MASK);
}
/* Execute a delay between ranging exchanges. */
Sleep(RNG_DELAY_MS);
}
Responder code:
#include "dw3000.h"
#define APP_NAME "DS TWR RESP v1.0"
// connection pins
const uint8_t PIN_RST = 27; // reset pin
const uint8_t PIN_IRQ = 34; // irq pin
const uint8_t PIN_SS = 4; // spi select pin
/* Default communication configuration. We use default non-STS DW mode. */
static dwt_config_t config = {
5, /* Channel number. */
DWT_PLEN_128, /* 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. */
(129 + 8 - 8), /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
DWT_STS_MODE_OFF, /* STS disabled */
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 2 below. */
#define TX_ANT_DLY 16385
#define RX_ANT_DLY 16385
/* Frames used in the ranging process. See NOTE 3 below. */
static uint8_t rx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'C', 'C', 'D', 'D', 0x21, 0, 0};
static uint8_t tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'D', 'D', 'C', 'C', 0x10, 0x02, 0, 0, 0, 0};
static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'C', 'C', 'D', 'D', 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 3 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 RESP_MSG_POLL_RX_TS_IDX 10
#define RESP_MSG_RESP_TX_TS_IDX 14
#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 RESP_MSG_TS_LEN 4
/* 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 25 // Must be less than FRAME_LEN_MAX_EX
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 1 below. */
#ifdef RPI_BUILD
#define POLL_RX_TO_RESP_TX_DLY_UUS 550
#endif // RPI_BUILD
#ifdef STM32F429xx
#define POLL_RX_TO_RESP_TX_DLY_UUS 450
#endif // STM32F429xx
#ifdef NRF52840_XXAA
#define POLL_RX_TO_RESP_TX_DLY_UUS 650
#endif // NRF52840_XXAA
#define POLL_RX_TO_RESP_TX_DLY_UUS 650
#define RESP_TX_TO_FINAL_RX_DLY_UUS 500
#define FINAL_RX_TIMEOUT_UUS 220
#define PRE_TIMEOUT 5
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 1000
/* Timestamps of frames transmission/reception. */
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;
static double tof, t_round_1, t_reply_a, t_round_2, t_reply_b, distance;
/* 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. See NOTE 5 below. */
extern dwt_txconfig_t txconfig_options;
void setup()
{
UART_init();
test_run_info((unsigned char *)APP_NAME);
Serial.begin(115200);
/* Configure SPI rate, DW3000 supports up to 38 MHz */
/* Reset DW IC */
spiBegin(PIN_IRQ, PIN_RST);
spiSelect(PIN_SS);
delay(2); // Time needed for DW3000 to start up (transition from INIT_RC to IDLE_RC, or could wait for SPIRDY event)
while (!dwt_checkidlerc()) // Need to make sure DW IC is in IDLE_RC before proceeding
{
UART_puts("IDLE FAILED\r\n");
while (1)
;
}
if (dwt_initialise(DWT_DW_INIT) == DWT_ERROR)
{
UART_puts("INIT FAILED\r\n");
while (1)
;
}
// Enabling LEDs here for debug so that for each TX the D1 LED will flash on DW3000 red eval-shield boards.
dwt_setleds(DWT_LEDS_ENABLE | DWT_LEDS_INIT_BLINK);
/* Configure DW IC. See NOTE 6 below. */
if (dwt_configure(&config)) // if the dwt_configure returns DWT_ERROR either the PLL or RX calibration has failed the host should reset the device
{
UART_puts("CONFIG FAILED\r\n");
while (1)
;
}
/* Configure the TX spectrum parameters (power, PG delay and PG count) */
dwt_configuretxrf(&txconfig_options);
/* Apply default antenna delay value. See NOTE 2 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Next can enable TX/RX states output on GPIOs 5 and 6 to help debug, and also TX/RX LEDs
* Note, in real low power applications the LEDs should not be used. */
dwt_setlnapamode(DWT_LNA_ENABLE | DWT_PA_ENABLE);
}
void loop()
{
// 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 6 below. */ // Duplicate
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG_BIT_MASK | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{
};
if (status_reg & SYS_STATUS_RXFCG_BIT_MASK)
{
uint32_t frame_len;
/* Clear good RX frame event in the DW IC status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG_BIT_MASK);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RXFLEN_MASK;
// Måske denne skal gå ned som i std ex
if (frame_len <= sizeof(rx_buffer))
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* 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_t resp_tx_time;
int ret;
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();
/* Compute response 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);
/* Set expected delay and timeout for final message reception. See NOTE 1 and 1.2 below. */
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
/* Set preamble timeout for expected frames. See NOTE 6 below. */
// dwt_setpreambledetecttimeout(PRE_TIMEOUT);
/* 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); /* Zero offset in TX buffer. */
dwt_writetxfctrl(sizeof(tx_resp_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
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. */
if (ret == DWT_SUCCESS)
{
/* Poll for reception of expected "final" frame or error/timeout. See NOTE 8 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG_BIT_MASK | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{
};
Serial.print("status: ");
Serial.println(status_reg);
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG_BIT_MASK)
{
test_run_info((unsigned char *)" after okay status reg");
/* Clear good RX frame event and TX frame sent in the DW IC status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG_BIT_MASK);
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS_BIT_MASK);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RXFLEN_MASK;
if (frame_len <= sizeof(rx_buffer))
{
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, resp_rx_ts, final_tx_ts;
uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
double Ra, Rb, Da, Db;
int64_t 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_t)poll_rx_ts;
resp_tx_ts_32 = (uint32_t)resp_tx_ts;
final_rx_ts_32 = (uint32_t)final_rx_ts;
t_round_1 = (double)(resp_rx_ts - poll_tx_ts);
t_round_2 = (double)(final_rx_ts_32 - resp_tx_ts_32);
t_reply_a = (double)(final_tx_ts - resp_rx_ts);
t_reply_b = (double)(resp_tx_ts_32 - poll_rx_ts_32);
tof_dtu = (int64_t)((t_round_1 * t_round_2 - t_reply_a * t_reply_b) / (t_round_1 + t_round_2 + t_reply_a + t_reply_b));
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
test_run_info((unsigned char *)dist_str);
/* as DS-TWR initiator is waiting for RNG_DELAY_MS before next poll transmission
* we can add a delay here before RX is re-enabled again */
Sleep(RNG_DELAY_MS - 10);
}
}
else
{
/* Clear RX error events in the DW IC status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
}
}
else
{
/* Clear RX error events in the DW IC status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}