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