DWM1000 多個標簽定位討論 --[藍點無限]

本文轉載自查看原文 2018-11-22 22:32 2171 DWM1000 Official

多標簽代碼已經基本實現，完成代碼可以在論壇上下載

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         }

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 DWM1000 定位操作流程--[藍點無限] DWM1000 三基站一標簽定位HEX DWM1000 測距原理簡單分析之 SS-TWR代碼分析1 -- [藍點無限] DWM1000 定位數據收發以及定位算法 DWM1000 定位上位機軟件以及源碼下載【DWM1000】非官方開源定位代碼bitcraze DWM1000 幀過濾代碼實現 UWB DWM1000 開源項目框架 DWM1000 自動應答代碼實現與實例 DWM1000 收發RXLED TXLED控制代碼修改