cph_deca_msg_header_t * cph_deca_read_frame(uint8_t * rx_buffer, uint32_t *frame_len) {
/* 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 <= CPH_MAX_MSG_SIZE) {
dwt_readrxdata(rx_buffer, *frame_len, 0);
#if defined(DEBUG_PRINT_RXFRAME)
printf("FRM: ");
for (int i=0;i<*frame_len;i++)
printf("%02X ", rx_buffer[i]);
printf("\r\n");
#endif
return (cph_deca_msg_header_t*)rx_buffer;
}
else {
TRACE("Bad length: %04X\r\n", *frame_len);
return 0;
}
}
static void rxcallback(const dwt_callback_data_t *rxd) {
if (rxd->event == DWT_SIG_RX_OKAY) {
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
if (rxd->datalength <= MAX_BUFF_SIZE) {
dwt_readrxdata(rx_buffer, rxd->datalength, 0);
irq_status = STATUS_RCV;
frame_len = rxd->datalength;}
else {
irq_status = STATUS_ERR;
frame_len = rxd->datalength;
}}
else {
dwt_readrxdata(rx_buffer, 20, 0);
frame_len = 20;
irq_status = STATUS_ERR;
}
}
cph_deca_msg_header_t * cph_deca_read_frame(uint8_t * rx_buffer, uint32_t *frame_len) {
/* 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 <= CPH_MAX_MSG_SIZE) {
dwt_readrxdata(rx_buffer, *frame_len, 0);
return (cph_deca_msg_header_t*)rx_buffer;
}
else {
return 0;
}
}
static void rxcallback(const dwt_callback_data_t *rxd) {
if (rxd->event == DWT_SIG_RX_OKAY) {
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
if (rxd->datalength <= MAXRXSIXZE) {
dwt_readrxdata(rx_buffer, rxd->datalength, 0);
trx_signal = SIGNAL_RCV;
frame_len = rxd->datalength;
} else {
trx_signal = SIGNAL_ERR_LEN;
error_status_reg = rxd->status;
}
}
else {
trx_signal = SIGNAL_ERR;
error_status_reg = rxd->status;
}
}
void cph_deca_rxcallback(const dwt_callback_data_t *rxd) {
cph_deca_event_t ev;
memcpy(&ev.info, rxd, sizeof(dwt_callback_data_t));
if (rxd->event == DWT_SIG_RX_OKAY) {
if (rxd->datalength <= CPH_MAX_MSG_SIZE) {
ev.status = CPH_EVENT_RX;
dwt_readrxdata(ev.data, rxd->datalength, 0);
ev.timestamp = get_rx_timestamp_u64();
cph_queue_push(&event_queue, &ev);
}
}
else {
ev.status = CPH_EVENT_ERR;
cph_queue_push(&event_queue, &ev);
// Auto enable should be on, so don't bother dwt_rxenable
}
TRACE("%02X\r\n", ev.status);
}
bool DWM1000_Anchor::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
while (true) {
WAIT_POLL: {
dwt_setrxtimeout(0); /* Clear reception timeout to start next ranging process. */
dwt_rxenable(0); /* Activate reception immediately. */
// dwt_setinterrupt(DWT_INT_RFCG, 1); // enable RXD interrupt
while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(1000);/* 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. */
clearInterrupt();
PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = _status_reg;
LOG<< HEX << " status reg.:" << status_reg << " ,interrupts : " << interruptCount << FLUSH;
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
LOG<< HEX << " IRQ pin : " << digitalRead(D2) << " status_reg DWM1000 " << status_reg << FLUSH;// PULL LOW
if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
break;
}
}
///____________________________________________________________________________
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $ "<<FLUSH;
uint32 frame_len;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); /* Clear good RX frame event in the DW1000 status register. */
/* 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);
}
/* 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) {
LOG<< " $$ "<<FLUSH;
uint32 resp_tx_time;
poll_rx_ts = get_rx_timestamp_u64(); /* Retrieve poll reception timestamp. */
/* Set send time for response. See NOTE 8 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. */
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
/* 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);
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
* See NOTE 7 below. */
// while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(10);
dwt_setinterrupt(DWT_INT_RFCG, 1);// enable
clearInterrupt();
// PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
// status_reg = _status_reg;
LOG<< HEX << " status reg2:" << status_reg << FLUSH;
// if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
// break;
// }
// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
// { };
/* Increment frame sequence number after transmission of the response message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $$$ "<<FLUSH;
/* 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;
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;
/* 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 10 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;
/* Display computed distance on LCD. */
// char dist_str[20];
// sprintf(dist_str,"%3.2f", distance);
// lcd_display_str(dist_str);
LOG<< " distance : " << (float)distance << "m. " << FLUSH;
}
} else {
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
} else {
/**
* Application entry point.
*/
int rxWait(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 i;
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
//spi_set_rate_low();
dwt_initialise(DWT_LOADNONE);
//spi_set_rate_high();
/* Configure DW1000. See NOTE 2 below. */
dwt_configure(&config);
/* Loop forever sending and receiving frames periodically. */
while (1)
{
/* Activate reception immediately. See NOTE 3 below. */
dwt_rxenable(0);
/* Poll until a frame is properly received or an error occurs. See NOTE 4 below.
* STATUS register is 5 bytes long but, as the events we are looking at are in the lower bytes of the register, we can use this simplest API
* function to access it. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
printf("Status reg now 0x%x\r\n",status_reg);
if (status_reg & 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 <= FRAME_LEN_MAX)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Clear good RX frame event in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
for (i=0;i<frame_len;i++) {
printf("%x ",rx_buffer[i]);
}
printf("\r\n");
/* Validate the frame is the one expected as sent by "TX then wait for a response" example. */
if ((frame_len == 14) && (rx_buffer[0] == 0xC5) && (rx_buffer[10] == 0x43) && (rx_buffer[11] == 0x2))
{
int i;
/* Copy source address of blink in response destination address. */
for (i = 0; i < 8; i++)
{
tx_msg[DATA_FRAME_DEST_IDX + i] = rx_buffer[BLINK_FRAME_SRC_IDX + i];
}
/* Write response frame data to DW1000 and prepare transmission. See NOTE 5 below.*/
dwt_writetxdata(sizeof(tx_msg), tx_msg, 0);
dwt_writetxfctrl(sizeof(tx_msg), 0);
/* Send the response. */
dwt_starttx(DWT_START_TX_IMMEDIATE);
/* Poll DW1000 until TX frame sent event set. */
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
/* Clear TX frame sent event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment the data frame sequence number (modulo 256). */
tx_msg[DATA_FRAME_SN_IDX]++;
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
printf("Some RX errors ...\r\n");
}
}
}
void listener_run(void) {
uint32_t announce_coord_ts = 0;
uint32_t elapsed = 0;
uint32_t last_ts = 0;
uint32_t count = 0;
irq_init();
pio_disable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
// Setup DW1000
dwt_txconfig_t txconfig;
// Setup DECAWAVE
reset_DW1000();
spi_set_rate_low();
dwt_initialise(DWT_LOADUCODE);
spi_set_rate_high();
dwt_configure(&cph_config->dwt_config);
dwt_setpanid(0x4350);
dwt_setaddress16(0x1234);
// Clear CLKPLL_LL
dwt_write32bitreg(SYS_STATUS_ID, 0x02000000);
uint32_t id = dwt_readdevid();
printf("Device ID: %08X\r\n", id);
#if 1
dwt_setcallbacks(0, rxcallback);
dwt_setinterrupt(
DWT_INT_TFRS | DWT_INT_RFCG
| (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/),
1);
pio_enable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
dwt_rxenable(0);
while (1) {
elapsed = cph_get_millis() - last_ts;
if (elapsed > 5000) {
printf("alive %d\r\n", count++);
last_ts = cph_get_millis();
}
if (trx_signal == SIGNAL_RCV) {
printf("[RCV] %d - ", frame_len);
for (int i = 0; i < frame_len; i++) {
printf("%02X ", rx_buffer[i]);
}
printf("\r\n");
trx_signal = SIGNAL_EMPTY;
dwt_rxenable(0);
}
else if(trx_signal == SIGNAL_ERR) {
printf("ERROR: %08X\r\n", error_status_reg);
trx_signal = SIGNAL_EMPTY;
dwt_rxenable(0);
}
else if(trx_signal == SIGNAL_ERR_LEN) {
printf("ERROR LENGTH: %08X\r\n", error_status_reg);
trx_signal = SIGNAL_EMPTY;
dwt_rxenable(0);
}
}
#else
while (1) {
/* Activate reception immediately. */
dwt_rxenable(0);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) {
};
if (status_reg & SYS_STATUS_RXFCG) {
uint32 frame_len;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= MAXRXSIXZE) {
dwt_readrxdata(rx_buffer, frame_len, 0);
} else {
frame_len = 0;
}
if (frame_len > 0) {
printf("[RCV] ");
for (int i = 0; i < frame_len; i++) {
printf("%02X ", rx_buffer[i]);
}
printf("\r\n");
} else {
printf("ERROR: frame_len == %d\r\n", frame_len);
}
} else {
printf("ERROR: dwt_rxenable has status of %08X\r\n", status_reg);
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR | SYS_STATUS_CLKPLL_LL);
}
}
#endif
}
/*! ------------------------------------------------------------------------------------------------------------------
* @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);
}
}
}
// Triggered when we receive a packet
void app_dw1000_rxcallback (const dwt_callback_data_t *rxd) {
// First grab a copy of local time when this arrived
rtimer_clock_t rt_timestamp = RTIMER_NOW();
DEBUG_B6_HIGH;
if (rxd->event == DWT_SIG_RX_OKAY) {
leds_toggle(LEDS_BLUE);
uint8_t packet_type;
uint64_t dw_timestamp;
uint8_t recv_pkt_buf[RX_PKT_BUF_LEN];
// Get the dw_timestamp first
uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0};
dwt_readrxtimestamp(txTimeStamp);
dw_timestamp = ((uint64_t) (*((uint32_t*) txTimeStamp))) | (((uint64_t) txTimeStamp[4]) << 32);
// Get the packet
dwt_readrxdata(recv_pkt_buf, MIN(RX_PKT_BUF_LEN, rxd->datalength), 0);
packet_type = recv_pkt_buf[offsetof(struct pp_tag_poll, message_type)];
global_round_num = recv_pkt_buf[offsetof(struct pp_tag_poll, roundNum)];
if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_POLL) {
struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf;
DEBUG_P("TAG_POLL rx: %u\r\n", pkt->subsequence);
if (global_subseq_num == pkt->subsequence) {
// Configurations matched, record arrival time
pp_anc_final_pkt.TOAs[global_subseq_num] = dw_timestamp;
} else {
// Tag/anchor weren't on same settings, so we
// drop this sample and catch the anchor up
global_subseq_num = pkt->subsequence;
}
if (!global_round_active) {
DEBUG_B4_LOW;
DEBUG_B5_LOW;
memset(antenna_statistics, 0, sizeof(antenna_statistics));
global_round_active = true;
start_of_new_subseq = true;
substate_timer_fired = true;
/*
subseq_start_time = rt_timestamp - US_TO_RT(TAG_SQ_START_TO_POLL_SFD_HIGH_US);
rtimer_clock_t set_to = subseq_start_time + US_TO_RT(POLL_TO_SS_US+SS_TO_SQ_US);
*/
rtimer_clock_t set_to = rt_timestamp + US_TO_RT(
POLL_TO_SS_US + SS_TO_SQ_US
- TAG_SQ_START_TO_POLL_SFD_HIGH_US
- ANC_MYSTERY_STARTUP_DELAY_US);
rtimer_set(&subsequence_timer,
set_to,
1,
(rtimer_callback_t)subsequence_task,
NULL);
}
//Keep a running total of the number of packets seen from each antenna
antenna_statistics[subseq_num_to_anchor_sel(global_subseq_num)]++;
} else if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_FINAL) {
if (!global_round_active) {
// The first packet we happened to receive was
// an ANC_FINAL. We have nothing interesting to
// reply with, so don't. But we *do* need to set
// receive mode again so that a new poll will be
// caught
dwt_rxenable(0);
return;
}
//We're likely in RX mode, so we need to exit before transmission
dwt_forcetrxoff();
pp_anc_final_pkt.TOAs[NUM_MEASUREMENTS] = dw_timestamp;
pp_anc_final_pkt.header.seqNum++;
const uint16 frame_len = sizeof(struct pp_anc_final);
dwt_writetxfctrl(frame_len, 0);
//Schedule this transmission for our scheduled time slot
DEBUG_B6_LOW;
uint32_t temp = dwt_readsystimestamphi32();
uint32_t delay_time = temp +
DW_DELAY_FROM_US(
ANC_FINAL_INITIAL_DELAY_HACK_VALUE +
(ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1))
);
/* I don't understand what exactly is going on
* here. The chip seems to want way longer for
* this preamble than others -- maybe something
* to do with the large payload? From empirical
* measurements, the 300 base delay is about the
* minimum (250 next tested as not working)
DW_DELAY_FROM_US(
REVISED_DELAY_FROM_PKT_LEN_US(frame_len) +
(2*ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1))
);
*/
delay_time &= 0xFFFFFFFE;
pp_anc_final_pkt.dw_time_sent = delay_time;
dwt_setdelayedtrxtime(delay_time);
int err = dwt_starttx(DWT_START_TX_DELAYED);
dwt_settxantennadelay(TX_ANTENNA_DELAY);
dwt_writetxdata(frame_len, (uint8_t*) &pp_anc_final_pkt, 0);
DEBUG_B6_HIGH;
#ifdef DW_DEBUG
// No printing until after all dwt timing op's
struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf;
DEBUG_P("TAG_FINAL rx: %u\r\n", pkt->subsequence);
#endif
if (err) {
#ifdef DW_DEBUG
uint32_t now = dwt_readsystimestamphi32();
DEBUG_P("Could not send anchor response\r\n");
DEBUG_P(" -- sched time %lu, now %lu (diff %lu)\r\n", delay_time, now, now-delay_time);
leds_on(LEDS_RED);
#endif
} else {
DEBUG_P("Send ANC_FINAL\r\n");
leds_off(LEDS_RED);
}
} else {
DEBUG_P("*** ERR: RX Unknown packet type: 0x%X\r\n", packet_type);
}
} else {
bool DWM1000_Tag::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
POLL_SEND: {
while (true) {
timeout(1000); // delay between POLL
PT_YIELD_UNTIL(timeout());
/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
dwt_writetxfctrl(sizeof(tx_poll_msg), 0);
/* 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. */
LOG<< " Start TXF " << FLUSH;
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);// SEND POLL MSG
dwt_setinterrupt(DWT_INT_TFRS, 0);
dwt_setinterrupt(DWT_INT_RFCG, 1); // enable
clearInterrupt();
_timeoutCounter = 0;
/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
timeout(10);
PT_YIELD_UNTIL(timeout() || isInterruptDetected()); // WAIT RESP MSG
if (isInterruptDetected())
LOG<< " INTERRUPT DETECTED " << FLUSH;
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
LOG<< HEX <<" SYS_STATUS " << status_reg << FLUSH;
if (status_reg == 0xDEADDEAD) {
init();
} else if (status_reg & SYS_STATUS_RXFCG)
goto RESP_RECEIVED;
else if (status_reg & SYS_STATUS_ALL_RX_ERR) {
if (status_reg & SYS_STATUS_RXRFTO)
INFO(" RX Timeout");
else
INFO(" RX error ");
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); /* Clear RX error events in the DW1000 status register. */
}
}
}
RESP_RECEIVED: {
LOG<< " Received " <<FLUSH;
frame_seq_nb++; /* Increment frame sequence number after transmission of the poll message (modulo 256). */
uint32 frame_len;
/* 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 iCHANGEt 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 "DS 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) { // CHECK RESP MSG
uint32 final_tx_time;
/* 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 9 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) (final_tx_time & 0xFFFFFFFE)) << 8)
+ TX_ANT_DLY;
/* Write all timestamps in the final message. See NOTE 10 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 7 below. */
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
dwt_writetxfctrl(sizeof(tx_final_msg), 0);
dwt_starttx(DWT_START_TX_DELAYED); // SEND FINAL MSG
/* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
timeout(10);
PT_YIELD_UNTIL((dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS) || timeout());;
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the final message (modulo 256). */
frame_seq_nb++;
} else {
// Called when the radio has received a packet.
static void anchor_rxcallback (const dwt_callback_data_t *rxd) {
if (rxd->event == DWT_SIG_RX_OKAY) {
// Read in parameters of this packet reception
uint64_t dw_rx_timestamp;
uint8_t buf[ONEWAY_ANCHOR_MAX_RX_PKT_LEN];
uint8_t message_type;
// Get the received time of this packet first
dwt_readrxtimestamp(buf);
dw_rx_timestamp = DW_TIMESTAMP_TO_UINT64(buf);
// Get the actual packet bytes
dwt_readrxdata(buf, MIN(ONEWAY_ANCHOR_MAX_RX_PKT_LEN, rxd->datalength), 0);
// We process based on the first byte in the packet. How very active
// message like...
message_type = buf[offsetof(struct pp_tag_poll, message_type)];
if (message_type == MSG_TYPE_PP_NOSLOTS_TAG_POLL) {
// This is one of the broadcast ranging packets from the tag
struct pp_tag_poll* rx_poll_pkt = (struct pp_tag_poll*) buf;
// Decide what to do with this packet
if (_state == ASTATE_IDLE) {
// We are currently not ranging with any tags.
if (rx_poll_pkt->subsequence < NUM_RANGING_CHANNELS) {
// We are idle and this is one of the first packets
// that the tag sent. Start listening for this tag's
// ranging broadcast packets.
_state = ASTATE_RANGING;
// Clear memory for this new tag ranging event
memset(pp_anc_final_pkt.TOAs, 0, sizeof(pp_anc_final_pkt.TOAs));
memset(_anchor_antenna_recv_num, 0, sizeof(_anchor_antenna_recv_num));
// Record the EUI of the tag so that we don't get mixed up
memcpy(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8);
// Record which ranging subsequence the tag is on
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
// Record the timestamp. Need to subtract off the TX+RX delay from each recorded
// timestamp.
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] =
dw_rx_timestamp - oneway_get_txrxdelay_from_subsequence(ANCHOR, _ranging_broadcast_ss_num);
// Also record parameters the tag has sent us about how to respond
// (or other operational parameters).
_ranging_operation_config.reply_after_subsequence = rx_poll_pkt->reply_after_subsequence;
_ranging_operation_config.anchor_reply_window_in_us = rx_poll_pkt->anchor_reply_window_in_us;
_ranging_operation_config.anchor_reply_slot_time_in_us = rx_poll_pkt->anchor_reply_slot_time_in_us;
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = oneway_subsequence_number_to_antenna(ANCHOR, rx_poll_pkt->subsequence);
_anchor_antenna_recv_num[recv_antenna_index]++;
// Now we need to start our own state machine to iterate
// through the antenna / channel combinations while listening
// for packets from the same tag.
timer_start(_anchor_timer, RANGING_BROADCASTS_PERIOD_US, ranging_broadcast_subsequence_task);
} else {
// We found this tag ranging sequence late. We don't want
// to use this because we won't get enough range estimates.
// Just stay idle, but we do need to re-enable RX to
// keep receiving packets.
dwt_rxenable(0);
}
} else if (_state == ASTATE_RANGING) {
// We are currently ranging with a tag, waiting for the various
// ranging broadcast packets.
// First check if this is from the same tag
if (memcmp(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8) == 0) {
// Same tag
if (rx_poll_pkt->subsequence == _ranging_broadcast_ss_num) {
// This is the packet we were expecting from the tag.
// Record the TOA, and adjust it with the calibration value.
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] =
dw_rx_timestamp - oneway_get_txrxdelay_from_subsequence(ANCHOR, _ranging_broadcast_ss_num);
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = oneway_subsequence_number_to_antenna(ANCHOR, _ranging_broadcast_ss_num);
_anchor_antenna_recv_num[recv_antenna_index]++;
} else {
// Some how we got out of sync with the tag. Ignore the
// range and catch up.
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
}
// Check to see if we got the last of the ranging broadcasts
if (_ranging_broadcast_ss_num == _ranging_operation_config.reply_after_subsequence) {
// We did!
ranging_listening_window_setup();
}
} else {
// Not the same tag, ignore
}
} else {
// We are in some other state, not sure what that means
}
} else {
// Other message types go here, if they get added
// We do want to enter RX mode again, however
dwt_rxenable(0);
}
} else {
// Called when the radio has received a packet.
void dw1000_anchor_rxcallback (const dwt_callback_data_t *rxd) {
if (rxd->event == DWT_SIG_RX_OKAY) {
// Read in parameters of this packet reception
uint64_t dw_rx_timestamp;
uint8_t buf[DW1000_ANCHOR_MAX_RX_PKT_LEN];
uint8_t message_type;
// Get the received time of this packet first
dwt_readrxtimestamp(buf);
dw_rx_timestamp = DW_TIMESTAMP_TO_UINT64(buf);
// Get the actual packet bytes
dwt_readrxdata(buf, MIN(DW1000_ANCHOR_MAX_RX_PKT_LEN, rxd->datalength), 0);
// We process based on the first byte in the packet. How very active
// message like...
message_type = buf[offsetof(struct pp_tag_poll, message_type)];
if (message_type == MSG_TYPE_PP_NOSLOTS_TAG_POLL) {
// This is one of the broadcast ranging packets from the tag
struct pp_tag_poll* rx_poll_pkt = (struct pp_tag_poll*) buf;
// Decide what to do with this packet
if (_state == ASTATE_IDLE) {
// We are currently not ranging with any tags.
if (rx_poll_pkt->subsequence < NUM_RANGING_CHANNELS) {
// We are idle and this is one of the first packets
// that the tag sent. Start listening for this tag's
// ranging broadcast packets.
_state = ASTATE_RANGING;
// Clear memory for this new tag ranging event
memset(pp_anc_final_pkt.TOAs, 0, sizeof(pp_anc_final_pkt.TOAs));
memset(_anchor_antenna_recv_num, 0, sizeof(_anchor_antenna_recv_num));
// Record the EUI of the tag so that we don't get mixed up
memcpy(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8);
// Record which ranging subsequence the tag is on
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
// Record the timestamp
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] = dw_rx_timestamp;
// Also record parameters the tag has sent us about how to respond
// (or other operational parameters).
_ranging_operation_config.reply_after_subsequence = rx_poll_pkt->reply_after_subsequence;
_ranging_operation_config.anchor_reply_window_in_us = rx_poll_pkt->anchor_reply_window_in_us;
_ranging_operation_config.anchor_reply_slot_time_in_us = rx_poll_pkt->anchor_reply_slot_time_in_us;
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = subsequence_number_to_antenna(ANCHOR, rx_poll_pkt->subsequence);
_anchor_antenna_recv_num[recv_antenna_index]++;
// Now we need to start our own state machine to iterate
// through the antenna / channel combinations while listening
// for packets from the same tag.
_ranging_broadcast_ss_num++;
dw1000_set_ranging_broadcast_subsequence_settings(ANCHOR, _ranging_broadcast_ss_num, FALSE);
timer_start(_ranging_broadcast_timer, RANGING_BROADCASTS_PERIOD_US, ranging_broadcast_subsequence_task);
} else {
// We found this tag ranging sequence late. We don't want
// to use this because we won't get enough range estimates.
// Just stay idle.
}
} else if (_state == ASTATE_RANGING) {
// We are currently ranging with a tag, waiting for the various
// ranging broadcast packets.
// First check if this is from the same tag
if (memcmp(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8) == 0) {
// Same tag
if (rx_poll_pkt->subsequence == _ranging_broadcast_ss_num) {
// This is the packet we were expecting from the tag.
// Record the TOA.
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] = dw_rx_timestamp;
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = subsequence_number_to_antenna(ANCHOR, _ranging_broadcast_ss_num);
_anchor_antenna_recv_num[recv_antenna_index]++;
} else {
// Some how we got out of sync with the tag. Ignore the
// range and catch up.
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
}
// Check to see if we got the last of the ranging broadcasts
if (_ranging_broadcast_ss_num == _ranging_operation_config.reply_after_subsequence) {
// We did!
// Stop iterating through timing channels
timer_stop(_ranging_broadcast_timer);
// We no longer need to receive and need to instead
// start transmitting.
dwt_forcetrxoff();
// Update our state to the TX response state
_state = ASTATE_RESPONDING;
// Set the listening window index
_ranging_listening_window_num = 0;
// Determine which antenna we are going to use for
// the response.
uint8_t max_packets = 0;
uint8_t max_index = 0;
for (uint8_t i=0; i<NUM_ANTENNAS; i++) {
if (_anchor_antenna_recv_num[i] > max_packets) {
max_packets = _anchor_antenna_recv_num[i];
max_index = i;
}
}
pp_anc_final_pkt.final_antenna = max_index;
// Now we need to setup a timer to iterate through
// the response windows so we can send a packet
// back to the tag
timer_start(_ranging_broadcast_timer,
_ranging_operation_config.anchor_reply_window_in_us,
ranging_listening_window_task);
}
} else {
// Not the same tag, ignore
}
} else {
// We are in some other state, not sure what that means
}
} else {
// Other message types go here, if they get added
}
} else {
/*! ------------------------------------------------------------------------------------------------------------------
* @fn main()
*
* @brief Application entry point.
*
* @param none
*
* @return none
*/
int ssTwrResp(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 i;
int status;
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */
//spi_set_rate_low();
dwt_initialise(DWT_LOADUCODE);
//spi_set_rate_high();
/* Configure DW1000. See NOTE 5 below. */
dwt_configure(&config);
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);
btn = buttons();
printf("%s entering main loop\r\n",__FUNCTION__);
/* Loop forever responding to ranging requests. */
while (1)
{
/* Activate reception immediately. */
dwt_rxenable(0);
/* Poll for reception of a frame or error/timeout. See NOTE 6 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 (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_RXFL_MASK_1023;
//printf("Frame length %d\r\n",frame_len);
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/*
for (i=0;i<frame_len;i++) {
printf("%x ",rx_buffer[i]);
}
printf("\r\n");
*/
/* 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 resp_tx_time;
//printf("Poll MSG\r\n");
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();
//printf("RX timestamp: %lld\r\n",poll_rx_ts);
/* Compute final 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);
//printf("TX time: %d\r\n",resp_tx_time);
/* Response TX timestamp is the transmission time we programmed plus the antenna delay. */
resp_tx_ts = (((uint64)(resp_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;
//printf("TX timestamp: %lld\r\n",resp_tx_ts);
/* Write all timestamps in the final message. See NOTE 8 below. */
resp_msg_set_ts(&tx_resp_msg[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
resp_msg_set_ts(&tx_resp_msg[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
/* Write and send the response message. See NOTE 9 below. */
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
status = dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);
status = dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
if (DWT_SUCCESS != status) printf("API error line %d\r\n",__LINE__);
status = dwt_starttx(DWT_START_TX_DELAYED);
if (DWT_SUCCESS != status) {
printf("API error line %d\r\n",__LINE__);
printf("RX timestamp: %llu\r\n",poll_rx_ts);
printf("TX time: %llu\r\n",((uint64)resp_tx_time) << 8);
printf("TX timestamp: %llu\r\n",resp_tx_ts);
}
// poll only if starttx was OK
if (DWT_SUCCESS == status) {
/* Poll DW1000 until TX frame sent event set. See NOTE 6 below. */
u32 tx_stat;
while (!(tx_stat = dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ }; //printf("Waiting. status reg 0x%x\r\n",tx_stat); }
//printf("After Poll: status reg now 0x%x\r\n",tx_stat);
}
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the poll message (modulo 256). */
frame_seq_nb++;
}
}
