zoukankan html css js c++ java

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         }

查看全文

相关阅读:
51nod 1087 1 10 100 1000（找规律+递推+stl）
51nod 1082 与7无关的数（打表预处理）
51 nod 1080 两个数的平方和
 1015 水仙花数（水题）
51 nod 1003 阶乘后面0的数量
 51nod 1002 数塔取数问题
 51 nod 1001 数组中和等于K的数对
 51 nod 1081 子段求和
 51nod 1134 最长递增子序列 (O(nlogn)算法）
51nod 1174 区间中最大的数(RMQ)

原文地址：https://www.cnblogs.com/tuzhuke/p/10004456.html