Hi all,
I’m trying to implement double side raning for my application.
I’m doing a custom protocol, that is similar to the one suggested by the qorvo application note:
→ POLL (conversation initiated by TAG)
→ POLL ACK (reply from anchor)
→ MEX1
→ MEX1 ACK
→ RANGE REPORT
→ RANGE REPORT ACK
I’m using extra messages because I know the tx timestamp after the actual transmission of the frame (I’m not estimating it by RX + delta but I’m reading the TX timestamp register)
These are the key points:
- Sending the poll in TX IMMEDIATE mode. (On reception of poll ack from anchor, I read the tx timestamp register which contains the tx timestam of POLL in order to get the T1 aka the real timestamp for transmission). In the meanwhile, we read the rx timestamp, that is the T4.
Basically, I apply the same logic for all the messages until I get T1 .. T6 and after that I calculate the distance applying the formula:
double numerator = (double)tround1_signed * (double)tround2_signed - (double)treply1_signed * (double)treply2_signed;
double denominator = (double)tround1_signed + (double)tround2_signed + (double)treply1_signed + (double)treply2_signed;
double tof_dtu = numerator / denominator;
double tof_seconds = tof_dtu * DWT_TIME_UNITS;
double raw_distance = tof_seconds * SPEED_OF_LIGHT;
where the Deltas are coming from this:
int64_t tround1_signed = calculateTimeDifference(ds_twr_timestamps.T4, ds_twr_timestamps.T1);
int64_t treply1_signed = calculateTimeDifference(ds_twr_timestamps.T3, ds_twr_timestamps.T2);
int64_t tround2_signed = calculateTimeDifference(ds_twr_timestamps.T6, ds_twr_timestamps.T3);
int64_t treply2_signed = calculateTimeDifference(ds_twr_timestamps.T5, ds_twr_timestamps.T4);
which implementation is:
int64_t DW1000RangingClass::calculateTimeDifference(uint64_t later_ts, uint64_t earlier_ts)
{
const uint64_t MASK_40BIT = 0xFFFFFFFFFFULL;
later_ts &= MASK_40BIT;
earlier_ts &= MASK_40BIT;
int64_t diff = (int64_t)later_ts - (int64_t)earlier_ts;
const int64_t HALF_RANGE = (1LL << 39);
if (diff < -HALF_RANGE)
{
diff += (1LL << 40);
}
else if (diff > HALF_RANGE)
{
diff -= (1LL << 40);
}
return diff;
}
Apart from POLL and the final messages, I send with the mode (DWT_START_TX_DLY_RS | DWT_RESPONSE_EXPECTED), this is assuring that I have a constant time between reception and consecutive transmission. This is to compensate errors. For my tests I’m applying an offset of 374400 count (which is 1.5ms).
But I have very unstable measures.
I also enabled CIA DONE interrupt (for the reception) and I’m basing on it. I know that there is an other reception interrupt but I saw that with this flag I’m stabler
I also enabled TX interrupt (to understand that we can correctly transmit, and to notify my MCU to proceed to read the TX timestamp).
This in an example of send:
void DW1000RangingClass::sendMiddleMessage()
{
memcpy(tx_msg, rx_msg, 9);
uint16_t temp = *(uint16_t *)&tx_msg[5];
*(uint16_t *)&tx_msg[5] = *(uint16_t *)&tx_msg[7];
*(uint16_t *)&tx_msg[7] = temp;
// Payload MIDDLE
ds_middle_msg_t *middle_payload = (ds_middle_msg_t *)&tx_msg[9];
middle_payload->msg_type = MSG_MIDDLE;
middle_payload->session_id = current_session_id;
middle_payload->t1_poll_tx_instant = ds_twr_timestamps.T1;
middle_payload->t4_poll_ack_rx_instant = ds_twr_timestamps.T4;
size_t total_size = 9 + sizeof(ds_middle_msg_t);
// Delayed TX per FINAL
volatile uint32_t deltaFromRx;
deltaFromRx = COMMON_DELAY;
deltaFromRx = deltaFromRx & 0xFFFFFFFE;
dwt_setdelayedtrxtime(deltaFromRx);
dwt_writetxdata(total_size, tx_msg, 0);
dwt_writetxfctrl(total_size + 2, 0, 1);
if (dwt_starttx(DWT_START_TX_DLY_RS | DWT_RESPONSE_EXPECTED) == DWT_ERROR)
{
DEBUG_SESS_PRINTF("ERROR: Delayed TX failed, using immediate\r\n");
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
}
}
But I cannot get “stable” measurements even after filtering (aka with median filtering).
For sure there’s something I’m missing but in theory should work.
Can you guys help me out or give me a hint?
Thanks,
Marco.