UWB测距过程很简单，两个设备（A和B）。设备A先发送，设备B等待接收文章来源：https://www.toymoban.com/news/detail-618526.html
- 设备A 发送 P 给 设备B，此时读取时间戳，也可以等收到应答再进行读取，发送时间戳和接收时间戳都会保留最新一次，只要没有被覆盖（发送两次，或者接收两次），都是可以随时读取的。
- 设备B 收到P 后，发送R 给设备A，此时读取接收时间戳，和发送 时间戳。
- 设备A收到 R 后，发送F给设备B，读取发送时间戳。
- 设备B收到 F后，读取接收时间戳。
- 设备A 把所有时间戳发给设备B。
- 设备B收到 设备A的时间戳，再结合自己的时间戳根据DS——TWR算出两者之间的距离。
- 设备B算完距离后再打包发给设备A。
注意，因为时间戳是五个字节的，而我们为了方便发送，是只用了四个字节，舍掉高字节。如果前面的时间戳比后面的时间戳大，说明已经过了一个周期，后者需加上一个周期后再进行计算。因为跑完32位计数都需要 15.65 ps x 2^32 = 67.216ms ,所以不用担心会溢出两次的问题，因为一个测距过程也就是几个毫秒内完成，光速传播。
文章来源地址https://www.toymoban.com/news/detail-618526.html
//首先发送端
static volatile uint8_t stage;
static uint32_t TX_set_delay_times;
static volatile uint64_t TX_poll_times,TX_resp_times,TX_final_times;
static uint8_t Poll_Buff[]={0x15,0x16,0xab,0,0}; //第一次发送无关数据
static uint8_t Resp_Buff[5]={0}; //接收回应的无关数据的buff
static uint8_t First_Buff[]={0x15,0x16,0xae,0,0,0,0,0,0,0,0,0,0}; //发送第一次发送和接收的时间戳
static uint8_t NO_Buff[5]={0}; //接收无关信息
static uint8_t Final_Buff[9]={0x15,0x16,0xbc,0,0,0,0,0,0}; //发送最后一次发送数据的时间戳
static uint8_t Distance_Buff[13]={0}; //接收距离
//static uint8_t Tx_Final_Buff[]={0x15,0x16,0xae,0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0x1a,0x1b,0,0}; //最后一次发送时间戳信息过去
unsigned char LCD_Buff[10];
//为了减少发送次数，把时间戳放在握手协议里面发了
int main(void)
{
uint8 len = 0,i;
/* Start with board specific hardware init. */
peripherals_init();
printf("hello dwm1000!\r\n");
// dwt_dumpregisters();
/* 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) == -1)
{
printf("dwm1000 init fail!\r\n");
OLED_ShowString(0,0,"INIT FAIL");
while (1)
{
STM_EVAL_LEDOn(LED1);
deca_sleep(100);
STM_EVAL_LEDOff(LED1);
deca_sleep(100);
}
}
spi_set_rate_high();
/* Configure DW1000. See NOTE 6 below. */
dwt_configure(&config);
dwt_setleds(2);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
OLED_ShowString(0,0,"INIT PASS");
printf("init pass!\r\n");
deca_sleep(1000);
while(1)
{
dwt_setrxtimeout(8300); //设置接收超时时间
dwt_setrxmode(0,0,0); //设置接收为正常模式
dwt_writetxdata(sizeof(Poll_Buff), Poll_Buff, 0);
dwt_writetxfctrl(sizeof(Poll_Buff), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE|DWT_RESPONSE_EXPECTED);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ }; //等待接收完成
if (status_reg & SYS_STATUS_RXFCG)
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获取接收长度
if (len <= sizeof(Resp_Buff))
{
dwt_readrxdata(Resp_Buff, len, 0);
}
if(Resp_Buff[0]==0x15&&Resp_Buff[1]==0x16) //判断头
{
if(Resp_Buff[2] ==0xac) //判断特有标置位。每次发都不一样，可理解为每个设备特定标签号或基站编号
{
TX_poll_times = get_tx_timestamp_u64(); //获取发送端第一次发送的时间戳
TX_resp_times = get_rx_timestamp_u64(); //获取发送端接收应答数据的的时间戳
// TX_set_delay_times = (TX_resp_times + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8; //获取延时发送时间点
// dwt_setdelayedtrxtime(TX_set_delay_times); //设置延时发送时间点
// TX_final_times = (((uint64_t)(TX_set_delay_times&0xfffffffe))<<8) + TX_ANT_DLY; //获取发送端最后一次发送的时间戳
final_msg_set_ts(&First_Buff[3], TX_poll_times);
final_msg_set_ts(&First_Buff[7], TX_resp_times);
dwt_writetxdata(sizeof(First_Buff), First_Buff, 0);
dwt_writetxfctrl(sizeof(First_Buff), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE|DWT_RESPONSE_EXPECTED);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ }; //等待接收距离完成
if (status_reg & SYS_STATUS_RXFCG)
{
//
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获取接收长度
if (len <= sizeof(NO_Buff))
{
dwt_readrxdata(NO_Buff, len, 0);
}
if((NO_Buff[0]==0x15)&&(NO_Buff[1]==0x16)) //判断头
{
if(NO_Buff[2] ==0xad) //判断特有标置位。每次发都不一样，可理解为每个设备特定标签号或基站编号
{
TX_final_times = get_tx_timestamp_u64();
final_msg_set_ts(&Final_Buff[3], TX_final_times); //将时间戳放入buff发送给接收端
dwt_writetxdata(sizeof(Final_Buff), Final_Buff, 0);
dwt_writetxfctrl(sizeof(Final_Buff), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE|DWT_RESPONSE_EXPECTED);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ }; //等待接收距离完成
if (status_reg & SYS_STATUS_RXFCG)
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获取接收长度
if (len <= sizeof(Distance_Buff))
{
dwt_readrxdata(Distance_Buff, len, 0);
}
if((Distance_Buff[0]==0x15)&&(Distance_Buff[1]==0x16)) //判断头
{
if(Distance_Buff[2] ==0xdc) //判断特有标置位。每次发都不一样，可理解为每个设备特定标签号或基站编号
{
Distance = (double)Distance_Buff[3] + ((double)Distance_Buff[4])/100; //接收距离
printf("Distance=%lf\r\n",Distance);
sprintf(LCD_Buff, "Dis:%3.2fm", Distance);
OLED_ShowString(0,2,LCD_Buff);
}
else
{
printf("接收距离判断特殊位失败\r\n");
}
}
else
{
printf("接收距离判断头错误\r\n");
}
}
else
{
printf("TX-F接收失败\r\n");
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); //清除错误标志
printf("发送端接收距离信息的特有标准错误\r\n");
}
}
else
{
printf("发送端接收距离信息的帧头错误\r\n");
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); //清除错误标志
}
}
else
{
// stage = POLL; //数据错误，从头开始
printf("发送端应答接收特定标签号错误\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Tx_Resp_Buff[i]);
// Tx_Resp_Buff[i] =0;
// }
}
}
else
{
printf("发送端应答接收头错误\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Tx_Resp_Buff[i]);
// Tx_Resp_Buff[i] =0;
// }
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); //清除错误标志
printf("发送端应答接收数据错误 !\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Tx_Resp_Buff[i]);
// Tx_Resp_Buff[i] =0;
// }
}
}
}
//首先接收端
#define CYCLE (4294967295.0) //一个周期
unsigned char LCD_Buff[10];
static volatile uint8_t stage;
static uint32_t TX_poll_times,TX_resp_times,TX_final_times;
static uint32_t RX_poll_times,RX_resp_times,RX_final_times;
static volatile uint64_t RX_poll_times_64,RX_resp_times_64,RX_final_times_64;
static uint8_t Poll_Buff[5]={0}; //第一次接收数据的buff
static uint8_t Resp_Buff[5]={0x15,0x16,0xac,0,0}; //回应无关数据给发送端数据的buff
static uint8_t First_Buff[13]={0}; // 接收发送端第一次发送和接收的时间戳
static uint8_t NO_Buff[5]={0x15,0x16,0xad,0,0}; //回应无关信息
static uint8_t Final_Buff[9]={0}; //接收发送端最后一次发送的时间戳
static uint8_t Distance_Buff[13]={0x15,0x16,0xdc,0,0,0,0}; //发送距离
double Ra,Rb,Da,Db,Distance;
int tof_dtu;
int main(void)
{
uint8 len = 0,i;
/* Start with board specific hardware init. */
peripherals_init();
printf("hello dwm1000!\r\n");
// dwt_dumpregisters();
/* 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) == -1)
{
printf("dwm1000 init fail!\r\n");
OLED_ShowString(0,0,"INIT FAIL");
while (1)
{
STM_EVAL_LEDOn(LED1);
deca_sleep(100);
STM_EVAL_LEDOff(LED1);
deca_sleep(100);
}
}
spi_set_rate_high();
/* Configure DW1000. See NOTE 6 below. */
dwt_configure(&config);
dwt_setleds(1);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
OLED_ShowString(0,0,"INIT PASS");
printf("init pass!\r\n");
while(1)
{
dwt_setrxtimeout(0);
dwt_setrxmode(0,0,0);
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)
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (len <= sizeof(Poll_Buff))
{
dwt_readrxdata(Poll_Buff, len, 0);
}
if(Poll_Buff[0]==0x15&&Poll_Buff[1]==0x16) //判断头
{
if(Poll_Buff[2] ==0xab) //判断特有标置位。每次发都不一样，可理解为每个设备特定标签号或基站编号
{
RX_poll_times_64 = get_rx_timestamp_u64(); //获取接收端第一次接收数据的的时间戳
dwt_setrxaftertxdelay(0); //设置发送接收后打开接收的延时时间
dwt_setrxtimeout(6800); //设置接收超时时间
dwt_setrxmode(0,0,0); //设置接收为正常模式
dwt_writetxdata(sizeof(Resp_Buff), Resp_Buff, 0);
dwt_writetxfctrl(sizeof(Resp_Buff), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE|DWT_RESPONSE_EXPECTED);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
if (status_reg & SYS_STATUS_RXFCG)
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
// printf("len =%d\r\n",len);
if (len <= sizeof(First_Buff))
{
dwt_readrxdata(First_Buff, len, 0);
}
if(First_Buff[0]==0x15&&First_Buff[1]==0x16) //判断头
{
if(First_Buff[2] ==0xae) //判断特有标置位。每次发都不一样，可理解为每个设备特定标签号或基站编号
{
RX_resp_times_64 = get_tx_timestamp_u64(); //获取接收端第一次发送数据的的时间戳
RX_final_times_64 = get_rx_timestamp_u64(); //获取接收端最后一次接收的时间戳
final_msg_get_ts(&First_Buff[3], &TX_poll_times);
final_msg_get_ts(&First_Buff[7], &TX_resp_times);
RX_poll_times = (uint32_t)RX_poll_times_64;
RX_resp_times = (uint32_t)RX_resp_times_64;
RX_final_times = (uint32_t)RX_final_times_64; //得到接收端的时间戳
dwt_writetxdata(sizeof(NO_Buff), NO_Buff, 0);
dwt_writetxfctrl(sizeof(NO_Buff), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE|DWT_RESPONSE_EXPECTED);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
if (status_reg & SYS_STATUS_RXFCG)
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
// printf("len =%d\r\n",len);
if (len <= sizeof(Final_Buff))
{
dwt_readrxdata(Final_Buff, len, 0);
}
if(Final_Buff[0]==0x15&&Final_Buff[1]==0x16) //判断头
{
if(Final_Buff[2] ==0xbc) //判断特有标置位。每次发都不一样，可理解为每个设备特定标签号或基站编号
{
final_msg_get_ts(&Final_Buff[3], &TX_final_times); //提取发送端的时间戳
// printf("/*****************************************************/\r\n");
// Ra = (double)(TX_resp_times - TX_poll_times);
// Da = (double)(TX_final_times - TX_resp_times);
// Rb = (double)(RX_final_times - RX_resp_times);
// Db = (double)(RX_resp_times - RX_poll_times);
//
if(TX_resp_times > TX_poll_times) //判断是否超过一周期
{
Ra = (double)((double)TX_resp_times - (double)TX_poll_times);
}
else
{
// printf("/****************************/\r\n");
Ra = (double)((double)TX_resp_times - (double)TX_poll_times);
// printf("Ra=%lf\r\n",Ra);
Ra += CYCLE; //加上周期
// printf("Ra=%lf\r\n",Ra);
}
if(TX_final_times > TX_resp_times)
{
Da = (double)((double)TX_final_times - (double)TX_resp_times);
}
else
{
// printf("/****************************/\r\n");
Da = (double)((double)TX_final_times - (double)TX_resp_times);
// printf("Da=%lf\r\n",Da);
Da += CYCLE;
// printf("Da=%lf\r\n",Da);
}
if(RX_final_times > RX_resp_times)
{
Rb = (double)((double)RX_final_times - (double)RX_resp_times);
}
else
{
//printf("/****************************/\r\n");
Rb = (double)((double)RX_final_times - (double)RX_resp_times);
// printf("Rb=%lf\r\n",Rb);
Rb += CYCLE;
// printf("Rb=%lf\r\n",Rb);
}
if(RX_resp_times > RX_poll_times)
{
Db = (double)(RX_resp_times - RX_poll_times);
}
else
{
//printf("/****************************/\r\n");
Db = (double)((double)RX_resp_times - (double)RX_poll_times);
// printf("Db=%lf\r\n",Db);
Db += CYCLE;
// printf("Db=%lf\r\n",Db);
}
Distance =0;
tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));
tof = tof_dtu * DWT_TIME_UNITS;
Distance = tof * SPEED_OF_LIGHT;
//printf("Dis=%f\r\n",Distance);
Distance = Distance - dwt_getrangebias(config.chan,(float)Distance, config.prf);//距离减去矫正系数
// printf("Ra =%lf\r\n",Ra);
// printf("Rb =%lf\r\n",Rb);
// printf("Da =%lf\r\n",Da);
// printf("Db =%lf\r\n",Db);
// printf("Ra-Db =%lf\r\n",Ra-Db);
// printf("Rb-Da =%lf\r\n",Rb-Da);
//
printf("Dis=%f\r\n",Distance);
int temp =0;
temp = (int)(Distance*100);
//printf("temp=%d\r\n",temp);
Distance_Buff[3] = temp/100; //取整
Distance_Buff[4] = temp%100; //取小数 //打包发送距离
// printf("/**************************************************/\r\n");
// printf("TX_poll_times =%u\r\n",TX_poll_times);
// printf("RX_poll_times =%u\r\n",RX_poll_times);
// printf("RX_resp_times =%u\r\n",RX_resp_times);
// printf("TX_resp_times =%u\r\n",TX_resp_times);
// printf("TX_final_times =%u\r\n",TX_final_times);
// printf("RX_final_times =%u\r\n",RX_final_times);
//
// printf("Distance_Buff[3]=%d\r\n",Distance_Buff[3]);
// printf("Distance_Buff[4]=%d\r\n",Distance_Buff[4]);
sprintf(LCD_Buff, "Dis:%3.2fm", Distance);
OLED_ShowString(0,2,LCD_Buff);
dwt_writetxdata(sizeof(Distance_Buff), Distance_Buff, 0);
dwt_writetxfctrl(sizeof(Distance_Buff), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE);
}
else
{
printf("端接收接收最后一次时间戳失败\r\n");
}
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
printf("接收发送端最后一次时间戳失败\r\n");
}
}
else
{
//stage = POLL; //数据错误，从头开始
printf("接收端最后一次接收特定标签号错误\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Rx_Final_Buff[i]);
// Rx_Final_Buff[i] =0;
// }
}
}
else
{
//stage = POLL; //数据错误，从头开始
printf("接收端最后一次接收头错误\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Rx_Final_Buff[i]);
// Rx_Final_Buff[i] =0;
// }
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
//stage = POLL;
//
printf("接收端最后一次接收数据错误\r\n");
//
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Rx_Final_Buff[i]);
// Rx_Final_Buff[i] =0;
// }
}
}
else
{
printf("接收端第一次接收特定标签号错误\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Rx_Poll_Buff[i]);
// Rx_Poll_Buff[i] =0;
// }
}
}
else
{
printf("接收端第一次接收头错误\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Rx_Poll_Buff[i]);
// Rx_Poll_Buff[i] =0;
// }
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
// stage = POLL;
//
printf("接收端第一次接收数据错误\r\n");
// for(i=0;i<len;i++)
// {
// printf("buff[%d]=%x\r\n",i,Rx_Poll_Buff[i]);
// Rx_Poll_Buff[i] =0;
// }
}
}
}
到了这里，关于UWB-DW1000的TWR测距及代码（五）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
