Hi all,
I am testing Double Sided Two Way Ranging Localization method. I have one initiator and four responder (anchor). I want communication like the picture below.
First, Initiator sends POLL msg and all the anchors receive it and its own RESP msg. After receiving all the RESP msg initiator will send the FINAL msg.
Is it possible to make this communication?
I have tried many times but when anchors send RESP msg initiator only reads one RESP msg. I want to read all of them.
uint32_t Initiator( const uint8_t destAddress, uint8_t *srcAddress )
{
uint32_t status;
// Buffer to save packets
uint8_t poll_msg_buffer[UWB_POLL_MSG_LEN] = {0};
uint8_t resp_msg_buffer[UWB_RESP_MSG_LEN] = {0};
uint8_t final_msg_buffer[UWB_FINAL_MSG_LEN] = {0};
uint8_t address[2] = {destAddress, *srcAddress};
// Timestamps
uint64_t poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32_t final_tx_time;
// Transmit POLL msg
kSerial_Pack(poll_msg_buffer, address, NULL, 0, KS_NTYPE);
UWB_SendPacket(poll_msg_buffer, UWB_POLL_MSG_LEN, UWB_RANGING | UWB_IMMEDIATE | UWB_TX_RESPONSE);
// Receive RESP msg
for (int i=0; i<4; i++) {
status = UWB_RecvPacket(resp_msg_buffer, UWB_RESP_MSG_LEN, UWB_RX_CONTINUE);
if (status == KS_OK) {
if(resp_msg_buffer[3] == KS_I8) { // If received RESP msg
// Transmit FINAL msg
kSerial_Pack(final_msg_buffer, address, time_bytes, 12, KS_U8);
UWB_SendPacket(final_msg_buffer, UWB_FINAL_MSG_LEN, UWB_RANGING | UWB_DELAYED);
}
}
}
return KS_OK;
}
uint32_t Anchor( uint8_t *destAddress, uint8_t *srcAddress, float32_t *distance )
{
uint32_t status;
// Buffer for packets
uint8_t tx_buffer[UWB_FINAL_MSG_LEN] = {0};
uint8_t resp_msg_buffer[UWB_RESP_MSG_LEN] = {0};
uint8_t address[2] = {*destAddress, *srcAddress};
uint64_t poll_rx_ts, resp_tx_ts, final_rx_ts;
// Receive POLL msg
status = UWB_RecvPacket(tx_buffer, 1024, DWT_RX_IMMEDIATE);
if (status == KS_OK) {
if (tx_buffer[3] == KS_NTYPE){ // If received POLL msg
poll_rx_ts = DWT_ReadRxTimestamp64();
t_resp = UWB_GetResponderTime(&tx_time);
UWB_SetDelayedTxRxTime(tx_time);
DWT_SetRxAfterTxDelay(UWB_RESP_TX_TO_FINAL_RX_DLY_UUS);
DWT_SetRxTimeout(UWB_FINAL_RX_TIMEOUT_UUS);
// Transmit RESP msg
kSerial_Pack(resp_msg_buffer, address, time_bytes, 4, KS_I8);
UWB_SendPacket(resp_msg_buffer, UWB_RESP_MSG_LEN, UWB_RANGING | UWB_DELAYED);
// Receive FINAL msg
status = UWB_RecvPacket(tx_buffer, 1024, DWT_RX_IMMEDIATE);
if (status == KS_OK) {
if(tx_buffer[3] == KS_U8) { // If received FINAL msg
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;
float64_t Ra, Rb, Da, Db;
int64_t tof_dtu;
resp_tx_ts = DWT_ReadTxTimestamp64();
final_rx_ts = DWT_ReadRxTimestamp64();
poll_tx_ts = UWB_GetReplyTime(&tx_buffer[6]);
resp_rx_ts = UWB_GetReplyTime(&tx_buffer[10]);
final_tx_ts = UWB_GetReplyTime(&tx_buffer[14]);
// 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;
Ra = (float64_t)(resp_rx_ts - poll_tx_ts);
Rb = (float64_t)(final_rx_ts_32 - resp_tx_ts_32);
Da = (float64_t)(final_tx_ts - resp_rx_ts);
Db = (float64_t)(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;
dist = tof * UWB_SPEED_OF_LIGHT;
*distance = dist;
// Display computed distance on LCD.
printf("DIST: %3.7f m\r\n", dist);
return KS_OK;
}
}
}
}
}
