UWB 超带宽寻迹定位模块——STM32设计部分

洛克玩泡泡堂

已于 2022-07-02 22:20:05 修改

阅读量5.7k

点赞数 8

分类专栏： UWB模块文章标签： stm32

于 2022-06-26 18:31:17 首次发布

本文链接：https://blog.csdn.net/weixin_54441404/article/details/125461224

版权

UWB模块专栏收录该内容

1 篇文章

订阅专栏

uwb简介

UWB模块通过在室内定位采用无线通讯、基站定位、惯导定位等多种技术集成形成一套室内位置定位体系，从而实现人员、物体等在室内空间中的位置监控。在场地4脚布置坐标的定位基站，安装在机器上的定位标签按照一定的频率发射脉冲，不断和4个已知位置的基站进行测距，通过一定的精确算法定出位置，从而进行标记点在房间内寻迹定位，并将数据传输给用户，更加直观的看到机器的具体位置情况。

放置基站

将烧录好程序的基站放置到指定位置，若三个基站，则放置为三角形，建议等边三角形。四个基站，则放置为矩形，建议正方形。对于3基站1标签定位，只能实现2D定位，即所有模块需要在一个平面上，推荐使用等高的移动电源供电，所有模块都垂直放置到同一水平面。模块之间不要放置障碍物，障碍物对于定位误差影响很大。基站0和基站1在x轴上，基站2在y轴上。举例如下图所示，基站0放置的坐标是(0,0)，基站1放置在x轴上，基站2放置在 (x,y) ，可以在(5,8)处，5是中点，8是自定义值。

代码介绍

标签基站的选择

跟踪开关

BPhero-UWB 代码除了可用于测距定位，还可以用于简易跟踪小车。通过修改如下代码可用于跟踪小车。

基站0与基站1的距离

定义基站 0 与基站 1 之间的距离，并将距离单位换算成米。该距离建议尽量远（建议 0.3m以上），两个模块越远算出的角度越精确。

余弦定理计算角度

通过余弦定理计算出弧度（cos），再转换为角度（angle），最后将角度信息显示在液晶上，便于调试。

设置启动距离与角度

if(dis1 >1) 为设置小车启动距离，当小车与被跟踪模块大于 1时距离时启动，小于这个距离时，小车停止不动。

if(angle >110，else if angle <75) 为设置小车的左、右转向。当角度小于 75 度时，右转；但角度大与 110度时，左转。

收发信息

系统将角度与距离信息放置在 rx_buffer[10] 中，MASTER TAG 通过射频将该信息发送给基站 0 。基站 0的串口 TX 与小车主控 MCU 的 RX 相连，基站 0 通过串口将发送数据给主控MCU，主控 MCU负责控制小车前后左右行驶。

计算角度发送到上位机的总函数

static void compute_angle_send_to_anthor0(int distance1, int distance2,int distance3)
{
static int framenum = 0 ;

#if 0 //compute angle for smartcar
float dis3_constans = DISTANCE3;
float cos = 0;
float angle = 0 ;
float dis1 = (float)distance1/1000; //m
float dis2 = (float)distance2/1000; //m

if(dis1 + dis3_constans < dis2 || dis2+dis3_constans < dis1)
{
}
cos = (dis1*dis1 + dis3_constans* dis3_constans - dis2*dis2)/(2*dis1*dis3_constans);
angle = acos(cos)*180/3.1415926;
printf("cos = %f, arccos = %f\r\n",cos,angle);
sprintf(dist_str, "angle: %3.2f m", angle);
OLED_ShowString(0, 6," ");
OLED_ShowString(0, 6,dist_str);

if(dis1 > 1)
{
if(angle > 110)
{
printf("turn right\r\n");
angle_msg[10] = 'R';
}
else if(angle < 75)
{
printf("turn left\r\n");
angle_msg[10] = 'L';
}
else
{
printf("forward\r\n");
angle_msg[10] = 'F';
}
}
else
{
printf("stay here\r\n");
angle_msg[10] = 'S';
}
angle_msg[LOCATION_FLAG_IDX] = 0;

#else
//location
{
uint8 len = 0;
angle_msg[LOCATION_FLAG_IDX] = 1;

angle_msg[LOCATION_INFO_START_IDX + (len++)] = 'm';
angle_msg[LOCATION_INFO_START_IDX + (len++)] = 'r';

angle_msg[LOCATION_INFO_START_IDX + (len++)] = 0x02;
angle_msg[LOCATION_INFO_START_IDX + (len++)] = TAG_ID;//TAG ID

angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)(framenum&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((framenum>>8)&0xFF);

angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10)&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10 >>8)&0xFF);

angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance2/10)&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance2/10 >>8)&0xFF);

angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance3/10)&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance3/10 >>8)&0xFF);

if(ANCHOR_MAX_NUM > 3)
{
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((Anthordistance[3]/10)&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((Anthordistance[3]/10 >>8)&0xFF);
}
else
{
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10)&0xFF);
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distance1/10 >>8)&0xFF);
}

angle_msg[LOCATION_INFO_START_IDX + (len++)] = '\n';
angle_msg[LOCATION_INFO_START_IDX + (len++)] = '\r';

angle_msg[LOCATION_INFO_LEN_IDX] = len;
//MAX LEN
if(LOCATION_INFO_START_IDX + len -2 >ANGLE_MSG_MAX_LEN)
{
while(1);
}
//USART_puts((char*)angle_msg,len);
}
#endif
//only anthor0 recive angle message
angle_msg[ALL_MSG_SN_IDX] = framenum;
angle_msg[ALL_MSG_TAG_IDX] = TAG_ID;

dwt_writetxdata(sizeof(angle_msg), angle_msg, 0);
dwt_writetxfctrl(sizeof(angle_msg), 0);

/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
* set by dwt_setrxaftertxdelay() has elapsed. */
dwt_starttx(DWT_START_TX_IMMEDIATE );
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };

framenum++;
}

信息发送函数

void Tag_Measure_Dis(void)
{
uint8 dest_anthor = 0,frame_len = 0;
float final_distance = 0;
for(dest_anthor = 0 ; dest_anthor<ANCHOR_MAX_NUM; dest_anthor++)
{
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
tx_poll_msg[ALL_MSG_TAG_IDX] = TAG_ID;//»ùÕ¾ÊÕµ½±êÇ©µÄÐÅÏ¢£¬ÀïÃæÓÐTAG_ID,ÔÚ»ùÕ¾»Ø¸´±êÇ©µÄÊ±ºò£¬Ò²ÐèÒªÖ¸¶¨TAG_ID,Ö»ÓÐTAG_IDÒ»ÖÂ²Å×ö´¦Àí

dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
dwt_writetxfctrl(sizeof(tx_poll_msg), 0);

/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
* set by dwt_setrxaftertxdelay() has elapsed. */
dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED);

//GPIO_SetBits(GPIOA,GPIO_Pin_2);
//TODO
dwt_rxenable(0);//Õâ¸öºó¼ÓµÄ£¬Ä¬ÈÏtxºóÓ¦¸Ã×Ô¶¯ÇÐ»»rx£¬µ«ÊÇÄ¿Ç°debug ·¢ÏÖ²¢Ã»ÓÐ×Ô¶¯´ò¿ª£¬ÕâÀïÇ¿ÖÆ´ò¿ªrx

/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };
GPIO_SetBits(GPIOA,GPIO_Pin_1);

if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}

if(rx_buffer[ALL_MSG_TAG_IDX] != TAG_ID)//¼ì²âTAG_ID
continue;
rx_buffer[ALL_MSG_TAG_IDX] = 0;

/* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;

if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 final_tx_time;

/* Retrieve poll transmission and response reception timestamp. */
poll_tx_ts = get_tx_timestamp_u64();
resp_rx_ts = get_rx_timestamp_u64();

/* Compute final message transmission time. See NOTE 9 below. */
final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(final_tx_time);

/* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;

/* Write all timestamps in the final message. See NOTE 10 below. */
final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);

/* Write and send final message. See NOTE 7 below. */
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
tx_final_msg[ALL_MSG_TAG_IDX] = TAG_ID;
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
dwt_writetxfctrl(sizeof(tx_final_msg), 0);

dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED );

while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };

/* Increment frame sequence number after transmission of the poll message (modulo 256). */
if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good/fail RX frame event in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}

if(rx_buffer[ALL_MSG_TAG_IDX] != TAG_ID)
continue;
rx_buffer[ALL_MSG_TAG_IDX] = 0;

/*As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;

if (memcmp(rx_buffer, distance_msg, ALL_MSG_COMMON_LEN) == 0)
{
// final_distance = rx_buffer[10] + (float)rx_buffer[11]/100;
Anthordistance[rx_buffer[12]] +=(rx_buffer[10]*1000 + rx_buffer[11]*10);
Anthordistance_count[rx_buffer[12]] ++;
{
int Anchor_Index = 0;
while(Anchor_Index < ANCHOR_MAX_NUM)
{
if(Anthordistance_count[Anchor_Index] >=ANCHOR_REFRESH_COUNT )
{
distance_mange();
Anchor_Index = 0;
//clear all
while(Anchor_Index < ANCHOR_MAX_NUM)
{
Anthordistance_count[Anchor_Index] = 0;
Anthordistance[Anchor_Index] = 0;
Anchor_Index++;
}
break;
}
Anchor_Index++;
}
}
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
}
else
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
/* Execute a delay between ranging exchanges. */
// deca_sleep(RNG_DELAY_MS);
frame_seq_nb++;
}

主函数

int main(void)
{
uint8 anthor_index = 0;
uint8 tag_index = 0;

uint8 Semaphore_Enable = 0 ;
uint8 Waiting_TAG_Release_Semaphore = 0;
int8 frame_len = 0;

/* Start with board specific hardware init. */
peripherals_init();
printf("hello dwm1000!\r\n");
// dwt_dumpregisters();
/* Reset and initialise DW1000.
* For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum
* performance. */
reset_DW1000(); /* Target specific drive of RSTn line into DW1000 low for a period. */

spi_set_rate_low();
if(dwt_initialise(DWT_LOADUCODE) == -1)
{
printf("dwm1000 init fail!\r\n");
OLED_ShowString(0,0,"INIT FAIL");
while (1)
{
STM_EVAL_LEDOn(LED1);
deca_sleep(100);
STM_EVAL_LEDOff(LED1);
deca_sleep(100);
}
}
spi_set_rate_high();

/* Configure DW1000. See NOTE 6 below. */
dwt_configure(&config);
dwt_setleds(1);
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
OLED_ShowString(0,0,"INIT PASS");

printf("init pass!\r\n");

AnchorList[0].x =0.12;
AnchorList[0].y =0.34;
AnchorList[0].z =0;

AnchorList[1].x =0.25;
AnchorList[1].y =0;
AnchorList[1].z =0;

AnchorList[2].x =0;
AnchorList[2].y =0;
AnchorList[2].z =0;
int rx_ant_delay =32880;
int index = 0 ;
#ifdef ANTHOR
Anchor_Array_Init();
/* Loop forever initiating ranging exchanges. */
       OLED_ShowString(0,0," 51UWB Node");
       sprintf(dist_str, " ANTHOR:%02X", ANCHOR_IND);
OLED_ShowString(0,2,dist_str);
       OLED_ShowString(0,6," www.51uwb.cn");
while (1)
{
/* Clear reception timeout to start next ranging process. */
dwt_setrxtimeout(0);
/* Activate reception immediately. */
dwt_rxenable(0);

/* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };

if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is a poll sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */

if(rx_buffer[ALL_MSG_SN_IDX]%ANCHOR_MAX_NUM != ANCHOR_IND)
continue;

anthor_index = rx_buffer[ALL_MSG_SN_IDX]%ANCHOR_MAX_NUM;
tag_index = rx_buffer[ALL_MSG_TAG_IDX];

rx_buffer[ALL_MSG_SN_IDX] = 0;
rx_buffer[ALL_MSG_TAG_IDX] = 0;

if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0)
{
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();

/* Set expected delay and timeout for final message reception. */
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);

/* Write and send the response message. See NOTE 9 below.*/
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
tx_resp_msg[ALL_MSG_TAG_IDX] = tag_index;
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);

while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };

if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}

rx_buffer[ALL_MSG_SN_IDX] = 0;
if(tag_index != rx_buffer[ALL_MSG_TAG_IDX])
continue;
rx_buffer[ALL_MSG_TAG_IDX] = 0;
if (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
double Ra, Rb, Da, Db;
int64 tof_dtu;

/* Retrieve response transmission and final reception timestamps. */
resp_tx_ts = get_tx_timestamp_u64();
final_rx_ts = get_rx_timestamp_u64();

/* Get timestamps embedded in the final message. */
final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
poll_rx_ts_32 = (uint32)poll_rx_ts;
resp_tx_ts_32 = (uint32)resp_tx_ts;
final_rx_ts_32 = (uint32)final_rx_ts;
Ra = (double)(resp_rx_ts - poll_tx_ts);
Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);
Da = (double)(final_tx_ts - resp_rx_ts);
Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);
tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));

tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
distance = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);//¾àÀë¼õÈ¥½ÃÕýÏµÊý
//½«¼ÆËã½á¹û·¢ËÍ¸øTAG
int temp = (int)(distance*100);
distance_msg[10] = temp/100;
// a=x; //×Ô¶¯ÀàÐÍ×ª»»£¬È¡ÕûÊý²¿·Ö
distance_msg[11] = temp%100; //³Ë100ºó¶Ô100È¡Óà£¬µÃµ½2Î»Ð¡ÊýµãºóÊý×Ö
distance_msg[12] = anthor_index;

distance_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
distance_msg[ALL_MSG_TAG_IDX] = tag_index;
dwt_writetxdata(sizeof(distance_msg), distance_msg, 0);
dwt_writetxfctrl(sizeof(distance_msg), 0);

/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
* set by dwt_setrxaftertxdelay() has elapsed. */
dwt_starttx(DWT_START_TX_IMMEDIATE );
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}

else if (memcmp(rx_buffer, angle_msg, ALL_MSG_COMMON_LEN) == 0 && ANCHOR_IND == 0)
{
if(rx_buffer[LOCATION_FLAG_IDX] == 1)//location infomartion
{
rx_buffer[ALL_MSG_TAG_IDX] = tag_index;
USART_puts(&rx_buffer[LOCATION_INFO_START_IDX],rx_buffer[LOCATION_INFO_LEN_IDX]);
}
else //follow car
{
putchar(rx_buffer[10]);
}
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
#endif

#ifdef TAG
/* Set expected response's delay and timeout. See NOTE 4 and 5 below.
* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
if(TAG_ID == MASTER_TAG)
{
               OLED_ShowString(0,0," 51UWB Node");
               OLED_ShowString(0,6," www.51uwb.cn");
OLED_ShowString(0,2," MASTER TAG ");
}
else
{
               OLED_ShowString(0,0," 51UWB Node");
               OLED_ShowString(0,6," www.51uwb.cn");
OLED_ShowString(0,0," SLAVE TAG ");
}

if(TAG_ID == MASTER_TAG)
{
Semaphore_Enable = 1 ;
Semaphore_Init();
Waiting_TAG_Release_Semaphore = 0;
}
else
{
Semaphore_Enable = 0 ;
}
//Master TAG0
while(1)
{
if(Semaphore_Enable == 1)
{
GPIO_ResetBits(GPIOA,GPIO_Pin_1);
GPIO_ResetBits(GPIOA,GPIO_Pin_2);

//send message to anthor,TAG<->ANTHOR
Tag_Measure_Dis();//measuer distance between tag and all anthor
Semaphore_Enable = 0 ;

if(TAG_ID != MASTER_TAG)
{
//send release semaphore to master tag
Semaphore_Release[ALL_MSG_SN_IDX] = frame_seq_nb;
Semaphore_Release[ALL_MSG_TAG_IDX] = TAG_ID;
dwt_writetxdata(sizeof(Semaphore_Release), Semaphore_Release, 0);
dwt_writetxfctrl(sizeof(Semaphore_Release), 0);

dwt_starttx(DWT_START_TX_IMMEDIATE );
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
GPIO_SetBits(GPIOA,GPIO_Pin_2);
}
}

if(TAG_ID == MASTER_TAG)//master tag
{
//statistics tag
if(Sum_Tag_Semaphore_request() == 0)
{
for(tag_index = SLAVE_TAG_START_INDEX; tag_index <MAX_SLAVE_TAG; tag_index++)
{
Tag_Statistics[ALL_MSG_SN_IDX] = 0;
Tag_Statistics[ALL_MSG_TAG_IDX] = tag_index;
dwt_writetxdata(sizeof(Tag_Statistics), Tag_Statistics, 0);
dwt_writetxfctrl(sizeof(Tag_Statistics), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);

while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };

if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
rx_buffer[ALL_MSG_SN_IDX] = 0;

if(rx_buffer[ALL_MSG_TAG_IDX] == tag_index)
{
uint8 temp = rx_buffer[ALL_MSG_TAG_IDX] ;
rx_buffer[ALL_MSG_TAG_IDX] =0;
if (memcmp(rx_buffer, Tag_Statistics_response, ALL_MSG_COMMON_LEN) == 0)
{
Semaphore[temp] = 1;
GPIO_SetBits(GPIOA,GPIO_Pin_2);
}
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
//GPIO_SetBits(GPIOA,GPIO_Pin_1);
}
}
//print all the tags in network
for(tag_index = SLAVE_TAG_START_INDEX; tag_index <MAX_SLAVE_TAG; tag_index++)
{
if(Semaphore[tag_index] == 1)
{
// printf("Tag%d In NetWork!\r\n",tag_index);
}
}
}
//pick one tag ,send Semaphore message
//release to specific tag(TAG ID)
//master tag send release signal,and the specific tag send comfirm message
if(Waiting_TAG_Release_Semaphore == 0 && Sum_Tag_Semaphore_request() != 0)
{
Semaphore[0] = 0;//slave tag must not use tag_id = 0x00!!
for(tag_index = SLAVE_TAG_START_INDEX; tag_index <MAX_SLAVE_TAG; tag_index++)
{
if(Semaphore[tag_index] == 1)
{
// printf("Release Semaphore to Tag%d!\r\n",tag_index);
// dwt_setrxtimeout(0);

dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
Master_Release_Semaphore[ALL_MSG_SN_IDX] = 0;
Master_Release_Semaphore[ALL_MSG_TAG_IDX] = tag_index;
dwt_writetxdata(sizeof(Master_Release_Semaphore), Master_Release_Semaphore, 0);
dwt_writetxfctrl(sizeof(Master_Release_Semaphore), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);

while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };

if (status_reg & SYS_STATUS_RXFCG)
{
GPIO_SetBits(GPIOA,GPIO_Pin_1);
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
rx_buffer[ALL_MSG_SN_IDX] = 0;

if(rx_buffer[ALL_MSG_TAG_IDX] == tag_index)
{
rx_buffer[ALL_MSG_TAG_IDX] = 0;
GPIO_SetBits(GPIOA,GPIO_Pin_3);
// USART_puts(rx_buffer,frame_len);
if (memcmp(rx_buffer, Master_Release_Semaphore_comfirm, ALL_MSG_COMMON_LEN) == 0)
{
//if the tag recive a semaphore, wait release remaphore
Waiting_TAG_Release_Semaphore ++;
break;//only release one semphore once
}
}
}
else//the tag may leave net,clear semaphore
{
Semaphore[tag_index] = 0 ;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
}
}

if(Waiting_TAG_Release_Semaphore == 0 )
{
// GPIO_SetBits(GPIOA,GPIO_Pin_2);GPIO_SetBits(GPIOA,GPIO_Pin_1);
}
//Master tag waitting for specific tag Semaphore Release message
if( Waiting_TAG_Release_Semaphore >0)
{
// printf("Waiting for Release Semaphore!\r\n");
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*5);//about 10ms,need adjust!!
dwt_rxenable(0);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };

if (status_reg & SYS_STATUS_RXFCG)
{
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}

rx_buffer[ALL_MSG_SN_IDX] = 0;
uint8 temp=rx_buffer[ALL_MSG_TAG_IDX] ;
rx_buffer[ALL_MSG_TAG_IDX] = 0;
if (memcmp(rx_buffer, Semaphore_Release, ALL_MSG_COMMON_LEN) == 0)
{
if(Semaphore[temp] == 1)
{
Semaphore[temp] = 0 ;
if(Waiting_TAG_Release_Semaphore > 0 )
{
Waiting_TAG_Release_Semaphore --;
}
}
}
}
else
{
//maybe the tag leave network
if(Waiting_TAG_Release_Semaphore > 0)
{
Waiting_TAG_Release_Semaphore--;
Semaphore[tag_index] = 0 ;
}
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
//¿ÉÄÜ´æÔÚµÄÎÊÌâ£¬TAGÊÕµ½Semaphore Ã»ÓÐÊÍ·Å¾ÍÀë¿ªÍøÂç£¬µ¼ÖÂMaster TAGÎÞ·¨ÊÕ»ØSemaphore£¬Õâ¸öÐèÒª¶¨Ê±Æ÷ÊµÏÖ£¬¶¨Ê±Ò»¶ÎÊ±¼ä£¬ÈôÒÀÈ»Ã»ÓÐÊÕµ½TAG ÊÍ·ÅSemaphore£¬ÐèÒªÇ¿ÖÆÈ¡Ïû
//if all tag have serviced by master tag
//master tag can measure the distance
if(Sum_Tag_Semaphore_request() == 0)
{
Semaphore_Enable = 1 ;
Waiting_TAG_Release_Semaphore= 0;
}
}
else //slave tags
{
//SLAVE TAG Æô¶¯Ä¬ÈÏµÈ´ýMASTER TAG·¢ËÍÍ³¼ÆÐÅÏ¢ÒÔ¼°ÊÍ·ÅÐÅºÅÁ¿
dwt_setrxtimeout(0);
dwt_rxenable(0);

/* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
{ };

if (status_reg & SYS_STATUS_RXFCG)
{
/* Clear good RX frame event in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG|SYS_STATUS_TXFRS);//clear rx & tx flag at the same time

/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN)
{
dwt_readrxdata(rx_buffer, frame_len, 0);
}
rx_buffer[ALL_MSG_SN_IDX] = 0;
if(rx_buffer[ALL_MSG_TAG_IDX] == TAG_ID)
{
rx_buffer[ALL_MSG_TAG_IDX] = 0;
if (memcmp(rx_buffer, Tag_Statistics, ALL_MSG_COMMON_LEN) == 0)
{
//GPIO_SetBits(GPIOA,GPIO_Pin_3);
Tag_Statistics_response[ALL_MSG_SN_IDX] = frame_seq_nb;
Tag_Statistics_response[ALL_MSG_TAG_IDX] = TAG_ID;
dwt_writetxdata(sizeof(Tag_Statistics_response), Tag_Statistics_response, 0);
dwt_writetxfctrl(sizeof(Tag_Statistics_response), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE );

GPIO_SetBits(GPIOA,GPIO_Pin_2);
}

if (memcmp(rx_buffer, Master_Release_Semaphore, ALL_MSG_COMMON_LEN) == 0)
{
Master_Release_Semaphore_comfirm[ALL_MSG_SN_IDX] = frame_seq_nb;
Master_Release_Semaphore_comfirm[ALL_MSG_TAG_IDX] = TAG_ID;
dwt_writetxdata(sizeof(Master_Release_Semaphore_comfirm), Master_Release_Semaphore_comfirm, 0);
dwt_writetxfctrl(sizeof(Master_Release_Semaphore_comfirm), 0);

dwt_starttx(DWT_START_TX_IMMEDIATE);
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };

Semaphore_Enable = 1;
GPIO_SetBits(GPIOA,GPIO_Pin_1);
}
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
}
#endif