v2.8

yincheng.zhong

2023-05-19 cf92a2fedf94d889b1a39e8557221149ea554a3c

 Src/application/dw_app.c

@@ -29,7 +29,7 @@
#include <stdio.h>
#include "beep.h"
#include "modbus.h"

#include "CRC.h"
/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100
@@ -69,7 +69,7 @@
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4

#define STARTPOLL  REGPOLL//#define SWITCHBASE_DIST
#define STARTPOLL  DISCPOLL//#define SWITCHBASE_DIST
#define SWITCHBASE_ZHUANDIAN
#define SWITCHBASE_DIST
enum enumtagstate
@@ -117,9 +117,10 @@
uint32_t g_UWB_com_interval = 0; 
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器
uint16_t current_slotpos = 0;



uint8_t rec_anc_sync_sec,tagpoll_sync_sec,current_sync_sec,flag_tagnewpos;
void GetNearMsg(void);
static uint64_t get_tx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
@@ -470,10 +471,11 @@
u8 nearbase_num=0,last_nearbase_num,next_nearbase_num,last_slotnum, para_update,para_len,stationary_flag=0;
u32 rec_tagpos_binary;
int16_t offset=4700,temptimer;
u8 motor_state,rec_remotepara_state,rec_remotepara[80];
static u8 motor_state,rec_remotepara_state,rec_remotepara[80];


#define SINGLEPOLL_BASENUM 5
#define ANCHORTYPE_NUM   3
uint16_t singlepoll_baseid[20]={0x8166,0x9804,0x9689,0x9686,0x8279,0x9815,0x9803,0x9814,0x9687,0x9774,0x8278,0x9743,0x9688,0x9771,0x8277};
int32_t singlepoll_basedist[20];

@@ -496,7 +498,7 @@
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      
   frame_seq_nb++;
   for(uint8_t singlepoll_i=0;singlepoll_i<10;singlepoll_i++)
   for(uint8_t singlepoll_i=0;singlepoll_i<ANCHORTYPE_NUM;singlepoll_i++)
   {
       

@@ -515,7 +517,7 @@
 //   memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*6],&intheight,2);
   send_buffer[MESSAGE_TYPE_IDX] = DISCOVERPOLL;   
//   singlepoll_basedist[singlepoll_i]= 0x1ffff;
   send_buffer[NEARP_TAGFREQ_INDEX] = tag_frequency;	
   //send_buffer[NEARP_TAGFREQ_INDEX] = tag_frequency;	
   send_buffer[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;   
    send_buffer[ANCHOR_ID_IDX] = singlepoll_i;
   memcpy(&send_buffer[TAG_ID_IDX],&dev_id,2);   
@@ -613,19 +615,13 @@
         trygetnearmsg_times = 0;
            tag_state = GETNEARMSG;
            mainbase_id = mindist_ancid;   
            GetNearMsg();
    }
   
   if(getsync_flag==0)
   {
//   tagslotpos--;
//         if(tagslotpos==0||tagslotpos>max_slotpos)
//      {		
//         tagslotpos=max_slotpos;	
//      }
//      tyncpoll_time=(tagslotpos-1)*slottime;
   }
//   mainbase_id=minid;
//   mainbase_id = 0x4;

}
int16_t target_offsettime = 850;
//#define TAGET_OFFSETTIME 940
@@ -640,7 +636,7 @@
u16 delaycount;
int16_t poll_offsettime=-6900,clockoffset;
extern u16 slotpos_intoatl, slotpos;
void NextPollDelay(u32 anchor_time)
void NextPollDelay(u32 anchor_time)  //根据当前基站的时间，计算下一次需要工作的时间的延迟。然后设定定时器到这个时间。
{
      //tagslotpos = 0;
      error_time = anchor_time-expect_anctime;
@@ -652,9 +648,9 @@
        temptime = (slotpos_intoatl*SLOTTIME_MS+100)*100;
        if(anchor_time<temptime-5000)
        { 
               current_slotnum = pd_i;
            current_slotnum = pd_i;
            delaytime = temptime-anchor_time+poll_offsettime+clockoffset;  
                  expect_anctime = temptime;
            expect_anctime = temptime;
            return ;
        }
    }
@@ -672,18 +668,18 @@
    NextPollDelay(anchor_time);
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    lpcount = current_count+delaytime/LPTIMER_LSB+(10-nearbase_num)*8;
      poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
      if(poll_startcount>=32768)
      {
      poll_startcount -=32768;
            poll_startcount -=32768;
      }
      if(lpcount>=32768)
      {
      lpcount -=32768;
            lpcount -=32768;
      }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
      last_lpcount = lpcount;
   }
}

void NextSlotDelayMs(int16_t delayms)
{
@@ -695,11 +691,11 @@
   }else{
      lpcount = last_lpcount+31457/tag_frequency+32.767*delayms;//(float)(200/tag_frequency*48+delayms*10)*3.3 ;
   }
            if(lpcount>=32768)
            {
            lpcount -=32768;
            }		
            poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(lpcount>=32768)
    {
    lpcount -=32768;
    }		
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
      if(poll_startcount>=32768)
      {
      poll_startcount -=32768;
@@ -707,8 +703,9 @@
         __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
            last_lpcount = lpcount;
}

u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;
uint16_t rec_tagperiod;
u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;
int8_t notenoughdist_count=0;
void GetNearMsg(void)
{
   u32 start_poll,frame_len;
@@ -719,15 +716,16 @@
        nearbase_distlist[nearmsg_i] = 0x1ffff;
    }
//mainbase_id = 0x9818;
    notenoughdist_count = -2;
   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
   memcpy(&tx_near_msg[TAG_ID_IDX],&dev_id,2);
   tx_near_msg[MESSAGE_TYPE_IDX] = POS_MSG;
	
   memcpy(&tx_near_msg[TAGMSG_CURRENTPOS_IDX],&current_slotpos,2);
    
   dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
  dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);	
   dwt_writetxdata(12, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(12, 0);//设置超宽带发送数据长度
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);	
   dwt_writetxdata(14, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(14, 0);//设置超宽带发送数据长度
      current_count=HAL_LPTIM_ReadCounter(&hlptim1);
      while(current_count<poll_startcount||current_count>poll_startcount+16384)
      {
@@ -754,57 +752,65 @@
      dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
      if (rx_buffer[MESSAGE_TYPE_IDX] == POS_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
      {
           
            memcpy(&mainbase_id,&rx_buffer[ANCHOR_ID_IDX],2);
         nearbase_num=rx_buffer[NEARBASENUM_INDEX];
         memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
      //   tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
         //tagslotpos=rx_buffer[TAGSLOTPOS];
			
         memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
         memcpy(nearbaseid_list2,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
         //slottime=ceil((nearbase_num+2)*0.3)+1;
         //tyncpoll_time=tagslotpos*slottime;
         //tyncpoll_time=(g_com_map[DEV_ID]%max_slotpos)*slottime;	
            memcpy(&current_slotpos,&rx_buffer[ANCMSG_TAGSLOTPOS_IDX],2);
            memcpy(&rec_tagperiod,&rx_buffer[ANCMSG_TAGPERIODMS_IDX],2);
            tagpoll_sync_sec = current_slotpos/200;
            tagslotpos = current_slotpos%200;
            max_slotpos = rec_tagperiod*20;
            tag_frequency = 10/rec_tagperiod;
            bigslot_num = TOTAL_SLOTNUM/tag_frequency;
         tag_state=NEARPOLL;
      }
   }else{
//         tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
   }
}
void SendHuiZongData(void)
{
                usart_send[2] = 0x0c;//正常模式 
               usart_send[3] = 15+8*(nearbase_num+1);//数据段长度
               memcpy(&usart_send[4],&dev_id,2);
                    usart_send[6] = frame_seq_nb;
                    usart_send[7] = frame_seq_nb>>8;
               usart_send[8] = bat_percent;
                    usart_send[9] = tx_near_msg[BUTTON_IDX];
//void SendHuiZongData(void)
//{
//                usart_send[2] = 0x0c;//正常模式 
//               usart_send[3] = 15+8*(nearbase_num+1);//数据段长度
//               memcpy(&usart_send[4],&dev_id,2);
//                    usart_send[6] = frame_seq_nb;
//                    usart_send[7] = frame_seq_nb>>8;
//               usart_send[8] = bat_percent;
//                    usart_send[9] = tx_near_msg[BUTTON_IDX];

                    memcpy(&usart_send[10],&intheight,2);
               usart_send[12] = tag_frequency;
                    usart_send[13] = tagslotpos;
                    usart_send[14] = 0;
                    usart_send[15] = 0;
               usart_send[16] = nearbase_num+1;
                    memcpy(&usart_send[17],&mainbase_id,2);
                    memcpy(&usart_send[19],&nearbaseid_list,2*nearbase_num);
                    memcpy(&usart_send[19+nearbase_num*2],&nearbase_distlist,4*nearbase_num+4);
                memcpy(&usart_send[19+nearbase_num*6+4],&rec_anc_signalpower,2*nearbase_num+2);
                    
               checksum = Checksum_u16(&usart_send[2],23+8*nearbase_num);
               memcpy(&usart_send[25+8*nearbase_num],&checksum,2);
               USART_puts(usart_send,27+8*nearbase_num);
               //HexToAsciiSendUDP(usart_send,27+8*nearbase_num);
}
//                    memcpy(&usart_send[10],&intheight,2);
//               usart_send[12] = tag_frequency;
//                    usart_send[13] = tagslotpos;
//                    usart_send[14] = 0;
//                    usart_send[15] = 0;
//               usart_send[16] = nearbase_num+1;
//                    memcpy(&usart_send[17],&mainbase_id,2);
//                    memcpy(&usart_send[19],&nearbaseid_list,2*nearbase_num);
//                    memcpy(&usart_send[19+nearbase_num*2],&nearbase_distlist,4*nearbase_num+4);
//                memcpy(&usart_send[19+nearbase_num*6+4],&rec_anc_signalpower,2*nearbase_num+2);
//                    
//               checksum = Checksum_u16(&usart_send[2],23+8*nearbase_num);
//               memcpy(&usart_send[25+8*nearbase_num],&checksum,2);
//               USART_puts(usart_send,27+8*nearbase_num);
//               //HexToAsciiSendUDP(usart_send,27+8*nearbase_num);
//}
uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;
uint8_t motor_flag;
uint16_t rec_ancpos;
uint8_t dist_noutenough_count;
uint8_t ancdist_hist[11];
uint16_t calCRC;
void NearPoll(void)
{
   
   uint32_t temp1,temp2,dw_systime,mindist_slavebaseid;
    static u8 notenoughdist_count=0;
    
   uint32_t final_tx_time;
   u32 start_poll;
   u8 i,j,getsync_flag=0,timeout,get_newdist;
   u8 i,j,getsync_flag=0,timeout,get_newdist,pos_nochange=1;
   // USART_putc(current_slotnum);
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      
@@ -821,31 +827,27 @@

   recbase_num=0;
//   motor_state=0;
    if(motor_state!=0&&motor_state!=3)
    {
        motor_flag = 1;
    }else{
        motor_flag = 0;
    }

   
   tx_near_msg[BATTARY_IDX] = bat_percent;
   tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5;
   tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
  tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
    tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
   tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
   memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
  memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
  memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);

    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);
    
    userkey_state = 0;
    if(intheight!=0)
    intheight+=g_com_map[HEIGHTOFFEST_INDEX];
    
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
   tx_near_msg[MESSAGE_TYPE_IDX] = POS_POLL;   
	
   tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;	
   tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;	
    
   tx_near_msg[NEARP_TAGFREQ_INDEX] = rec_tagperiod;	
   tx_near_msg[NEARP_TAGNEWPOSFLAG_INDEX] = flag_tagnewpos;	
   memcpy(&tx_near_msg[NEARP_TAGSLOTPOS_INDEX],&current_slotpos,2);	

   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);   
   dwt_writetxdata(29+8*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
@@ -903,11 +905,31 @@
                     dwt_rxenable(0);//打开接收
                     //HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
                     if (rx_buffer[MESSAGE_TYPE_IDX] == POS_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
                     {   u16 rec_nearbaseid,rec_nearbasepos;
                     {   u16 rec_nearbaseid,rec_nearbasepos,next_slotpos = current_slotpos;
                        poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
                        resp_rx_ts = get_rx_timestamp_u64();                              //获得RESPONSE接收时间T4   
                        recbase_num++;
                        memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
                                memcpy(&rec_ancpos,&rx_buffer[PR_TAGPOS_INDEX],2);
                                if(rec_ancpos<max_slotpos)  //接收到基站回复的时间片位置后，第一个不一样就改变，后续的不一样就找最大的时间片
                                {
                                    if(pos_nochange)
                                    {
                                        if(rec_ancpos!=next_slotpos)
                                        {   
                                            flag_tagnewpos = 1;
                                            next_slotpos = rec_ancpos;
                                            pos_nochange =0;
                                        }
                                    }else{
                                         if(rec_ancpos>next_slotpos)
                                         {
                                             flag_tagnewpos = 1;
                                             next_slotpos = rec_ancpos;
                                         }
                                    }                                 
                                }

                        if(rec_nearbaseid==mainbase_id)
                        {
                           //////////////////////////////////时间同步
@@ -933,7 +955,7 @@
                           dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
                           dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
                           result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送   
                           userkey_state = !GET_USERKEY;
                        //   userkey_state = !GET_USERKEY;

                           memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                                    if(temp_dist!=nearbase_distlist[0])
@@ -943,25 +965,23 @@
                           mainbase_dist=temp_dist;
                                    nearbase_distlist[0] = temp_dist;
                                    base_mindist = temp_dist;
                           if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
                              motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;

                                    if(frame_len==28)
                                     {
                                         memcpy(&rec_maxrangelen,&rx_buffer[MAXRANGE_DISTANCE],2);
                                         memcpy(&rec_anc_signalpower[0],&rx_buffer[ANC_SIGNALPOWER],2);
                                         if(mainbase_dist>rec_maxrangelen&&mainbase_dist>0&&rec_maxrangelen!=0)
                                         {
                                             if(mainbase_dist!=0x1ffff)
                                             if(outrange_times++>OUTRANGE_RESTARTTIMES)
                                             {
                                               // tag_state = STARTPOLL;
                                                tag_state = STARTPOLL;
                                             }
                                         }else{
                                             outrange_times = 0;
                                         }
                                     }
                                 
									
									
                                 									
                           mainbase_lost_count=0;
                           flag_finalsend=1;
                           memcpy(&mainbase_dist,&rx_buffer[DIST_IDX],4);
@@ -969,15 +989,22 @@

                           flag_getresponse=1;                        
                           //时间同步
                        tmp_time=tmp_time+450;
                                    tmp_time=tmp_time+450;
                           if(tmp_time>999)
                           {
                              tmp_time-=999;
                              sync_timer++;
                              if(sync_timer>=1000)
                                 {sync_timer=0;}
                              sync_timer++;                                       
                           }
                           ancsync_time=((sync_timer)*1000+tmp_time);	
                                    rec_anc_sync_sec = sync_timer/1000;
                                    current_sync_sec = rec_anc_sync_sec+1;
                                    if(current_sync_sec>=rec_tagperiod/10)
                                    {
                                        current_sync_sec = 0;
                                    }
                           ancsync_time=((sync_timer%1000)*1000+tmp_time);	
                                    current_slotpos = next_slotpos;
                                    tagpoll_sync_sec = current_slotpos/200;
                                    tagslotpos = current_slotpos%200;
                           SetLPTimer(ancsync_time);
                           if(result==0)
                                 {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
@@ -1009,6 +1036,7 @@
                                            mainbase_id = rec_nearbaseid;
                                            tag_state = GETNEARMSG;
                                 trygetnearmsg_times = 0;
                                            GetNearMsg();
                                        }
                                    #endif

@@ -1039,17 +1067,22 @@
               changemainbase_count++;
               if(changemainbase_count>tag_frequency*2)
               {
                              mainbase_id = mindist_slavebaseid;
                              tag_state = GETNEARMSG;
                              trygetnearmsg_times = 0;
                        mainbase_id = mindist_slavebaseid;
                        tag_state = GETNEARMSG;
                        trygetnearmsg_times = 0;
                        GetNearMsg();
               }
            }else{
               changemainbase_count = 0;
            }
         #endif
         }else if(mainbase_lost_count>5*tag_frequency)
         }else if(mainbase_lost_count>5)
         {
         //   tag_state = DISCPOLL;
            tag_state = DISCPOLL;
                current_slotpos = start_count%max_slotpos;  //如果丢失连接超过5次就随机换个时间片
                tagpoll_sync_sec = current_slotpos/200;
                tagslotpos = current_slotpos%200;
                flag_tagnewpos = 0;
         }   
         if(mainbase_lost_count!=0)
         {
@@ -1061,37 +1094,45 @@
         }   

         if(para_update)
         {
			
            pack_msgtype = rec_remotepara[0];
            pack_index = rec_remotepara[1];
            pack_length = rec_remotepara[2];
            if(pack_msgtype==2)
            {
               if( pack_index == MOTOR_ONTIME_INDEX)
               {
                       // if(motor_keeptime==0)
                            motor_keeptime = rec_remotepara[3];
               }else{
                        if(pack_index<200)
         {   
               
                calCRC = CRC_Compute(rec_remotepara,5);
                if(memcmp(&calCRC,&rec_remotepara[5],2)==0)
                {
                    pack_msgtype = rec_remotepara[0];
                    pack_index = rec_remotepara[1];
                    pack_length = rec_remotepara[2];
                    if(pack_msgtype==2)
                    {
                        if( pack_index == MOTOR_ONTIME_INDEX)
                        {
                            memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
                            //返回一个error状态
                            //SendComMap(pack_datalen,pack_index);
                            save_com_map_to_flash();
                            delay_ms(100);
                            NVIC_SystemReset();
                           // if(motor_keeptime==0)
                                motor_keeptime = rec_remotepara[3];
                        }else{
                            if(pack_index<200)
                            {
                                memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
                                //返回一个error状态
                                //SendComMap(pack_datalen,pack_index);
                                save_com_map_to_flash();
                                delay_ms(100);
                                NVIC_SystemReset();
                            }
                        }
               }
            }
                    }
                }
         }
            if(get_newdist>2)
            if(get_newdist>=2)
            {
                notenoughdist_count = 0;
            }else{
                if(notenoughdist_count++>3)
                {
                    notenoughdist_count = 0;
                    current_slotpos = start_count%max_slotpos;  //如果丢失连接超过5次就随机换个时间片
                    tagpoll_sync_sec = current_slotpos/200;
                    tagslotpos = current_slotpos%200;
                    flag_tagnewpos = 0;
             //       tag_state = DISCPOLL;
                }
            }
@@ -1100,21 +1141,27 @@
                if(nearbaseid_list[i]!=nearbaseid_list2[i])
                {
                    tag_state = GETNEARMSG;
                           trygetnearmsg_times = 0;
                           nearbaseid_list0[i]=1;
                    trygetnearmsg_times = 0;
                    nearbaseid_list0[i]=1;
                    GetNearMsg();
                }
         }
            if(pos_nochange)
            {
                flag_tagnewpos = 0;
            }
        //    SendHuiZongData();
   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
//HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET);

}

extern uint8_t module_power,imu_enable,motor_enable;
void Registor_Poll(void)
{
   static u8 regpoll_count=0;
   mainbase_lost_count = 0;
      tag_frequency = REGISTER_FREQUENCY;
      //tag_frequency = REGISTER_FREQUENCY;
      bigslot_num = TOTAL_SLOTNUM/tag_frequency;
      regpoll_count++;
      if(regpoll_count%2)
@@ -1123,14 +1170,14 @@
            tagslotpos=TOTAL_SLOTNUM;
      }
      //SetNextPollTime(tagslotpos);
      NextSlotDelayMs(-2);
      //NextSlotDelayMs(-2);
      
      dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      

      tx_near_msg[REGP_TAGSTATE_INDEX] = !GET_USERKEY<<2|imu_enable<<1|motor_enable;
      tx_near_msg[MESSAGE_TYPE_IDX] = REG_POLL;   
      tx_near_msg[REGP_FREQUENCY_INDEX] = 1000/g_com_map[COM_INTERVAL];
      tx_near_msg[REGP_FREQUENCY_INDEX] =rec_tagperiod;
      tx_near_msg[REGP_POWER_INDEX] = module_power;
      memcpy(&tx_near_msg[REGP_VERSION_INDEX],&g_com_map[VERSION],2);   
      memcpy(&tx_near_msg[REGP_IMUTHRES_INDEX],&g_com_map[IMU_THRES],2);
@@ -1165,46 +1212,34 @@
                           memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
                           current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//                           memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);      
                           tmp_time=tmp_time+450;
                           if(tmp_time>999)
                           {
                              tmp_time-=999;
                              sync_timer++;
                              if(sync_timer>=1010)
                                 {sync_timer=0;}
                           }
                           ancsync_time=((sync_timer)*1000+tmp_time);																			
                           SetLPTimer(ancsync_time);
//                           last_slotnum=current_slotnum;
//                           temptimer = sync_timer-10;
//                           if(temptimer<0)
//                           {temptimer+=1000;}
//                           current_slotnum=((float)temptimer/(SLOTTIME_MS*bigslot_num))+1;
//                           if(current_slotnum==last_slotnum-1)
//                           {flag_getresponse=1;}
//                           lastsync_timer=sync_timer;
//                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
//                           SetNextPollTime(tagslotpos);
//                           tmp_time=tmp_time+450;
//                           if(tmp_time>999)
//                           {
//                              tmp_time-=999;
//                              sync_timer++;					
//                           }
//                           ancsync_time=((sync_timer)*1000+tmp_time);																			
//                           SetLPTimer(ancsync_time);
                           mainbase_id=rec_nearbaseid;
                           nearbase_num=rx_buffer[NEARBASENUM_INDEX];
                           memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
                           memcpy(nearbaseid_list2,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
                           if(tagslotpos!=255)
                           tag_state = DISCPOLL;
//                           if(tagslotpos!=255)
//                           tag_state = DISCPOLL;
                        }
                     }
}
extern void FZNearPoll(void);
u32 id,error_times=0;

u8 ttt = 1;
extern float Height;
void Tag_App(void)//发送模式(TAG标签)
void UWBWkWp(void)
{
	
   //LED0_ON;
   SPIx_CS_GPIO->BRR = SPIx_CS;
   delay_us(700);
   SPIx_CS_GPIO->BSRR = SPIx_CS;
   id =  dwt_readdevid() ;
    
       while (DWT_DEVICE_ID != id) 
    {
      //   Dw1000_Init();
@@ -1217,6 +1252,13 @@
        }
    }
    error_times = 0;
}

void Tag_App(void)//发送模式(TAG标签)
{
   static uint8_t diccount = 0,diccount2;
   //LED0_ON;
    UWBWkWp();
   switch(tag_state)
   {
      case REGPOLL:
@@ -1225,14 +1267,26 @@
         LED_LG_OFF;
         break;
      case DISCPOLL:
         LED_LG_ON;		
         DiscPoll();
         LED_LG_ON;	
        
         //DiscPoll();
            mainbase_lost_count = 0;
            tagpoll_sync_sec = 0;
            rec_tagperiod = 10;
            mainbase_id = 0xffff;
            GetNearMsg();
            if(diccount++%10==0)
            {
                Registor_Poll();
            }        
            NextSlotDelayMs(-5);
         LED_LG_OFF;
         break;
      case GETNEARMSG:
         trygetnearmsg_times++;   
         LED_LG_ON;
            GetNearMsg();	 
     
            GetNearMsg();	 
      LED_LG_OFF;
      if(trygetnearmsg_times>5)
      {
@@ -1241,13 +1295,16 @@
         break;
      case NEARPOLL:   
         NearPoll();
       
         //GetPressAndHeight();
         //intheight = Height*100;
         break;
        case SINGLEPOLL:
          //  Poll();
            break;
   }
     userkey_state = !GET_USERKEY;
   }       
    FZNearPoll();
    dwt_forcetrxoff();
//    userkey_state = !GET_USERKEY;
   dwt_entersleep();
}