一、TDOA简介
TDOA定位是一种利用时间差定位的方法。通过测量信号到达监测站的时间，可以确定信号源的距离。
利用信号源到各个监测站的距离(以监测站为中心，距离为半径作圆)，就能确定信号的位置。但是绝对时间一般比较难测量，通过比较信号到达各个监测站的绝对时间差，就能作出以监测站为焦点，距离差为长轴的双曲线，双曲线的交点就是信号的位置。
不同于TOA，TDOA（到达时间差）是通过检测信号到达两个基站的绝对时间差，而不是到达的飞行时间来确定移动台的位置，降低了信号源与各个监测站的时间同步要求，但提高了各个监测站的时间同步要求。采用三个不同的基站可以测到两个TDOA，移动站位于两个TDOA决定的双曲线的交点上。
二、TDOA特点
因为是利用信号源到各个监测站的时间差来进行定位，所以各个监测站的时间是需要用同一个时钟才能使用。如果用他们自身的系统时钟，那每个监测站的时钟都不一样无法进行时间差计算。
举个例子：北京时间和纽约时间，如果不换算，直接按他们各自的时间去计算时间差值，那肯定是没办法算出时间差的。要计算时间差，就得把纽约时间换算成对应的北京时间，然后进行计算时间差。或者把北京时间换算成对应的纽约时间进行计算时间差。
也就是说，要使用TDOA定位或者测距，就必须然各个监测站的时间同步起来，让它们在同一时刻都是计时到相同的时间。
比如在15:00时刻，监测站1，2，3，4的计时时间都是15:00，而不是监测站1计时时间是15:00，监测站2计时时间是18:00，监测站3计时时间是22:00，监测站4计时时间是23:00。
三、同步
同步设及两个地方，一个是时间同步，还有一个是晶振偏移不同。晶振偏移不同就是说，同样过了三分钟，监测站1计时了三分零5秒，监测站2计时了两分53秒，监测站3计时了三分20秒，监测站4计时了两分48秒。每个设备的晶振振荡偏移是不一样的，所以晶振偏移也需要补偿。补偿到过了三分钟，每个监测站都是计时三分钟，或者都计时三分20秒，都计时两分50秒或者其他的都可以，只要所有设备计时是一样的就行。
为了方便说，下面把监测站说为基站，需要定位的信号源说为标签。
1.时钟同步
同步思路：用一个同步源去同步定位基站的时钟。让所有定位基站的时钟向同步源时钟对齐。这样所有定位基站都使用同步源的时钟，时钟就对齐了。
参数：
1.时钟同步源设备发送的时间戳为 T3
2.需要同步的基站接收的时间戳为 R3（每个基站都是各自的接收时间戳）
3.同步源设备发送信号到基站的时间戳为 DT（已知条件，所有定位基站必须知道自己与同步源设备的距离，然后根据他们之间的距离算出发送信号所需要的时间戳）。
步骤：
因为时间轴不一样，同步源设备和定位基站不是用同一个时钟，所以有一个实际接收时间戳R3，也有一个假设他们使用同一时钟计时产生的理论接收时间戳 R3a。
则有 R3a = T3+DT。 如果同步成功，也就是同步后他们的计时时钟用同一个，则有 R3 = R3a = T3+DT 。
但是实际上同步源设备和定位基站的时钟是不同步的， 而且分两种情况：
1.如果定位基站的实际接收时间戳比理论接收时间戳大，即R3>R3a，则定位基站和同步源设备的时钟偏差为： d = R3 - R3a = R3 - (T3+DT) 。
2.如果定位基站的实际接收时间戳比理论接收时间戳小，即R3<R3a，则定位基站和同步源设备的时钟偏差为： d = R3a - R3 = (T3+DT) - R3。
定位基站进行时钟同步：
1.如果定位基站的实际接收时间戳比理论接收时间戳大，即R3>R3a，则定位基站同步为同步源设备的时间戳 R3a = R3 - d。
2.如果定位基站的实际接收时间戳比理论接收时间戳小，即R3<R3a，则定位基站同步为同步源设备的时间戳 R3a = R3 + d。
同步后则有：
1.如果R3>R3a，则定位基站同步时间戳 R3a = R3 - d = R3 - (R3 - (T3+DT) ) = T3 + TD 。
2.如果R3<R3a，则定位基站同步时间戳 R3a = R3 + d = R3 + ((T3+DT) - R3) = T3 +TD 。
时钟同步成功，所有定位基站都在用同步源设备的时钟计时。
2.晶振振荡偏移补偿
不同设备之间，晶振振荡偏移是有点偏差的，虽然偏差较小，但是结合光速后，一点偏差也能影响不小。
因为光速太快，所有基站同时发TDOA信号会出现标签同时收到多个基站信号的情况导致定位失败， 所以需要根据基站编号依次进行信号发送。因为从校准完成到发送信号这一时间段内，
但是各基站的晶振偏差不一样，相同时间内，不同基站计时不一样，导致时间误差。为了修正晶振偏差，定位基站要使用同步源设备的晶振偏差。
参数：
1.同步源发送的上一次时间戳为 T1（上一次）
2.同步源发送的当前时间戳为 T2 （当前）
3.基站接收的上一次时间戳为 R1（上一次）
4.基站接收的当前时间戳为 R2 （当前）
5.基站发送TDOA信号时间戳为 Ta
假设同步源设备从R2 到 Ta时刻，晶振偏移了 K，因为基站的晶振偏移要用同步源设备的晶振偏移，则相同时间内，基站的晶振应该也是偏移的 K。
举例说明：
假设基站C 同步完成，但是需要延时2ms 发送TDOA信号，那么在这2ms时间内，同步源设备计时 2100ms, 而自己本身计时 2000ms,那么基站C的延时发送时间就应该是2100ms ,设置的延时发送时间戳应该是 R2+2100ms ,而不是R2+2000ms 。通过这样把所有基站都使用同步源设备的晶振计时，来补偿各个基站的晶振偏移误差。
校准方法：
通过线性插值，用当前和上一次的收发时间戳以及基站的延时发送时间来推导相同延时发送时间上的同步源设备的延时时间
假设基站发送TDOA的延时时间为 TD1，相同延时时间内同步源设备计时时间为 TD2，则有 ：
TD2 = （T2-T1）/（R2-R1）* TD1 //y = kx;
所以，虽然设置的延时时间是 TD1，但是为了跟同步源设备的晶振偏差对齐，所以设置的延时发送时间戳应该是 Ta = R2+ TD2, 而不是R2+ TD1。
TD2是通过TD1 以及当前和上一次的收发时间戳推导出来的，把同步源的晶振偏移误差也用在定位基站上面，这样所有定位基站的晶振偏移都用同步源的晶振偏移。
如果同步源和定位基站之间的晶振偏移是线性关系，那么通过线性插值以后基站的晶振偏移是完全跟同步源同步的。但是同步源和基站之间的晶振偏移基本都是非线性关系。但是把该曲线尽可能分割很小，也接近直线。所以每次定位都计算晶振偏差，误差也很小了。
这样晶振偏移误差就校准了。
我在同步过程中遇到的问题：
1.基站设置的延时发送时间跨周期了。（官方代码是为了方便发时间戳的低四个字节，我直接发五个字节。跨周期后加上一个周期就行）。
2.同步源重新上电，基站接收到的前两次数据不能用。（因为基站是根据当前和上一次的数据进行晶振偏移校准，同步源重新上电后，当前的时间戳是刚上电第一次的时间戳，而上一次的时间戳是断电前的时间戳，中间断开了，不能用，所以同步源每次上电都会在数据包里面的某一位告诉基站是第一次发送数据，基站知道了，就等待接收两次数据后再进行同步）。
3.设备之间的时钟差几秒，导致设置的延时发送有问题（延时发送的时间戳是用同步后的时间戳，而不是自身设备的时间戳，要确保代码执行到延时发送的时候，当前系统时间戳比设置的延时发送时间戳小，如果代码执行到延时发送处时候当前系统时间戳以及大于设置的延时发送时间戳就会卡死不发）
4.TDOA定位基站之间互相收到数据干扰.（可以使用延时接收的方法，到自己该收数据的时间再打开接收）
5.设置延时时间是设置高32位，所以实际延时时间是设置时间的256倍，两个设备时间的延时时间戳差值也是设置延时时间差值的256倍.（因为基站算出的同步时间戳是左移8位后的时间戳，所以获取时间戳的时候不需要左移8位了）。
6.不需要把基站的发送时间戳对齐到同一时刻发送，只需要知道，标签到每个基站的时间差就行。
以下是代码：
获取和设置时间戳代码：
#define TIME_MSG_TS_LEN 5 //时间信息的长度--官方代码是设置和获取低四个字节，我把五个字节都设置和获取出来了
static void final_msg_get_ts(const uint8 *ts_field, uint64_t *ts)
{
int i;
*ts = 0;
for (i = 0; i < TIME_MSG_TS_LEN; i++)
{
*ts += (uint64_t)ts_field[i] << (i * 8); //强转u64类型必须加，不然会出问题
}
} // 从接收的数组里面获取五个字节时间戳
static void final_msg_set_ts(uint8 *ts_field, uint64_t ts)
{
int i;
for (i = 0; i < TIME_MSG_TS_LEN; i++)
{
ts_field[i] = (uint8) ts;
ts >>= 8;
} // 把五个字节时间戳放到数组里面准备发出去
}
同步源设备代码：
uint64_t CCMRAM now_systime,Last_systime,now_systime1,now_systime2;
uint64_t Last_Tx_timestamp;
uint64_t Tx_timestamp = 0;
static uint32_t Txtime;
static uint8_t Tx_Final_Buff[BUFF_SIZE]={0x15,0x16,0xae,0,0,0,0,0,0,0}; //最后一次发送时间戳信息过去
u8 Send_Data_flag;
volatile u8 Pack_Num;
void UWB_SYNC_Source(void)
{
dwt_readsystime((uint8_t *)&now_systime); //获取系统当前时间点
Txtime = (now_systime + (MASTER_TIMES * UUS_TO_DWT_TIME*(ANCHOR_MAX+1)))>> 8; //计算发送时间点
dwt_setdelayedtrxtime(Txtime); //设置延迟发送时间点
// dwt_readsystime((uint8_t *)&now_systime1); //获取系统当前时间点
Tx_timestamp = ((uint64_t)(Txtime&0xfffffffe))<<8|Tx_Ant_Delay; //获取发送端最后一次发送的时间戳
// printf("now_systime = %llu\r\n",now_systime);
// printf("Txtime =%u\r\n",Txtime);
// printf("Tx_timestamp = %llu\r\n",Tx_timestamp);
if(Send_Data_flag ==0) //上电发第一次，告诉基站
{
Send_Data_flag = 1;
Tx_Final_Buff[2] =1;
}
else
{
Tx_Final_Buff[2] = 0x17;
}
if(Pack_Num<255)
{
Pack_Num++;
}
else
{
Pack_Num =1;
}
Tx_Final_Buff[8] = Pack_Num;
final_msg_set_ts(&Tx_Final_Buff[3], Tx_timestamp); //将时间戳放入buff发送给接收端
dwt_writetxdata(sizeof(Tx_Final_Buff), Tx_Final_Buff, 0);
dwt_writetxfctrl(sizeof(Tx_Final_Buff), 0);
dwt_starttx(DWT_START_TX_DELAYED); //延时发送
// printf("now_systime =%llu\r\n",now_systime);
// printf("Txtime = %u\r\n",Txtime);
// printf("Tx_timestamp = %llu\r\n",Tx_timestamp);
// printf("Tx_timestamp - Last_Tx_timestamp=%llu\r\n",(Tx_timestamp - Last_Tx_timestamp));
// printf("now_systime - Last_systime=%llu\r\n",(now_systime - Last_systime));
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_TXFRS)))
{ }; //等待接收距离完成
dwt_readsystime((uint8_t *)&now_systime2); //获取系统当前时间点
// printf("Tx_timestamp=%llu\r\n",Tx_timestamp);
// printf("now_systime1=%llu\r\n",now_systime1);
// printf("now_systime2=%llu\r\n",now_systime2);
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_TX);
printf("Send Complate\r\n");
Last_systime = now_systime;
Last_Tx_timestamp = Tx_timestamp;
}
定位基站代码
uint64_t CCMRAM now_systime,Last_systime,now_systime1,now_systime2;
int64_t Value_timestamp;
double tof_m = 0;
uint64_t Rx_timestamp = 0;
uint64_t Tx_timestamp2 = 0;
uint8_t Timestamp_flag; //1+ 2-
uint8_t Get_DisTimestamp_flag,Cal_K_flag;
double Distance_m = 0.5;
uint8_t len;
static uint8_t TDOA_TxBuff[BUFF_SIZE]={0xad,0xaa};
static uint8_t Rx_Buff[BUFF_SIZE]; //最后一次发送时间戳信息过去
volatile u8 Start_Rece_flag =1;
static uint32_t TDOA_Tx_SetDelayTimes;
static uint64_t TDOA_Tx_ForBuff_Times;
uint64_t T1,T2,R1,R2,R2a,DT;
uint64_t Calibration_Device_DelayTimes,Synchronizer_DelayTimes;
volatile double K,Ky,Kx; //斜率
u32 Rx_Delay_Timestamp;
float x,y,z;
static uint32 status_reg = 0; //状态标志
void UWB_ANCHOR(void)
{
// if(ANCHOR_ID ==ANCHOR_MAX)
{
dwt_setrxtimeout(0);
dwt_setrxmode(0,0,0);
dwt_rxenable(0);
}
// else
// {
//
// dwt_readsystime((uint8_t *)&now_systime);
//
// Rx_Delay_Timestamp = (now_systime + (RX_DELAY_TIMES*UUS_TO_DWT_TIME*(ANCHOR_MAX -ANCHOR_ID))) >> 8;
//
// dwt_setdelayedtrxtime(Rx_Delay_Timestamp); //设置延时接收时间点
//
// dwt_setrxtimeout(0);
// dwt_setrxmode(0,0,0);
// dwt_rxenable(1);
//
// }
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
if (status_reg & SYS_STATUS_RXFCG)
{
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
//printf("len = %d\r\n",len);
if (len <= sizeof(Rx_Buff))
{
dwt_readrxdata(Rx_Buff, len, 0);
}
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); //清除错误标志
if(Rx_Buff[0]==0x15&&Rx_Buff[1]==0x16)
{
if(Rx_Buff[2]==1) //第一次发数据
{
Start_Rece_flag =1;
}
Rx_timestamp = get_rx_timestamp_u64(); //记录接收时间戳
final_msg_get_ts(&Rx_Buff[3], &Tx_timestamp2); //提取发送端的时间戳
/******************************* 晶振偏移补偿*****************************/
if(Start_Rece_flag ==1) //第一次接收
{
Start_Rece_flag++;
T1 = Tx_timestamp2;
R1 = Rx_timestamp;
}
else if(Start_Rece_flag ==2)//第二次接收可以求出K值,并且可以进行TDOA定位
{
Start_Rece_flag++;
T2 = Tx_timestamp2;
R2 = Rx_timestamp;
if(T1>T2)
{
Ky = T2 + CYCLE -T1;
}
else
{
Ky = T2 - T1;
}
if(R1>R2)
{
Kx = R2 + CYCLE -R1;
}
else
{
Kx = R2 - R1;
}
K = (double)Ky/Kx;
Cal_K_flag = 1;
}
else //大于2次需要把最前面那次去除，只留最新的两组收发数据计算最新的K值
{
T1 = T2;
T2 = Tx_timestamp2;
R1 = R2;
R2 = Rx_timestamp;
if(T1>T2)
{
Ky = T2 + CYCLE -T1;
}
else
{
Ky = T2 - T1;
}
if(R1>R2)
{
Kx = R2 + CYCLE -R1;
}
else
{
Kx = R2 - R1;
}
K = (double)Ky/Kx;
Cal_K_flag = 1;
//printf("k=%.15lf\r\n",K);
}
/**********************************晶振偏移补偿完成开始进行时钟同步*******************************************/
if(Get_DisTimestamp_flag ==0) //还没有计算出TD (同步源发送信号到校准源的固定距离算出)
{
tof_m = Distance_m / SPEED_OF_LIGHT;
DT = (uint64_t)(tof_m / DWT_TIME_UNITS);
Get_DisTimestamp_flag =1;
}
R2a = T2 + DT; //跟基站同步时的接收时间戳
// if(R2a > R2) // //对比基站同步时间戳和飞机真实时间戳的差
// {
//
// Timestamp_flag = 1;
// Value_timestamp = R2a - R2;
// printf("Value_timestamp=%llu\r\n",Value_timestamp);
//
// }
// else //(a+b) <= c
// {
//
// Timestamp_flag = 2;
// Value_timestamp = R2 - R2a; //d = c - (a+b)
// printf("Value_timestamp=%llu\r\n",Value_timestamp);
// }
if(Cal_K_flag == 1) //延时发送并进行晶振偏移补偿
{
Cal_K_flag = 0;
TDOA_TxBuff[2] = ANCHOR_ID; //写入基站ID
TDOA_TxBuff[8] = Rx_Buff[8];
Calibration_Device_DelayTimes = TX_DELAY_TIMES*UUS_TO_DWT_TIME*ANCHOR_ID; //每个基站间隔2000uus发信号
TDOA_Tx_SetDelayTimes = (R2 + (Calibration_Device_DelayTimes)) >> 8; //获取延时发送时间点,用设备自己的系统时间去获取
dwt_setdelayedtrxtime(TDOA_Tx_SetDelayTimes); //设置延时发送时间点
Synchronizer_DelayTimes = R2a + (uint64_t)(Calibration_Device_DelayTimes * K); //用同步时间加上延时时间就是延时发送时间戳
// printf("Calibration_Device_DelayTimes=%llu\r\n",Calibration_Device_DelayTimes);
// printf("Synchronizer_DelayTimes=%llu\r\n",Synchronizer_DelayTimes);
TDOA_Tx_ForBuff_Times = Synchronizer_DelayTimes+Tx_Ant_Delay;
// if(TDOA_Tx_ForBuff_Times>=CYCLE)
// {
// printf("/****************************TimesVlaue>CYCLE**********************************/\r\n");
// printf("TDOA_Tx_ForBuff_Times=%llu\r\n",TDOA_Tx_ForBuff_Times);
// }
final_msg_set_ts(&TDOA_TxBuff[3], TDOA_Tx_ForBuff_Times); //将时间戳放入buff发送给接收端
dwt_writetxdata(sizeof(TDOA_TxBuff), TDOA_TxBuff, 0);
dwt_writetxfctrl(sizeof(TDOA_TxBuff), 0);
dwt_starttx(DWT_START_TX_DELAYED); //延时发送
// printf("TDOA_Tx_SetDelayTimes=%u\r\n",TDOA_Tx_SetDelayTimes);
// dwt_readsystime((uint8_t *)&now_systime);
// if((((uint64_t)(TDOA_Tx_SetDelayTimes&0xfffffffe))<<8)<now_systime)
// {
// printf("Times<now_systime\r\n");
// }
// else
// {
// printf("Delay_T =%llu\r\n",((uint64_t)(TDOA_Tx_SetDelayTimes&0xfffffffe))<<8);
// printf("now_systime=%llu\r\n",now_systime);
// }
// printf("TDOA_Tx_ForBuff_Times=%llu\r\n",TDOA_Tx_ForBuff_Times);
// printf("(R2 + (Calibration_Device_DelayTimes)=%llu\r\n",(R2 + Calibration_Device_DelayTimes));
// dwt_starttx(DWT_START_TX_IMMEDIATE); //立刻发送
// printf("Start Send Data\r\n");
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_TXFRS)))
{ }; //等待发送完成
// printf("Send Data Complate\r\n");
}
}
// else
// {
printf("接收头错误\r\n");
printf("Rx_Buff[0]=%x\r\n",Rx_Buff[0]);
printf("Rx_Buff[1]=%x\r\n",Rx_Buff[1]);
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); //清除错误标志
// }
}
else
{
// printf("Error\r\n");
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); //清除错误标志
}
for(uint8_t i =0;i<3;i++)
{
Rx_Buff[i] = 0;
}
}
标签接收代码
uint8_t len;
double X1=0.0,Y1=0.0,Z1=1.0;
double X2=0.0,Y2=2.0,Z2=1.0;
double X3=2.0,Y3=2.0,Z3=1.0;
double X4=2.0,Y4=0.0,Z4=1.0;
static uint32 status_reg = 0; //状态标志
uint64_t CCMRAM now_systime1,now_systime2;
u8 Tag_Rece_Buff[BUFF_SIZE] ={0};
u64 Anchor4_Timestamp,Anchor1_Timestamp,Anchor2_Timestamp,Anchor3_Timestamp;
u64 Tag_Rece4_Timestamp,Tag_Rece1_Timestamp,Tag_Rece2_Timestamp,Tag_Rece3_Timestamp;
u64 A4_Tag_Timestamp_DiffValue,A1_Tag_Timestamp_DiffValue,A2_Tag_Timestamp_DiffValue,A3_Tag_Timestamp_DiffValue;
u8 Rece_Complate_flag;
u8 ID_Buff[100];
u8 ID_Count;
void Tag_Receive(void)
{
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)))
{ };
// dwt_readsystime((uint8_t *)&now_systime1);
if (status_reg & SYS_STATUS_RXFCG)
{
len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (len <= sizeof(Tag_Rece_Buff))
{
dwt_readrxdata(Tag_Rece_Buff, len, 0);
}
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); //清除发送和接收状态
if(Tag_Rece_Buff[0]==0xad&&Tag_Rece_Buff[1]==0xaa)
{
//printf("ANCHOR_ID=%d\r\n",Tag_Rece_Buff[2]);
// ID_Buff[ID_Count] = Tag_Rece_Buff[2];
// ID_Count++;
//
if(Tag_Rece_Buff[2]==1)
{
Tag_Rece1_Timestamp = get_rx_timestamp_u64();
final_msg_get_ts(&Tag_Rece_Buff[3], &Anchor1_Timestamp);
if(Tag_Rece1_Timestamp>Anchor1_Timestamp)
{
A1_Tag_Timestamp_DiffValue = Tag_Rece1_Timestamp - Anchor1_Timestamp;
}
else
{
A1_Tag_Timestamp_DiffValue = Tag_Rece1_Timestamp + CYCLE - Anchor1_Timestamp;
}
}
else if(Tag_Rece_Buff[2]==2)
{
Tag_Rece2_Timestamp = get_rx_timestamp_u64();
//Tag_Rece2_Timestamp += TX_DELAY_TIMES*UUS_TO_DWT_TIME*2;
final_msg_get_ts(&Tag_Rece_Buff[3], &Anchor2_Timestamp);
if(Tag_Rece2_Timestamp>Anchor2_Timestamp)
{
A2_Tag_Timestamp_DiffValue = Tag_Rece2_Timestamp - Anchor2_Timestamp;
}
else
{
A2_Tag_Timestamp_DiffValue = Tag_Rece2_Timestamp + CYCLE - Anchor2_Timestamp;
}
}
else if(Tag_Rece_Buff[2]==3)
{
Tag_Rece3_Timestamp = get_rx_timestamp_u64();
//Tag_Rece3_Timestamp += TX_DELAY_TIMES*UUS_TO_DWT_TIME*2;
final_msg_get_ts(&Tag_Rece_Buff[3], &Anchor3_Timestamp);
if(Tag_Rece3_Timestamp>Anchor3_Timestamp)
{
A3_Tag_Timestamp_DiffValue = Tag_Rece3_Timestamp - Anchor3_Timestamp;
}
else
{
A3_Tag_Timestamp_DiffValue = Tag_Rece3_Timestamp + CYCLE - Anchor3_Timestamp;
}
}
else if(Tag_Rece_Buff[2]==4)
{
Tag_Rece4_Timestamp = get_rx_timestamp_u64();
// Tag_Rece4_Timestamp += TX_DELAY_TIMES*UUS_TO_DWT_TIME*3;
final_msg_get_ts(&Tag_Rece_Buff[3], &Anchor4_Timestamp);
if(Tag_Rece4_Timestamp>Anchor4_Timestamp)
{
A4_Tag_Timestamp_DiffValue = Tag_Rece4_Timestamp - Anchor4_Timestamp;
}
else
{
A4_Tag_Timestamp_DiffValue = Tag_Rece4_Timestamp + CYCLE - Anchor4_Timestamp;
}
Rece_Complate_flag = 1;
}
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); //清除错误标志
}
// dwt_readsystime((uint8_t *)&now_systime2);
//
// printf("now_systime1=%llu\r\n",now_systime1);
// printf("now_systime2=%llu\r\n",now_systime2);
//
// if(ID_Count ==100)
// {
// for(u8 i=0;i<100;i++)
// {
// printf("%d-ID=%d\r\n",i,ID_Buff[i]);
if(ID_Buff[i]-ID_Buff[i-1]!=1&&ID_Buff[i]!=1)
{
printf("Anchor1_Timestamp=%llu\r\n",Anchor1_Timestamp);
printf("Anchor2_Timestamp=%llu\r\n",Anchor2_Timestamp);
printf("Anchor3_Timestamp=%llu\r\n",Anchor3_Timestamp);
printf("Anchor4_Timestamp=%llu\r\n",Anchor4_Timestamp);
}
// }
// ID_Count =0;
// }
if(Rece_Complate_flag ==1)
{
printf("Anchor1_Timestamp=%llu\r\n",Anchor1_Timestamp);
printf("Anchor2_Timestamp=%llu\r\n",Anchor2_Timestamp);
printf("Anchor3_Timestamp=%llu\r\n",Anchor3_Timestamp);
printf("Anchor4_Timestamp=%llu\r\n",Anchor4_Timestamp);
// printf("Anchor1 to Tag Timestamp=%llu\r\n",A1_Tag_Timestamp_DiffValue);
// printf("Anchor2 to Tag Timestamp=%llu\r\n",A2_Tag_Timestamp_DiffValue);
// printf("Anchor3 to Tag Timestamp=%llu\r\n",A3_Tag_Timestamp_DiffValue);
// printf("Anchor4 to Tag Timestamp=%llu\r\n",A4_Tag_Timestamp_DiffValue);
//
// A1_Tag_Timestamp_DiffValue =0;
// A2_Tag_Timestamp_DiffValue =0;
// A3_Tag_Timestamp_DiffValue =0;
// A4_Tag_Timestamp_DiffValue =0;
Rece_Complate_flag =0;
}
}
A1_Tag_Timestamp_DiffValue 到 A4_Tag_Timestamp_DiffValue （t1 到 t4）就是标签分别到四个基站的时间差值。
知道四个定位基站的坐标（已知条件），标签到四个基站的时间差也计算出来了，然后根据下面的双曲线公式就可以计算出标签的位置
UWB文章整理如下：
UWB-DW1000初始化、发送和接收详解（一）：https://blog.youkuaiyun.com/weixin_46107106/article/details/128221089?spm=1001.2014.3001.5501
UWB-DW1000的天线延迟补偿和发射功率调节（二）：https://blog.youkuaiyun.com/weixin_46107106/article/details/128715094?spm=1001.2014.3001.5502
DW1000的温度补偿及基站，标签天线延迟校准（三）：https://blog.youkuaiyun.com/weixin_46107106/article/details/128806871?spm=1001.2014.3001.5502
UWB测距 方法，双向双边测距法（DS-TWR）（四）：https://blog.youkuaiyun.com/weixin_46107106/article/details/129200945?spm=1001.2014.3001.5502
UWB-DW1000的TWR测距及代码（五）：https://blog.youkuaiyun.com/weixin_46107106/article/details/129204267?spm=1001.2014.3001.5502
UWB定位 - 三球定位及算出的两个交点取舍（六）：https://blog.youkuaiyun.com/weixin_46107106/article/details/130645080?spm=1001.2014.3001.5502
UWB定位的误差原因及摆放和布局（七）：https://blog.youkuaiyun.com/weixin_46107106/article/details/130807768?spm=1001.2014.3001.5502
由把dwm1000模块从STM32单片机移植到N32单片机（国民技术）问题整理（八）：https://blog.youkuaiyun.com/weixin_46107106/article/details/138178919?spm=1001.2014.3001.5502
UWB - DW1000的延时发送和延时接收（九）：https://blog.youkuaiyun.com/weixin_46107106/article/details/144590332
TDOA-无线时钟同步及晶振偏差校准（十）：https://blog.youkuaiyun.com/weixin_46107106/article/details/145174675
扫一扫
1002
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
