UWB-DW1000的TWR测距及代码（五）
UWB测距过程很简单，两个设备（A和B）。设备A先发送，设备B等待接收
- 设备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 ,所以不用担心会溢出两次的问题，因为一个测距过程也就是几个毫秒内完成，光速传播。
//首先发送端
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
// {
// printf("buff[%d]=%x\r\n",i,Tx_Resp_Buff[i]);
// Tx_Resp_Buff[i] =0;
// }}}else{printf("发送端应答接收头错误\r\n");
// for(i=0;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
// {
// 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
// {
// 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
// {
// 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
// {
// printf("buff[%d]=%x\r\n",i,Rx_Final_Buff[i]);
// Rx_Final_Buff[i] =0;
// }} }else{printf("接收端第一次接收特定标签号错误\r\n");
// for(i=0;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
// {
// 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
// {
// printf("buff[%d]=%x\r\n",i,Rx_Poll_Buff[i]);
// Rx_Poll_Buff[i] =0;
// }}}
}
本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！