DWM1000 多个标签定位讨论 --[蓝点无限]
多标签代码已经基本实现,完成代码可以在论坛上下载
http://bphero.com.cn/forum.php?mod=viewthread&tid=53&fromuid=2
蓝点DWM1000 模块已经打样测试完毕,有兴趣的可以申请购买了,更多信息参见 蓝点论坛
20181125 更新,按照下面的逻辑,已经完成代码,全部代码编译通过,需要后期测试。
1 单个标签定位模式
定位实现基于DWM1000 基础API 实验SS-TWR或者DS-TWR方法,非官方定位源码,官方定位源码使用的是大状态机。
当单个标签定位基本流程是TAG 与ANTHOR0 基于SS/DS-TWR计算距离,然后依次再 ANTHOR1 ANTHOR2计算距离,分别步骤如下图的0 , 1 ,2 ,其中每一步都会有poll、response 以及finnal 多条信息。
而且,通过代码知道,tag 发送一定信息后,会进入等待状态,而anthor0 发送一条信息后也会进入等待状态,但是由于TAG分别与各个ANTHOR通信,所以中间的等待状态不会被破坏。
2 多个标签定位模式
下图是一个多标签定位简图,图中 有两个TAG,3个ANTHOR,其中每个TAG依然单独分别按照顺序与ANTHOR0/1/2 通信
但是会出现如下几种特殊情况需要处理:
1 TAG0 可能和 TAG1 同时给某一个ANCHOR发送信息,这个同时是相对广义的,因为通过代码发现,TAG和ANCHOR 之间测距需要一段时间,交换多条信息后才能实现,假如TAG0 和 ANTHOR已经建立连接,正在交换信息,若TAG1 也发送Poll 信息给ANTHOR0时,ANTHOR0 需要丢弃TAG1 的信息。
处理方法:当ANTHOR 接收到一条Poll信息后,更新目标TAG ID,若后面再接收到信息TAG ID与poll 信息TAG ID一致回复finnal 信息,否则认为冲突不做处理,若连续接收到两条poll 信息,更新目标TAG ID,以最后一次的TAG ID为准。
2 更为严重的是,TAG1 发送的任何信息TAG0都会收到,当然可以和上面一样比较TAG ID后可以丢弃,但是会导致TAG0 退出正常的测距循环(每次TAG 和 ANTHOR 进行测距,多条信息依次发送,当TAG发送一条信息后进入等待状态,若此时收到TAG1的信息,那TAG0就会退出与ANTHOR之间测距),这样的严重后果是,两个TAG相互发送数据,导致每个TAG均不能完成任何依次完成的测距。
处理办法:这个问题有一点类似“多核”问题,需要引入"锁"或者“信号量”的概念,ANTHOR 可以认为是资源。
1 程序初始化,TAG0默认获得"锁"或者“信号量”,与三个ANTHOR 发送信息,测试距离,当测距后。
2 TAG0 发送释放信号量信息,若网络中有其他TAG,收到该信息后,回复给TAG0(只有TAG可以回复该信息),且将TAG ID回复给TAG0,可以扩展其他信息。
3 TAG0 收到信号量请求信息后,发送“信号量”释放信息,“信号量”释放信息包含TAG ID以及其他扩展信息。
4 TAG收到信号量后,与系统中的三个ANTHOR进行数据交换,进行测距,测距完成后,发送释放“信号量”信息
5 TAG0 收到释放信号量信息,回收信号量,并回复给之前TAG
6 TAG0 根据网络状况,决定自己测距,还是继续释放信号量给其他TAG
其它问题:SS-TWR/DS-TWR 都会遇到问题,同一个区域内,如果有多个定位网络,则由于没有进行有效过滤,多个网络如果模式相同,数据相互影响(相同模式:包含射频频率 preamble 等设定相同)
后期需要引入PANID 以及 filter等,将不符合的信息直接在底层就过滤掉。
TAG 共享信号量代码参考
1 #ifdef TAG 2 /* Set expected response's delay and timeout. See NOTE 4 and 5 below. 3 * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */ 4 dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); 5 dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); 6 7 OLED_ShowString(0,0,"DS TWR TAG:"); 8 OLED_ShowString(0,2,"Distance:"); 9 10 if(TAG_ID == MASTER_TAG) 11 { 12 Semaphore_Enable = 1 ; 13 Waiting_TAG_Release_Semaphore = 0; 14 } 15 else 16 { 17 Semaphore_Enable = 0 ; 18 } 19 //Master TAG0 20 while(1) 21 { 22 if(Semaphore_Enable == 1) 23 { 24 //send message to anthor,TAG<->ANTHOR 25 //measure_distance(); add later 26 Semaphore_Enable = 0 ; 27 if(TAG_ID != MASTER_TAG) 28 { 29 //send release semaphore to master tag 30 Semaphore_Release[ALL_MSG_SN_IDX] = frame_seq_nb; 31 Semaphore_Release[ALL_MSG_TAG_IDX] = TAG_ID; 32 dwt_writetxdata(sizeof(Semaphore_Release), Semaphore_Release, 0); 33 dwt_writetxfctrl(sizeof(Semaphore_Release), 0); 34 35 dwt_starttx(DWT_START_TX_IMMEDIATE ); 36 while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)) 37 } 38 } 39 40 if(TAG_ID == MASTER_TAG)//master tag 41 { 42 //statistics tag 43 if(sum(Semaphore[]) == 0) 44 { 45 for(TAG_INDEX = 0; TAG_INDEX <MAX_TAG; TAG_INDEX++) 46 { 47 Tag_Statistics[ALL_MSG_SN_IDX] = frame_seq_nb; 48 Tag_Statistics[ALL_MSG_TAG_IDX] = TAG_INDEX; 49 dwt_writetxdata(sizeof(Tag_Statistics), Tag_Statistics, 0); 50 dwt_writetxfctrl(sizeof(Tag_Statistics), 0); 51 dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); 52 53 while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) 54 { }; 55 56 if (status_reg & SYS_STATUS_RXFCG) 57 { 58 /* Clear good RX frame event and TX frame sent in the DW1000 status register. */ 59 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); 60 61 /* A frame has been received, read it into the local buffer. */ 62 frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; 63 if (frame_len <= RX_BUF_LEN) 64 { 65 dwt_readrxdata(rx_buffer, frame_len, 0); 66 } 67 68 if (memcmp(rx_buffer, Tag_Statistics_response, ALL_MSG_COMMON_LEN) == 0) 69 { 70 Semaphore[Tag_Statistics_response[TAG_INX]] = 1; 71 } 72 } 73 else{ 74 /* Clear RX error events in the DW1000 status register. */ 75 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); 76 } 77 } 78 } 79 //pick one tag ,send Semaphore message 80 //release to specific tag(TAG ID) 81 //master tag send release signal,and the specific tag send comfirm message 82 if(Waiting_TAG_Release_Semaphore == 0 && sum(Semaphore[]) != 0) 83 { 84 for(TAG_INDEX = 0; TAG_INDEX <MAX_TAG; TAG_INDEX++) 85 { 86 if(Semaphore[TAG_INDEX] == 1) 87 { 88 Master_Release_Semaphore[ALL_MSG_SN_IDX] = frame_seq_nb; 89 Master_Release_Semaphore[ALL_MSG_TAG_IDX] = TAG_INDEX; 90 dwt_writetxdata(sizeof(Master_Release_Semaphore), Master_Release_Semaphore, 0); 91 dwt_writetxfctrl(sizeof(Master_Release_Semaphore), 0); 92 dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); 93 94 while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) 95 { }; 96 97 if (status_reg & SYS_STATUS_RXFCG) 98 { 99 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS); 100 101 frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; 102 if (frame_len <= RX_BUF_LEN) 103 { 104 dwt_readrxdata(rx_buffer, frame_len, 0); 105 } 106 107 if (memcmp(rx_buffer, Master_Release_Semaphore_comfirm, ALL_MSG_COMMON_LEN) == 0) 108 { 109 //if the tag recive a semaphore, wait release remaphore 110 Waiting_TAG_Relaease_Semphore = 1; 111 } 112 } 113 else//the tag may leave net,clear semaphore 114 { 115 Semaphore[TAG_INDEX] = 0 ; 116 Waiting_TAG_Relaease_Semphore = 0; 117 /* Clear RX error events in the DW1000 status register. */ 118 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); 119 } 120 break;//only release one semphore once 121 } 122 } 123 } 124 //Master tag waitting for specific tag send Semaphore Release 125 if( Waiting_TAG_Relaease_Semphore == 1) 126 { 127 dwt_setrxtimeout(0); 128 dwt_rxenable(0); 129 130 while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) 131 { }; 132 133 if (status_reg & SYS_STATUS_RXFCG) 134 { 135 static uint32 frame_len; 136 137 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); 138 frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023; 139 if (frame_len <= RX_BUFFER_LEN) 140 { 141 dwt_readrxdata(rx_buffer, frame_len, 0); 142 } 143 if (memcmp(rx_buffer, Semaphore_Release, ALL_MSG_COMMON_LEN) == 0) 144 { 145 Semaphore[Semaphore_Release[ALL_MSG_TAG_IDX]] = 0 ; 146 Waiting_TAG_Relaease_Semphore = 0; 147 } 148 } 149 else{ 150 /* Clear RX error events in the DW1000 status register. */ 151 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); 152 } 153 } 154 //if all tag have serviced by master tag 155 //master tag can measure the distance 156 if(sum(Semaphore[]) == 0) 157 { 158 Semaphore_Enable = 1 ; 159 } 160 } 161 else //specific tag 162 { 163 dwt_setrxtimeout(0); 164 dwt_rxenable(0); 165 166 /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */ 167 while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) 168 { }; 169 170 if (status_reg & SYS_STATUS_RXFCG) 171 { 172 static uint32 frame_len; 173 /* Clear good RX frame event in the DW1000 status register. */ 174 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); 175 176 /* A frame has been received, read it into the local buffer. */ 177 frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023; 178 if (frame_len <= RX_BUFFER_LEN) 179 { 180 dwt_readrxdata(rx_buffer, frame_len, 0); 181 } 182 183 if (memcmp(rx_buffer, Tag_Statistics, ALL_MSG_COMMON_LEN) == 0) 184 { 185 Tag_Statistics_response[ALL_MSG_SN_IDX] = frame_seq_nb; 186 Tag_Statistics_response[ALL_MSG_TAG_IDX] = TAG_ID; 187 dwt_writetxdata(sizeof(Tag_Statistics_response), Tag_Statistics_response, 0); 188 dwt_writetxfctrl(sizeof(Tag_Statistics_response), 0); 189 190 dwt_starttx(DWT_START_TX_IMMEDIATE ); 191 while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)) 192 { }; 193 } 194 195 if (memcmp(rx_buffer, Master_Release_Semaphore, ALL_MSG_COMMON_LEN) == 0) 196 { 197 Master_Release_Semaphore_comfirm[ALL_MSG_SN_IDX] = frame_seq_nb; 198 Master_Release_Semaphore_comfirm[ALL_MSG_TAG_IDX] = TAG_ID; 199 dwt_writetxdata(sizeof(Master_Release_Semaphore_comfirm), Master_Release_Semaphore_comfirm, 0); 200 dwt_writetxfctrl(sizeof(Master_Release_Semaphore_comfirm), 0); 201 202 dwt_starttx(DWT_START_TX_IMMEDIATE ); 203 while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)) 204 { }; 205 } 206 207 } 208 else 209 { 210 /* Clear RX error events in the DW1000 status register. */ 211 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); 212 } 213 }
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步