Hi all,
I use the DWM1001 to porting TWR sample code.
If not enter sleep mode, the code can work well. The source code as main_05b_no_sleep.c and log file as main_05b_no_sleep.log
If add dwt_entersleep() function after TWR flow, then has err code in dwt_starttx() function. The source code as main_05b_enter_sleep.c and log file as main_05b_enter_sleep.log
Hardware platform -> DWM1001-DEV
DW1000_DRIVER_VERSION -> 0x040006
Nordic SDK -> nRF5_SDK_12.3.0
main_05b_no_sleep.c
codeint deca_main_routine(void)
{
static int deca_cnt=0;
uint8 dw_id_data[4];
static int loop_cnt=0;
int i;
#if 0
/* Start with board specific hardware init. */
peripherals_init();
/* Display application name on LCD. */
lcd_display_str(APP_NAME);
/* 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. */
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
spi_set_rate_low();
if (dwt_initialise(DWT_LOADUCODE) == DWT_ERROR)
{
lcd_display_str("INIT FAILED");
while (1)
{ };
}
spi_set_rate_high();
#endif
#if 1
/* Configure DW1000. See NOTE 7 below. */
dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Configure and wake-up parameters. */
//dwt_configuresleep(DWT_PRESRV_SLEEP | DWT_CONFIG, DWT_WAKE_CS | DWT_SLP_EN);
dwt_configuresleep(DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV, DWT_XTAL_EN|DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN);
//dwt_configuresleep(DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV, DWT_WAKE_CS | DWT_SLP_EN);
#endif
/* Set preamble timeout for expected frames. See NOTE 6 below. */
//dwt_setpreambledetecttimeout(PRE_TIMEOUT);
// enable GPIO 5,6 to indicate Tx and RX function
//dwt_setlnapamode(1,1);
/* Loop forever responding to ranging requests. */
while (1)
{
//NRF_LOG_INFO("--loop_cnt=%d\r\n",loop_cnt++);
loop_sync_flag = 0;
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
#if 0
/* Configure DW1000. See NOTE 7 below. */
dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Configure and wake-up parameters. */
dwt_configuresleep(DWT_PRESRV_SLEEP | DWT_CONFIG, DWT_WAKE_CS | DWT_SLP_EN);
//dwt_configuresleep(DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV, DWT_WAKE_CS | DWT_SLP_EN);
#endif
if (NRF_LOG_PROCESS() == false)
{
//power_manage();
}
/* Clear reception timeout to start next ranging process. */
//dwt_setrxtimeout(0);
dwt_setrxtimeout(POLL_RX_TIMEOUT_UUS);
/* 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_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{ };
if (status_reg & SYS_STATUS_RXFCG)
{
uint32 frame_len;
NRF_LOG_INFO("s1_reg=%X---%d\r\n",status_reg,loop_cnt++);
/* 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_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
//test_p03_set();
#if 1 //DECA_DEBUG_FLAG
NRF_LOG_INFO("Poll len=%d\r\n",frame_len);
//NRF_LOG_HEXDUMP_INFO(rx_buffer, 10);
#endif
/* 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 resp_tx_time;
int ret;
#if 1 //DECA_DEBUG_FLAG
NRF_LOG_INFO("Poll ok.\r\n");
#endif
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();
//NRF_LOG_INFO("poll_rx_ts = 0x%X\r\n",(uint32)poll_rx_ts);
/* 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;
//NRF_LOG_INFO("resp_tx_ts = 0x%X,diff=0x%X\r\n",resp_tx_time,resp_tx_time-(uint32)poll_rx_ts);
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), 0, 1); /* Zero offset in TX buffer, ranging. */
//test_p03_clear();
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)
{
NRF_LOG_INFO("Err in line=%d\r\n",__LINE__);
/* Clear RX error/timeout events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
continue;
//goto deca_05b_end;
}
//test_p04_set();
/* Poll DW1000 until TX frame sent event set. See NOTE 9 below. */
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
//test_p04_clear();
//test_p03_set();
/* 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 | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{ };
//test_p03_clear();
NRF_LOG_INFO("s2_reg=%X\r\n",status_reg);
/* Increment frame sequence number after transmission of the response message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
NRF_LOG_INFO("Final len=%d\r\n",frame_len);
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 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;
uint32 final_tx_time;
int ret;
NRF_LOG_INFO("Final ok.\r\n");
/* 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;
u16_distance_cm = (uint16)(distance*100);
#if 1
// To do next, then send resp2 msg to tag----Start
/* Compute resp2 message transmission time. See NOTE 10 below. */
final_tx_time = (final_rx_ts + (FINAL_RX_TO_RESP2_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(final_tx_time);
/* Write the expected timestamp in the resp2 message. See NOTE 11 below. */
final_msg_set_ts(&tx_reap2_msg[RESP2_MSG_RESP_TX_TS_IDX], resp_tx_ts);
//final_msg_set_ts(&tx_reap2_msg[RESP2_MSG_FINAL_RX_TS_IDX], final_rx_ts);
memcpy(&tx_reap2_msg[RESP2_MSG_FINAL_RX_TS_IDX], (uint8 *)&u16_distance_cm,2);
/* Write and send final message. See NOTE 8 below. */
tx_reap2_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_reap2_msg), tx_reap2_msg, 0); /* Zero offset in TX buffer. */
dwt_writetxfctrl(sizeof(tx_reap2_msg), 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)
{
//test_p04_set();
/* Poll DW1000 until TX frame sent event set. See NOTE 9 below. */
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
//test_p04_clear();
NRF_LOG_INFO("Send Final_Resp ok.\r\n");
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
}
// To do next, then send resp2 msg to tag----End
#endif
/* Display computed distance on LCD. */
//sprintf(dist_str, "DIST: %3.2f m", distance);
//lcd_display_str(dist_str);
//NRF_LOG_INFO("DIST: %3.2f m\r\n", distance);
//NRF_LOG_INFO("deca measure: %d cm\r\n",distance*100);
NRF_LOG_INFO("distance_cm: %d cm, hex=%X \r\n",u16_distance_cm,u16_distance_cm);
loop_sync_flag = 1;
goto deca_05b_end;
}
}
else
{
/* Clear RX error/timeout events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
}
}
else
{
/* Clear RX error/timeout events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
deca_05b_end:
#if 1
if (loop_sync_flag==1)
{
NRF_LOG_INFO("Anchor deca_cnt=%d\r\n",deca_cnt++);
NRF_LOG_INFO("...... dwt_entersleep...........\r\n");
if (NRF_LOG_PROCESS() == false)
{
//power_manage();
}
/* Put DW1000 to sleep. */
dwt_entersleep();
deca_sleep(100);
#if 1
deca_spi_wakeup();
while(DWT_DEVICE_ID != dwt_readdevid())
{
dwt_readfromdevice(0,0,4,&dw_id_data[0]);
NRF_LOG_INFO("%d -> DW_ID =0x%.2X%.2X%.2X%.2X \r\n",i++,dw_id_data[0],dw_id_data[1],dw_id_data[2],dw_id_data[3]);
deca_spi_wakeup();
}
dwt_readfromdevice(0,0,4,&dw_id_data[0]);
NRF_LOG_INFO("%d -> DW_ID =0x%.2X%.2X%.2X%.2X \r\n",i,dw_id_data[0],dw_id_data[1],dw_id_data[2],dw_id_data[3]);
#endif
deca_sleep(10);
//deca_init();
}
#else
if (loop_sync_flag==1)
{
NRF_LOG_INFO("Anchor deca_cnt=%d\r\n",deca_cnt++);
NRF_LOG_INFO("...... deca_sleep...........\r\n");
if (NRF_LOG_PROCESS() == false)
{
//power_manage();
}
/* Put DW1000 to sleep. */
//dwt_entersleep();
deca_sleep(100);
}
#endif
}
//NRF_LOG_INFO("Anchor deca_cnt=%d\r\n",deca_cnt++);
}[/code]
main_05b_no_sleep.log
:INFO:Poll len=12
:INFO:Poll ok.
:INFO:s2_reg=806FF2
:INFO:Final len=24
:INFO:Final ok.
:INFO:Send Final_Resp ok.
:INFO:distance_cm: 26 cm, hex=1A
:INFO:Anchor deca_cnt=19
:INFO:… deca_sleep…
:INFO:s1_reg=806F72—20
:INFO:Poll len=12
:INFO:Poll ok.
:INFO:s2_reg=806FF2
:INFO:Final len=24
:INFO:Final ok.
:INFO:Send Final_Resp ok.
:INFO:distance_cm: 25 cm, hex=19
:INFO:Anchor deca_cnt=20
:INFO:… deca_sleep…
:INFO:s1_reg=806F72—21
:INFO:Poll len=12[/code]
main_05b_enter_sleep.c
codeint deca_main_routine(void)
{
static int deca_cnt=0;
uint8 dw_id_data[4];
static int loop_cnt=0;
int i;
#if 0
/* Start with board specific hardware init. */
peripherals_init();
/* Display application name on LCD. */
lcd_display_str(APP_NAME);
/* 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. */
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
spi_set_rate_low();
if (dwt_initialise(DWT_LOADUCODE) == DWT_ERROR)
{
lcd_display_str("INIT FAILED");
while (1)
{ };
}
spi_set_rate_high();
#endif
#if 1
/* Configure DW1000. See NOTE 7 below. */
dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Configure and wake-up parameters. */
//dwt_configuresleep(DWT_PRESRV_SLEEP | DWT_CONFIG, DWT_WAKE_CS | DWT_SLP_EN);
dwt_configuresleep(DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV, DWT_XTAL_EN|DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN);
//dwt_configuresleep(DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV, DWT_WAKE_CS | DWT_SLP_EN);
#endif
/* Set preamble timeout for expected frames. See NOTE 6 below. */
//dwt_setpreambledetecttimeout(PRE_TIMEOUT);
// enable GPIO 5,6 to indicate Tx and RX function
//dwt_setlnapamode(1,1);
/* Loop forever responding to ranging requests. */
while (1)
{
//NRF_LOG_INFO("--loop_cnt=%d\r\n",loop_cnt++);
loop_sync_flag = 0;
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
#if 0
/* Configure DW1000. See NOTE 7 below. */
dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Configure and wake-up parameters. */
dwt_configuresleep(DWT_PRESRV_SLEEP | DWT_CONFIG, DWT_WAKE_CS | DWT_SLP_EN);
//dwt_configuresleep(DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV, DWT_WAKE_CS | DWT_SLP_EN);
#endif
if (NRF_LOG_PROCESS() == false)
{
//power_manage();
}
/* Clear reception timeout to start next ranging process. */
//dwt_setrxtimeout(0);
dwt_setrxtimeout(POLL_RX_TIMEOUT_UUS);
/* 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_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{ };
if (status_reg & SYS_STATUS_RXFCG)
{
uint32 frame_len;
NRF_LOG_INFO("s1_reg=%X---%d\r\n",status_reg,loop_cnt++);
/* 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_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
//test_p03_set();
#if 1 //DECA_DEBUG_FLAG
NRF_LOG_INFO("Poll len=%d\r\n",frame_len);
//NRF_LOG_HEXDUMP_INFO(rx_buffer, 10);
#endif
/* 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 resp_tx_time;
int ret;
#if 1 //DECA_DEBUG_FLAG
NRF_LOG_INFO("Poll ok.\r\n");
#endif
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();
//NRF_LOG_INFO("poll_rx_ts = 0x%X\r\n",(uint32)poll_rx_ts);
/* 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;
//NRF_LOG_INFO("resp_tx_ts = 0x%X,diff=0x%X\r\n",resp_tx_time,resp_tx_time-(uint32)poll_rx_ts);
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), 0, 1); /* Zero offset in TX buffer, ranging. */
//test_p03_clear();
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)
{
NRF_LOG_INFO("Err in line=%d\r\n",__LINE__);
/* Clear RX error/timeout events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
continue;
//goto deca_05b_end;
}
//test_p04_set();
/* Poll DW1000 until TX frame sent event set. See NOTE 9 below. */
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
//test_p04_clear();
//test_p03_set();
/* 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 | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{ };
//test_p03_clear();
NRF_LOG_INFO("s2_reg=%X\r\n",status_reg);
/* Increment frame sequence number after transmission of the response message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
NRF_LOG_INFO("Final len=%d\r\n",frame_len);
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 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;
uint32 final_tx_time;
int ret;
NRF_LOG_INFO("Final ok.\r\n");
/* 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;
u16_distance_cm = (uint16)(distance*100);
#if 1
// To do next, then send resp2 msg to tag----Start
/* Compute resp2 message transmission time. See NOTE 10 below. */
final_tx_time = (final_rx_ts + (FINAL_RX_TO_RESP2_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(final_tx_time);
/* Write the expected timestamp in the resp2 message. See NOTE 11 below. */
final_msg_set_ts(&tx_reap2_msg[RESP2_MSG_RESP_TX_TS_IDX], resp_tx_ts);
//final_msg_set_ts(&tx_reap2_msg[RESP2_MSG_FINAL_RX_TS_IDX], final_rx_ts);
memcpy(&tx_reap2_msg[RESP2_MSG_FINAL_RX_TS_IDX], (uint8 *)&u16_distance_cm,2);
/* Write and send final message. See NOTE 8 below. */
tx_reap2_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_reap2_msg), tx_reap2_msg, 0); /* Zero offset in TX buffer. */
dwt_writetxfctrl(sizeof(tx_reap2_msg), 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)
{
//test_p04_set();
/* Poll DW1000 until TX frame sent event set. See NOTE 9 below. */
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
//test_p04_clear();
NRF_LOG_INFO("Send Final_Resp ok.\r\n");
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
}
// To do next, then send resp2 msg to tag----End
#endif
/* Display computed distance on LCD. */
//sprintf(dist_str, "DIST: %3.2f m", distance);
//lcd_display_str(dist_str);
//NRF_LOG_INFO("DIST: %3.2f m\r\n", distance);
//NRF_LOG_INFO("deca measure: %d cm\r\n",distance*100);
NRF_LOG_INFO("distance_cm: %d cm, hex=%X \r\n",u16_distance_cm,u16_distance_cm);
loop_sync_flag = 1;
goto deca_05b_end;
}
}
else
{
/* Clear RX error/timeout events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
}
}
else
{
/* Clear RX error/timeout events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
deca_05b_end:
#if 0
if (loop_sync_flag==1)
{
NRF_LOG_INFO("Anchor deca_cnt=%d\r\n",deca_cnt++);
NRF_LOG_INFO("...... dwt_entersleep...........\r\n");
if (NRF_LOG_PROCESS() == false)
{
//power_manage();
}
/* Put DW1000 to sleep. */
dwt_entersleep();
deca_sleep(100);
#if 1
deca_spi_wakeup();
while(DWT_DEVICE_ID != dwt_readdevid())
{
dwt_readfromdevice(0,0,4,&dw_id_data[0]);
NRF_LOG_INFO("%d -> DW_ID =0x%.2X%.2X%.2X%.2X \r\n",i++,dw_id_data[0],dw_id_data[1],dw_id_data[2],dw_id_data[3]);
deca_spi_wakeup();
}
dwt_readfromdevice(0,0,4,&dw_id_data[0]);
NRF_LOG_INFO("%d -> DW_ID =0x%.2X%.2X%.2X%.2X \r\n",i,dw_id_data[0],dw_id_data[1],dw_id_data[2],dw_id_data[3]);
#endif
deca_sleep(10);
//deca_init();
}
#else
if (loop_sync_flag==1)
{
NRF_LOG_INFO("Anchor deca_cnt=%d\r\n",deca_cnt++);
NRF_LOG_INFO("...... deca_sleep...........\r\n");
if (NRF_LOG_PROCESS() == false)
{
//power_manage();
}
/* Put DW1000 to sleep. */
//dwt_entersleep();
deca_sleep(100);
}
#endif
}
//NRF_LOG_INFO("Anchor deca_cnt=%d\r\n",deca_cnt++);
}[/code]
main_05b_enter_sleep.log
:INFO:Poll len=12
:INFO:Poll ok.
:INFO:s2_reg=806FF2
:INFO:Final len=24
:INFO:Final ok.
:INFO:Send Final_Resp ok.
:INFO:distance_cm: 21 cm, hex=15
:INFO:Anchor deca_cnt=0
:INFO:… dwt_entersleep…
:INFO:0 -> DW_ID =0xFFFFFFFF
:INFO:1 -> DW_ID =0x3001CADE
:INFO:s1_reg=6F02—1
:INFO:Poll len=12
:INFO:Poll ok.
:INFO:Err in line=321
:INFO:s1_reg=6F02—2
:INFO:Poll len=12
:INFO:Poll ok.
:INFO:Err in line=321
:INFO:s1_reg=6F02—3
:INFO:Poll len=12
:INFO:Poll ok.
:INFO:Err in line=321
:INFO:s1_reg=6F02—4
:INFO:Poll len=12
:INFO:Poll ok.
:INFO:Err in line=321
:INFO:s1_reg=6F02—5
:INFO:Poll len=12
:INFO:Poll ok.[/code]