UWB爬坑笔记之基于时间槽的测距实现,确实这是个细致活,关键的基础要求,是基站或者标签的tick必须是在可接受的误差范围内,至少是us级别的误差,误差太大,基本上做不出精准的时间槽位控制。
主要涉及的关键点:
1、基站投票选择主站,并对基站内部进行排序,并通过广播方式广播给其他的基站;
2、基站之间同步主站的时间戳,同步的过程其实就是选主站的过程;
基站上电后,就广播自己的tick,同时,接收其他基站的广播信息,然后根据基站的tick值大小,谁大决定谁是主站;
要考虑基站掉线的问题;
基站新进来的问题;
只有主站才分配时间槽;
3、由于基站是通过发送广播A包的方式回复标签的P帧,所以其他基站可以监听A包,然后基站根据自己的序号,决定自己在收到第几个A包后,回A包,解决多个站回A包冲突的问题;
4、标签根据基站的时间戳,和自己的时间戳,并根据基站返回的时间槽位号,修正测距定时器,保证每隔2秒的定位能落在基站分配的时间槽位号上,避免和其他标签产生冲突;
基站投票选择主站,并对基站内部进行排序,并通过广播方式广播给其他的基站;
typedef struct AnchorInfo_
{
uint32_t anchor_id;
uint32_t anchor_tick;
uint32_t receive_tick;
int rssi;
int distance;//SSR测距
int master_flag;//主标识
int master_anchor_id;//不是主站 认为谁是主站
} AnchorInfo;
static void check_and_choose_master_anchor(void){
int max_anchor_id_index = 0;
uint32_t max_anchor_id = 0;
int i,j,temp; //定义三个整型变量
int n = currentSyncAnchorNums+1;
static int continue_no_sync_packet_times = 0;
if (currentSyncAnchorNums == 0){
//
continue_no_sync_packet_times++;
if (continue_no_sync_packet_times >= 3){
if (!getAnchorMasterFlag()){
logD("choose master number:%d,continue_no_sync_packet_times:%d ", currentSyncAnchorNums, continue_no_sync_packet_times);
setAnchorMasterFlag(1);
}
}
return;
}
continue_no_sync_packet_times = 1;
anchorSyncList[currentSyncAnchorNums].anchor_id = getUWBAddr();
for (j=0;j<n-1;j++)
{
for (i=0;i<n-1-j;i++)
{
if(anchorSyncList[i].anchor_id < anchorSyncList[i+1].anchor_id)
{
swrap_anchor_info(&anchorSyncList[i], &anchorSyncList[i+1]);
}
}
}
max_anchor_id = anchorSyncList[0].anchor_id ;
max_anchor_id_index = 0;
if (max_anchor_id == getUWBAddr()){
//i will the master anchor
setAnchorInnerId(0);
setAnchorMasterFlag(1);
}else{
setAnchorMasterFlag(0);
for(int i=0; i < n; i++)
{
if (anchorSyncList[i].anchor_id == getUWBAddr()){
//内部序号 为后面多站测距排序做准备
setAnchorInnerId(i);
break;
}
}
}
//更新当前的主站标识
masterAnchorInf.anchor_id = max_anchor_id;
//
if(getDebugFlag() || anchor_sync_master_tick_flag){
logD("choose master number:%d selected anchor_id:%d, tick:%d,inner_id:%d", currentSyncAnchorNums, max_anchor_id, anchorSyncList[max_anchor_id_index].anchor_tick, getAnchorInnerId());
}
}
static int add_anchor_into_synclist(AnchorInfo *anchor_item)
{
if (anchor_item == NULL)
{
return -1;
}
uint32_t anchor_id = anchor_item->anchor_id;
int i = 0;
#if 0
for(; i < MAX_ANCHOR_NUM_INZONE; i++)
{
if (anchorSyncList[i].anchor_id == anchor_id)
{
anchorSyncList[i].anchor_tick = anchor_item->anchor_tick;
anchorSyncList[i].receive_tick = anchor_item->receive_tick;
anchorSyncList[i].distance = anchor_item->distance;
anchorSyncList[i].rssi = anchor_item->rssi;
anchorSyncList[i].master_flag = anchor_item->master_flag;
anchorSyncList[i].master_anchor_id = anchor_item->master_anchor_id;
return 0;
}
}
#endif
if (currentSyncAnchorNums < MAX_ANCHOR_NUM_INZONE)
{
i = currentSyncAnchorNums;
anchorSyncList[i].anchor_id = anchor_id;
anchorSyncList[i].anchor_tick = anchor_item->anchor_tick;
anchorSyncList[i].distance = anchor_item->distance;
anchorSyncList[i].rssi = anchor_item->rssi;
anchorSyncList[i].master_flag = anchor_item->master_flag;
anchorSyncList[i].master_anchor_id = anchor_item->master_anchor_id;
currentSyncAnchorNums++;
}
else
{
return -1;
}
return 0;
}基站之间同步主站的时间戳;
#define SYNC_ANCHOR_MASTER_TIMEOUT 20000
#define SYNC_ANCHOR_MASTER_RETRY_TIMES 5
#define SYNC_PAYLOAD_TICK_INDEX 2
#define SYNC_PAYLOAD_MASTER_FLAG_INDEX 6
#define SYNC_PAYLOAD_MASTER_ADDRESS_INDEX 7
#define MAX_ANCHOR_NUM_INZONE 6
void sync_tick_msg_send(uint8_t maybe_collision_flag)
{
//choose last received max id anchor
check_and_choose_master_anchor();
//清空上次的记录
currentSyncAnchorNums = 0;
memset((void *)anchorSyncList, 0x00, sizeof(AnchorInfo)*MAX_ANCHOR_NUM_INZONE);
//set idle
dwt_forcetrxoff();
//dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, (uint8)SYS_CTRL_TRXOFF) ; // Disable the radio
dwt_setrxtimeout(0);
dwt_setrxaftertxdelay(RX_RESPONSE_TURNAROUND);
//组数据帧
msg_sync_send.destAddr[0] = 0xFF;
msg_sync_send.destAddr[1] = 0xFF;
msg_sync_send.messageData[0] = 'X'; //SYNC TIMER
msg_sync_send.messageData[1] = tag_sync_seq++;
uint32_t curr_tick = HAL_GetTick();
final_msg_set_ts_32(&msg_sync_send.messageData[SYNC_PAYLOAD_TICK_INDEX], curr_tick);
final_msg_set_ts_32(&msg_sync_send.messageData[SYNC_PAYLOAD_MASTER_FLAG_INDEX], getAnchorMasterFlag());
final_msg_set_ts_32(&msg_sync_send.messageData[SYNC_PAYLOAD_MASTER_ADDRESS_INDEX], masterAnchorInf.anchor_id);
int delay_value = rand() % 10;
uint32_t random_delay = ( 10 * delay_value * UUS_TO_DWT_TIME) >> 8;
if (!maybe_collision_flag)
{
random_delay = 0;
}
int packet_length = 30;
dwt_writetxdata(packet_length, (uint8_t *)&msg_sync_send, 0); /* Zero offset in TX buffer. */
dwt_writetxfctrl(packet_length, 0, 1); /* Zero offset in TX buffer, ranging. */
uint32_t delay_tx_time = dwt_readsystimestamphi32() + poll_resp_delay_time_2 + random_delay;//poll_resp_delay_time_1
dwt_setdelayedtrxtime(delay_tx_time);
int ret = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
if (ret != DWT_SUCCESS)
{
rx_main();
return;
}
if(getDebugFlag() || anchor_sync_master_tick_flag){
logD("A sync_tick_msg_send [%d] success, tick:%d,delay_value:%d\r\n", msg_sync_send.messageData[1], curr_tick, delay_value);
}
}
接收包处理
case 'X':
{
uint32_t anchor_tick = 0;
uint32_t anchor_master_id = 0;
uint8_t seq = msg_f->messageData[1];
final_msg_get_ts(&msg_f->messageData[SYNC_PAYLOAD_TICK_INDEX], &anchor_tick);
final_msg_get_ts(&msg_f->messageData[SYNC_PAYLOAD_MASTER_ADDRESS_INDEX], &anchor_master_id);
AnchorInfo anchor_item = {0};
anchor_item.rssi = dwt_read16bitoffsetreg(RX_FQUAL_ID, 0x6) ;
anchor_item.anchor_id = sourceaddress;
anchor_item.anchor_tick = anchor_tick;
anchor_item.receive_tick = received_p_tick;
anchor_item.master_anchor_id = anchor_master_id;
anchor_item.master_flag = msg_f->messageData[SYNC_PAYLOAD_MASTER_FLAG_INDEX];
if(getDebugFlag() || anchor_sync_master_tick_flag)
{
logD("receive X sourceaddress:%d [%d ] rssi:%d ,tick:%d,anchor_tick:%d.\r\n", sourceaddress, seq, anchor_item.rssi, received_p_tick, anchor_tick);
}
//if (received_p_tick < anchor_tick)
if (masterAnchorInf.anchor_id == sourceaddress
&& (received_p_tick < anchor_tick))
{
//同步主站的TICK
uint32_t now = HAL_GetTick();
int diff = now - received_p_tick;
SYNC_TIME(anchor_tick + diff);
}
add_anchor_into_synclist(&anchor_item);
//reset status
dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_RX_GOOD | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_TX));
goto ok_end;
}
break;
case 'T':
{
//收到其他基站的同步帧包,如果自己是主站,则回复自己的tick,并延时回复M
uint32_t anchor_tick = 0;
final_msg_get_ts(&msg_f->messageData[SYNC_PAYLOAD_TICK_INDEX], &anchor_tick);
AnchorInfo anchor_item = {0};
anchor_item.rssi = dwt_read16bitoffsetreg(RX_FQUAL_ID, 0x6) ;
anchor_item.anchor_id = sourceaddress;
anchor_item.anchor_tick = anchor_tick;
anchor_item.receive_tick = received_p_tick;
anchor_item.master_flag = msg_f->messageData[SYNC_PAYLOAD_MASTER_FLAG_INDEX];
add_anchor_into_synclist(&anchor_item);
uint32_t now = HAL_GetTick();
int diff = now - received_p_tick;
uint8_t need_modify_self_2_manager = 0;
//if (received_p_tick < anchor_tick)
if (anchor_item.master_flag)
{
//同步主站的时间戳
SYNC_TIME(anchor_tick + diff);
}
if (!getAnchorMasterFlag())
{
//reset status
dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_RX_GOOD | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_TX));
goto ok_end;
}
msg_f_send.messageData[0] = 'R'; //Broadcast response message
msg_f_send.messageData[1] = WORK_MODE_BROADCAST_WITH_CALC_DISTANCE_TAG;//tag mode depend anchor configure
int delay_value = rand() % 5;
uint32_t random_delay = ( 100 * delay_value * UUS_TO_DWT_TIME) >> 8; //预防碰撞
uint64 resp_rx_ts = get_rx_timestamp_u64();
uint32_t delay_tx_time = dwt_readsystimestamphi32() + poll_resp_delay_time_2 + random_delay;//poll_resp_delay_time_1
dwt_setdelayedtrxtime(delay_tx_time) ;
msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_MASTERFLAG_INDEX] = (uint8_t)getAnchorMasterFlag();
uint32_t curr_tick = HAL_GetTick();
final_msg_set_ts_32(&msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_TICK_INDEX], curr_tick);
/* Response TX timestamp is the transmission time we programmed plus the antenna delay. */
uint64 delay_tx_time_ts = (((uint64)(delay_tx_time & 0xFFFFFFFEUL)) << 8) ;
if (TX_ANT_DLY != 0)
{
delay_tx_time_ts += getRxAntDelay();
}
final_msg_set_ts(&msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_TX_TIME_INDEX], delay_tx_time_ts);
final_msg_set_ts(&msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_RX_TIME_INDEX], resp_rx_ts);
uint16_t rssi = dwt_read16bitoffsetreg(RX_FQUAL_ID, 0x6) ;
msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_RSSI_INDEX] = (uint8_t)rssi & 0x00ff;
msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_RSSI_INDEX + 1] = (uint8_t)(rssi >> 8);
dwt_writetxdata(17 + SYNC_FRAME_T_ANCHOR_RSSI_INDEX + 4, (uint8 *)&msg_f_send, 0) ; // write the frame data
dwt_writetxfctrl(17 + SYNC_FRAME_T_ANCHOR_RSSI_INDEX + 4, 0, 1);
dwt_setrxtimeout(0);
int ret = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
if (ret != 0)
{
if(getDebugFlag() || anchor_sync_master_tick_flag)
{
logD("dwt_starttx ret:%d source:%d rssi:%d received.\r\n", ret, sourceaddress, rssi);
}
//reset status
dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_RX_GOOD | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_TX));
goto ok_end;
}
if(getDebugFlag() || anchor_sync_master_tick_flag)
{
logD("sourceaddress :%d rssi:%d received.\r\n", sourceaddress, rssi);
}
//reset status
dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_RX_GOOD | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_TX));
//进入rx
return;
}
break;
case 'R':
{
//收到主站回复的R,则自己为从站
uint32_t poll_tx_ts;
uint32 poll_rx_ts;
uint32 resp_tx_ts;
uint32_t resp_rx_ts;
int32 rtd_init, rtd_resp;
uint32_t master_anchor_curr_tick = 0;
poll_tx_ts = dwt_readtxtimestamplo32();
resp_rx_ts = dwt_readrxtimestamplo32();
//reset status
dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_RX_GOOD | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_TX));
final_msg_get_ts(&msg_f->messageData[SYNC_FRAME_T_ANCHOR_TX_TIME_INDEX], &resp_tx_ts);
final_msg_get_ts(&msg_f->messageData[SYNC_FRAME_T_ANCHOR_RX_TIME_INDEX], &poll_rx_ts);
final_msg_get_ts(&msg_f->messageData[SYNC_FRAME_T_ANCHOR_TICK_INDEX], &master_anchor_curr_tick);
float clockOffsetRatio ;
/* Read carrier integrator value and calculate clock offset ratio. See NOTE 11 below. */
clockOffsetRatio = dwt_readcarrierintegrator() * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
/* Compute time of flight and distance, using clock offset ratio to correct for differing local and remote clock rates */
rtd_init = resp_rx_ts - poll_tx_ts;
rtd_resp = resp_tx_ts - poll_rx_ts;
double tof = ((rtd_init - rtd_resp * (1.0f - clockOffsetRatio)) / 2.0f) * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
/* Display computed distance. */
int b = (int) (distance * 1000 + 0.5) ; //小数点后三位前移,并四舍五入
float c = (float)b / 1000;
int integer = (int)c;
int deciaml = (int)(((double)c - (double)(int)c) * 1000);
uint16_t rssi = (msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_RSSI_INDEX + 1] << 8) | msg_f_send.messageData[SYNC_FRAME_T_ANCHOR_RSSI_INDEX];
int distance_cm = integer * 100 + deciaml;//cm
if(getDebugFlag() || anchor_sync_master_tick_flag)
{
logD("sourceaddress :%d distance:%f rssi:%d distance_cm:%d %d.%d ignored.\r\n", sourceaddress, distance, rssi, distance_cm, integer, deciaml);
}
#if 0
if (integer > 200)
{
//忽略这个站
goto ok_end;
}
#endif//
//
if (!anchor_sync_master_tick_flag)
{
//如果不在同步主站时间戳的状态,收到了另一个主站的信息, 不做处理
if(getDebugFlag())
{
logD("ignore sourceaddress :%d rssi:%d ignored, order manager anchor:%d, old getAnchorMasterFlag:%d.\r\n", sourceaddress, rssi, masterAnchorInf.anchor_id, getAnchorMasterFlag());
}
goto ok_end;
}
if(getDebugFlag() || anchor_sync_master_tick_flag)
{
logD("sourceaddress :%d rssi:%d, master_tick:%d, order manager anchor:%d, old rssi:%d.\r\n", sourceaddress, rssi, master_anchor_curr_tick, masterAnchorInf.anchor_id, masterAnchorInf.rssi);
}
masterAnchorInf.anchor_id = sourceaddress;
masterAnchorInf.anchor_tick = master_anchor_curr_tick;
masterAnchorInf.distance = distance_cm;
masterAnchorInf.rssi = rssi;
//设本站为从站
setAnchorMasterFlag(0);
#if 1
//同步时间戳
uint32_t now = HAL_GetTick();
int diff = now - received_p_tick;
SYNC_TIME(master_anchor_curr_tick + diff);
//dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_RX_GOOD | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_TX));
if (anchor_sync_master_tick_flag){
//清除同步标识
anchor_sync_master_tick_flag = 0;
rx_main();
return;
}
#endif
}
break;基站根据自己的序号,决定自己在收到第几个A包后,回A包,解决多个站回A包冲突的问题;
case 'A':
{
//logD("\r\n[%u][tsr] msg[%d][%d][%d][%c],p:%d, f:%d,tick:%d\r\n", tick,sourceaddress, targetaddress,msg_f->seqNum, msg_f->messageData[0], received_p_count, received_p_count);
//dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, (uint8)SYS_CTRL_TRXOFF) ; // Disable the radio
//dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_SFDTOC_OFFSET, config.sfdTO);
dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_RX_GOOD | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_TX));
int status = get_anchor_status();
int inner_id = msg_f->messageData[RES_ANCHOR_INNER_INDEX];
//排队 1 2 3
//if(last_time_delay_post_a_frame_flag == 1 && (last_tag_range_num <= 2 || (inner_id + 1) == getAnchorInnerId()))
if(last_time_delay_post_a_frame_flag == 1 && ((inner_id + 1) == getAnchorInnerId() &&
inner_id < last_tag_range_num))
{
last_time_delay_post_a_frame_flag = 0;
int short_address = last_time_delay_post_a_frame_to_tag & 0xffff;
if (targetaddress != last_time_delay_post_a_frame_to_tag && targetaddress != short_address)
{
if(getDebugFlag())
{
logD("targetaddress :%d,current_wait_:%d\r\n", targetaddress, current_anchor_wait_f_tag_id);
}
goto ok_end;
}
int delay_value = rand() % 4;
uint32_t random_delay = ( 100 * delay_value * UUS_TO_DWT_TIME) >> 8;
if (!maybe_collision_flag)
{
random_delay = 0;
}
uint32_t delay_tx_time = dwt_readsystimestamphi32() + last_delay_poll_resp_delay_time + random_delay;//poll_resp_delay_time_1
dwt_setdelayedtrxtime(delay_tx_time) ;
dwt_setrxtimeout(0);
//设置最大的SFD超时,保准能收到F帧
//dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_SFDTOC_OFFSET, 0);
dwt_setrxaftertxdelay(TAG_POST_FINAL_DELAY_TIME);
int ret = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
if (ret != DWT_SUCCESS)
{
logD("not master-A dwt_starttx failed delay:%d\r\n", ret);
dwt_forcetrxoff();
goto ok_end;
}
//从站更新等待标签id的f帧
current_anchor_wait_f_tag_id = last_time_delay_post_a_frame_to_tag;
current_anchor_wait_f_last_tick = HAL_GetTick();
}
}标签根据基站的时间戳,和自己的时间戳,并根据基站返回的时间槽位号,修正测距定时器,保证每隔2秒的定位能落在基站分配的时间槽位号上,避免和其他标签产生冲突;
//master anchor check the slot
instance_data_t* inst = instance_get_local_structure_ptr(0);
int slot = index;//sourceaddress % inst->numSlots;
int error = 0;
int currentSlotTime = 0;
int expectedSlotTime = 0;
//find the time in the current superframe
int sframePeroid_ms = inst->sframePeriod_ms;
if (tag_type == TAG_TYPE_NORMAL_PEOPLE_MODE)
{
sframePeroid_ms = PEOPLE_MODE_PEROID;//ANCHOR_SLOT_PEROID_TIME * 200;
}
else if (tag_type == TAG_TYPE_CAR_MODE)
{
sframePeroid_ms = CAR_MODE_PEROID;//ANCHOR_SLOT_PEROID_TIME * 50;
}
else if (tag_type == TAG_TYPE_OTHER_MODE)
{
sframePeroid_ms = OTHER_MODE_PEROID;//ANCHOR_SLOT_PEROID_TIME * 100;
}
else if (tag_type == TAG_TYPE_LOWER_POWER)
{
sframePeroid_ms = SLEEP_MODE_PEROID;//ANCHOR_SLOT_PEROID_TIME * 15 * 200;//30 2s 15
}
currentSlotTime = current_taginfo[index].last_tick % sframePeroid_ms;
//this is the slot time the poll should be received in (Mask 0x07 for the 8 MAX tags we support in TREK)
expectedSlotTime = (slot) * inst->slotDuration_ms; //
//error = expectedSlotTime - currentSlotTime
error = expectedSlotTime - currentSlotTime;
inst->tagSleepCorrection_ms = error;
#if 1//基站计算需要标签修正的值,要么提前、要么延后,这部分逻辑可以在标签完成。
if (inst->tagSleepCorrection_ms < 0)
{
inst->tagSleepCorrection_ms = 0 - inst->tagSleepCorrection_ms;
//-值
inst->tagSleepCorrection_ms += sframePeroid_ms; //min is at least 1.5 periods
}
#endif
#ifndef MASTER_FLAG
inst->tagSleepCorrection_ms = 0;
#endif
msg_f_send.messageData[RES_TAG_SLP0] = inst->tagSleepCorrection_ms & 0xFF ;
msg_f_send.messageData[RES_TAG_SLP1] = (inst->tagSleepCorrection_ms >> 8) & 0xFF;
//
msg_f_send.messageData[RES_TAG_ADD0] = slot & 0xFF;
msg_f_send.messageData[RES_TAG_ADD1] = (slot >> 8) & 0xFF;
int slot_type = 0; //0:2000 1:500 2:1000
if (sframePeroid_ms == OTHER_MODE_PEROID)
{
slot_type = 2;
}
else if (sframePeroid_ms == CAR_MODE_PEROID)
{
slot_type = 1;
}
msg_f_send.messageData[RES_TAG_SLOT_TYPE] = slot_type;
current_taginfo[index].slot_num = slot;其它能力,通过UWB基站升级标签
1、基站配置进入升级模式
2、基站通过串口接收ota数据,然后通过UWB广播消息发送给标签,然后标签升级;
标签和基站之间的应答可靠性
CRC校验

-------------------广告线---------------
项目、合作,欢迎勾搭,邮箱:promall@qq.com
本文为呱牛笔记原创文章,转载无需和我联系,但请注明来自呱牛笔记 ,it3q.com
