目录

uwb简介

放置基站

代码介绍

标签基站的选择

跟踪开关

基站0与基站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

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

  1. 中鸣循迹机器人_基于智能i5的寻迹机器人的设计

    基于智能 i5 的寻迹机器人的设计 江旭 贾婷 杨超 张婷婷 沈阳工学院信息与控制学院 [摘 要] 摘要:本文论述了基于单片机的智能循迹小车的控制过程.智能循迹 是基于自动引导机器人系统,用以实现小车 ...

  2. 【毕业设计】39-基于单片机的智能小车寻迹系统的设计与实现（原理图工程+仿真工程+源代码+答辩论文+答辩PPT）

    [毕业设计]39-基于单片机的智能小车寻迹系统的设计与实现(原理图工程+仿真工程+源代码+答辩论文+答辩PPT) 文章目录 [毕业设计]39-基于单片机的智能小车寻迹系统的设计与实现(原理图工程+仿真 ...

  3. c语言小车寻迹实训报告,循迹小车项目实训报告.doc

    循迹小车项目实训报告 寻迹小车的设计与制作实训报告 课程名称: C51程序设计 EDA技术 专业班级: 应电0935班 指导老师: 方跃春 谭刚林 小组成员: 黄春桥 汤政 孙巍明 阳毅 实训项目要求 ...

  4. 基于图像识别的小车智能寻迹控制系统

    目录 摘要-- I Abstract II 基于图像识别的智能寻迹控制系统设计 I Design of Intelligent tracking Control system based on Ima ...

  5. 基于STM32设计的避障寻迹小车

    一.前言 1.1 项目背景 根据美国玩具协会在一项研究中,过去几年全球玩具销售增长与GDP的世界平均水平大致相同.但全球玩具市场的内部结构已经占据了巨大的位置变化:传统玩具的市场份额正在下降,高科技电 ...

  6. 无人机寻迹要两个单片机吗_基于OpenMV的循迹无人机设计

    基于OpenMV的循迹无人机设计 发表时间:2020-08-24 基于OpenMV机器视觉模块进行目标识别算法的研究.利用图像的滤波.二值化等算法对摄像头采集的图像进行预处理;利用边缘检测和形状识别算 ...

  7. 四轮寻迹智能车的设计（逐飞IMX-RT1064）

    四轮寻迹智能车的设计(逐飞IMX-RT1064) 目录 四轮寻迹智能车的设计(逐飞IMX-RT1064) 1.前篇 (1)adc (模数/数模转换) (2)pit(定时中断) (3)pwm(波特率) ...

  8. STM32红外寻迹小车

    STM32红外寻迹小车(寄存器版) 最近学习了STM32,想通过制作一辆小车来加深对STM32的理解,在平时学习时经常用正点原子提供的源代码稍加该装就行,但是正点原子没有提供关于红外寻迹模板的相关程序 ...

  9. 单片机寻迹小车ppt_基于单片机循迹小车的设计.doc

    目录 [摘   要]  2 第一章  绪论  3 1.1智能小车的来源  3 1.2研究智能小车的目的和意义  4 1.3智能小车的现状及未来  4 第二章  方案的设计与论证  4 2.1主控系统  ...

最新文章

  1. 微型计算机原理及应用李干林,微机原理及接口技术李干林习题参考解答-20210415154329.docx-原创力文档...
  2. 肖战被抵制？Python爬虫揭秘关于肖战粉丝的人群画像
  3. linux停止jar程序,Linux 启动停止SpringBoot jar 程序部署Shell 脚本的方法
  4. 艾蔻扫地机器人怎么充电_扫地机器人锂电池的正确使用方法
  5. 图片添加对话气泡app_刘雨昕、谢可寅、虞书欣代言农夫山泉TOT气泡饮
  6. 创造性模式——原型模式
  7. 编程：请写一个类，在任何时候都可以向它查询“你已经创建了多少个对象？”...
  8. 34、linux shell，常用函数strace
  9. Matlab数据可视化编程总结
  10. 关于高德地图坐标批量手动拾取的简易办法
  11. python UDP Socket编程
  12. ARM指令集--相关指令的功能
  13. 编译原理第二版5.1答案
  14. 在线PDF保护解除器，完全免费，没有文件数量限制 - PDF在线解锁器
  15. android 应用中 assets 下文件的绝对路径
  16. 使用vue-video-player播放视频 一个页面多个视频，并且只能唯一播放
  17. java 数独游戏_java数独游戏完整版分享
  18. Android Native 崩溃日志收集
  19. 《InsideUE4》GamePlay架构学习_Level和World
  20. 【拓展】腾讯十大最受欢迎的开源项目！

热门文章

  1. 深入浅出实现Electron判断屏幕当前是否是双屏显示？
  2. Tinkoff Internship Warmup Round 2018 and Codeforces Round #475 (Div. 2)
  3. 2017年石家庄最新房贷利率
  4. android机器人 人偶,模拟机器人偶游戏-模拟机器人偶安卓版预约 _5577安卓网
  5. php 红包雨,小程序红包雨的实现示例
  6. 分享：nuxt个性化特定页面的 Meta 标签
  7. （翻译）刻意缺欠模式（ Intentional gaps）
  8. nodejs使用superagent写爬虫dns超时
  9. ubuntu截图工具ksnapshot安装
  10. 《强化学习周刊》第44期：RL-CoSeg、图强化学习、安全强化学习