去掉打印，恢复默认不测距下1HZ20ua左右稳定测距版本

chen

2025-01-02 724f1e7164d903eb14151bb352e483fa0b2ea0fc

 keil/uwb_tag_.c

@@ -12,12 +12,13 @@
#if defined(MK_SS_TWR_DW_INIT)

extern int TagRange(void);

void Tag_uwb_init(void);
void TagListUpdate(void);
/* Ranging period: 1s */
#define RANGING_PERIOD_MS (1000)
#define RANGING_PERIOD_MS (10000)

/* This is the delay from the end of the poll frame transmission to the enable of the receiver */
#define POLL_TX_TO_RESP_RX_DLY_US 750U  //yuan700
#define POLL_TX_TO_RESP_RX_DLY_US 550U  //yuan700

#define RESP_RX_TO_FINAL_TX_DLY_US 550U

@@ -25,15 +26,15 @@
#define POLL_DELAY  100U //yuan100U

/* Receive response timeout */
#define RESP_RX_TIMEOUT_US 5000U //Yuan500 10mssuccess 300jixian

#define RESP_RX_TIMEOUT_US_DEFAULT 600U //Yuan500 10mssuccess 300jixian
#define RESP_RX_TIMEOUT_US 2000000
/* Field index in frame */
#define MSG_SEQ_NUM_IDX 2
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define DELAY_DEFAULT 50000
#define DELAY_BETWEEN_TWO_FRAME_UUS 1400
#define DELAY_BETWEEN_TWO_FRAME_UUS 500  //yuan1400
#define HALF_SECOND_TIME 62400000

uint16_t CmpTagInList(uint16_t tagid);
@@ -47,6 +48,7 @@
};
uint32_t dev_id;
uint8_t group_id;
#ifdef DW1000
/* Default communication configuration. */
static struct mk_uwb_configure config = {//yuan
    .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
@@ -63,6 +65,51 @@
    .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment       */
    .phy_cfg.rx_ant_id = UWB_RX_ANT_3,    /* UWB RX antenna port                       */
};
#elif defined STS_MODE
static struct mk_uwb_configure config = {
    .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
    .phy_cfg.ch_num = 5,                      /* Channel number.                           */
    .phy_cfg.code_index = 9,                  /* TRX preamble code                         */
    .phy_cfg.mean_prf = MEAN_PRF_64M,         /* Mean prf 64/128/256M                      */
    .phy_cfg.data_bit_rate = DATA_BR_6M8,     /* Data rate 6.8M                            */
    .phy_cfg.sync_sym = PREAM_LEN_128,        /* Preamble duration, length of preamble 128 */
    .phy_cfg.sfd_sym = BPRF_NSFD2_8,          /* Identifier for SFD sequence               */
    .phy_cfg.ranging_bit = 1,                 /* ranging bit set 1                         */
    .phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF,  /* IEEE802.15.4z - BPRF mode                 */
    .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_1,     /* SP1 Frame                                 */
    .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1,  /* Number of STS segments in the frame       */
    .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment       */
    .phy_cfg.rx_ant_id = UWB_RX_ANT_3,        /* UWB RX antenna port                       */
};
/* Use the default key and IV specified in the IEEE 802.15.4z attachment */
static struct UWB_STS_KEY_CONFIG_T sts_iv_key = {
    .sts_vcounter = 0x1F9A3DE4,
    .sts_vupper0 = 0xD37EC3CA,
    .sts_vupper1 = 0xC44FA8FB,
    .sts_vupper2 = 0x362EEB34,
    .sts_key0 = 0x14EB220F,
    .sts_key1 = 0xF86050A8,
    .sts_key2 = 0xD1D336AA,
    .sts_key3 = 0x14148674,
};
#else //MKmode
/* Default communication configuration. */
static struct mk_uwb_configure config = {
    .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
    .phy_cfg.ch_num = 9,                      /* Channel number.                           */
    .phy_cfg.code_index = 9,                  /* TX preamble code.                         */
    .phy_cfg.mean_prf = MEAN_PRF_64M,         /* Data rate 6.8M                            */
    .phy_cfg.data_bit_rate = DATA_BR_6M8,     /* data rate 6.8M.                           */
    .phy_cfg.sync_sym = PREAM_LEN_128,        /* Preamble duration, length of preamble 128 */
    .phy_cfg.sfd_sym = BPRF_NSFD2_8,          /* Identifier for SFD sequence               */
    .phy_cfg.ranging_bit = 1,                 /* ranging bit set.                          */
    .phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF,  /* IEEE802.15.4z - BPRF mode                 */
    .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_0,     /* SP0 Frame                                 */
    .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1,  /* Number of STS segments in the frame       */
    .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment       */
    .phy_cfg.rx_ant_id = UWB_RX_ANT_3,        /* UWB RX antenna port                       */
};
#endif
///* Default communication configuration. */
//static struct mk_uwb_configure config = {
//    .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
@@ -123,11 +170,11 @@
int32_t test2;
static uint8_t receive_flag=0;
static uint8_t rec_tag_index,tag_num_tosend;
uint16_t  taglist_total_num,taglist_current_index;  //当前列表总数量

uint32_t start_receive_count,end_receive_count,poll_timeout,current_count,temp_resp;
uint16_t  taglist_total_num,taglist_current_index;  //当前列表总数量
uint8_t  taglist_keeptime[MAX_TAG_LIST_NUM]; //每个标签存活时间
uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //标签距离和标签的ID
uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM],temp_index; //标签距离和标签的ID

int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
uint8_t anclost_times=0 , mainbase_lost_count=0,exsistbase_list[MAX_NEARBASE_NUM],get_newbase=0,nearbase_num,last_nearbase_num,next_nearbase_num;
@@ -151,17 +198,23 @@
};
void uwb_poll_buffer_construct(void)
{
   for(uint8_t i=0;i<nearbase_num;i++)   //标签距离32位转成16位
{
    u16_nearbase_distlist[i] = nearbase_distlist[i];
}
//   for(uint8_t i=0;i<nearbase_num;i++)   //标签距离32位转成16位
//{
//    u16_nearbase_distlist[i] = nearbase_distlist[i];
//}
	
    tag_num_tosend = taglist_total_num-taglist_current_index;
    if(tag_num_tosend>SEND_GROUP_MAX_NUM)  //这次发的数量
    {
        tag_num_tosend = SEND_GROUP_MAX_NUM;
    }
      uwb_sendbuffer[GROUP_ID_IDX] = group_id;
      memcpy(&uwb_sendbuffer[TAG_ID_IDX],&dev_id,2);
   //uwb_sendbuffer[BATTARY_IDX] = bat_percent;
    uwb_sendbuffer[SEQUENCE_IDX] = frame_seq_nb++;
      uwb_sendbuffer[NEARBASENUM_INDEX] = nearbase_num;
      memcpy(&uwb_sendbuffer[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);//基站id
    memcpy(&uwb_sendbuffer[NEARBASEID_INDEX+nearbase_num*2],&u16_nearbase_distlist,nearbase_num*2);
      uwb_sendbuffer[NEARBASENUM_INDEX] = tag_num_tosend;
      memcpy(&uwb_sendbuffer[NEARBASEID_INDEX],&taglist_id[taglist_current_index],tag_num_tosend*2);//基站id
    memcpy(&uwb_sendbuffer[NEARBASEID_INDEX+tag_num_tosend*2],&taglist_dist[taglist_current_index],tag_num_tosend*2);
      uwb_sendbuffer[MESSAGE_TYPE_IDX] = MBX_POLL;   
      memcpy(&uwb_sendbuffer[ANCHOR_ID_IDX],&mainbase_id,2);
}
@@ -199,13 +252,14 @@

    return rx_timestamp;
}

uint32_t temp_rx_first_stamp,temp_rx_second_stamp;
uint16_t temp_rx_first_id,temp_rx_second_id;
/* RX done process handler. */
static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
{
    // Power off radio
    power_off_radio();

      //tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);
    rx_state = rx_report->err_code;

    /** UWB RX success */
@@ -216,6 +270,14 @@
        memcpy(rx_buf, rx_report->pkt_data, rx_length);
        /* Calculate rx timestamp */
        resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
//         if(temp_rx_first_stamp==0)
//         {
//         temp_rx_first_stamp=(uint32_t)resp_rx_ts_i64;
//         memcpy(&temp_rx_first_id,&rx_buf[1],2);
//         }else{
//         memcpy(&temp_rx_second_id,&rx_buf[1],2);
//         temp_rx_second_stamp=(uint32_t)resp_rx_ts_i64;
//         }
            temp_count1=phy_timer_count_get();
               //获取发射端时钟偏差
         resp_rx_num++;
@@ -242,7 +304,7 @@
         temp_count2=phy_timer_count_get();
         
    }
		
      //tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
}

/* TX done process handler. */
@@ -272,7 +334,7 @@
//方法是将新注册的标签添加到下次发送的标签列表头部，其他标签依次向后挪动。
uint16_t AddNewTagIntoList(uint16_t tagid)  
{                                       
     uint16_t newindex_cal = taglist_current_index+25;//当前索引+25，为下次索引要发出的值的最大值，如果总数小于这个值，即可放到最后面。
     uint16_t newindex_cal = taglist_current_index+SEND_GROUP_MAX_NUM;//当前索引+25，为下次索引要发出的值的最大值，如果总数小于这个值，即可放到最后面。
      if(newindex_cal>=taglist_total_num) //如果当前索引是最后一包，那么就加到总索引的后面，如果不是就加到下一次起始索引位置。
      {
        newindex_cal = taglist_total_num;
@@ -288,7 +350,7 @@
      taglist_total_num++;
      taglist_id[newindex_cal] = tagid;
      taglist_dist[newindex_cal] = 0;
      taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
      taglist_keeptime[newindex_cal] = KEEP_TIMES;
      return newindex_cal;
}
uint8_t FindNearBasePos(uint16_t baseid)//寻找当前列表中的基站返回索引
@@ -322,7 +384,21 @@
         nearbase_num = next_nearbase_num;//更新现存基站数
         last_nearbase_num = next_nearbase_num;//更新当前基站数
}

int temp_j;
void TagListUpdate(void)
{
   uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
   for(i=0;i<taglist_total_num;i++)
   {
      if(taglist_keeptime[i]-->0)
      {
         taglist_id[j]=taglist_id[i];
         taglist_keeptime[j++]=taglist_keeptime[i];//自动覆盖，即从前往后寻找存活时间为0的标签，并把它用后面离得最近的第一个有效存活的标签覆盖，那么最后j的值就是当前有效个数
      }
   }
   temp_j=j;
   taglist_total_num=j;
}
void Rank_ANchor_list_by_dis(void)//按照目前的基站距离进行冒泡排序用临时数组方式将最近的放到最前面，最远的放到最后面
{
for(int i=0;i<last_nearbase_num-1;i++)
@@ -334,7 +410,7 @@
                  uint32_t temp_dist,temp_id,temp_exsis;
                  temp_dist=true_nearbase_distlist[j];
                  temp_id = true_nearbase_idlist[j];
                  temp_exsis=true_exsistbase_list[j];
                  temp_exsis=true_exsistbase_list[i];
                  true_nearbase_distlist[j]=true_nearbase_distlist[j+1];
                  true_nearbase_idlist[j]=true_nearbase_idlist[j+1];
                  true_exsistbase_list[j]=true_exsistbase_list[j+1];
@@ -359,7 +435,7 @@
   {
      if(memcmp(&tagid,&taglist_id[i],2)==0)
        {
            taglist_keeptime[i] = TAG_KEEPTIMES;
            taglist_keeptime[i] = KEEP_TIMES;
         return i;
        }
   }
@@ -368,15 +444,29 @@
   return temp;
}
uint32_t count_index;
int TagRange(void)
int tt=1;
float freqlost_count;
void Tag_uwb_init(void)
{
    // The following peripherals will be initialized in the uwb_open function
 // The following peripherals will be initialized in the uwb_open function
    // phy/mac/aes/lsp/phy timers initialized
    uwb_open();

         uwb_open();
#ifdef STS_MODE
      // Set STS key and IV
    phy_sts_key_configure(&sts_iv_key);
#endif
    // Set calibration parameters
    uwb_calibration_params_set(config.phy_cfg.ch_num);

#ifdef STS_MODE
	
    // set advanced parameters
    struct PHY_ADV_CONFIG_T adv_config = {
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .ranging_performance_mode = 3,
        .skip_weakest_port_en = 0,
    };
#else
    // set advanced parameters
     struct PHY_ADV_CONFIG_T adv_config = {
        .thres_fap_detect = 40,
@@ -384,26 +474,29 @@
        .ranging_performance_mode = 0,
        .skip_weakest_port_en = 0,
    };
#endif
    phy_adv_params_configure(&adv_config);

    // uwb configure
    uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);

#if (defined STS_MODE) || (defined MK_MODE)
      ranging_lib_init();
#endif
    ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);

}
int sleep_time_step=SLEEP_COUNT;
int TagRange(void)
{		
      phy_timer_open(1, IRQ_PRIORITY_HIGH);
    // Register rx interrupt callback function
    mac_register_process_handler(tx_int_callback, rx_int_callback);
      //taglist_total_num=50;//测试

//#if LOW_POWER_EN
//    // Initialize low power mode
//    power_init();
//    // Enable sleep timer
//    sleep_timer_open(true, SLEEP_TIMER_MODE_ONESHOT, NULL);
//#endif
      uwb_poll_buffer_construct();
      temp_tag_num=0;//临时数量为0
      poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
      tempflag=uwb_tx(uwb_sendbuffer,13+4*nearbase_num,1,poll_tx_en_start_u32);//立即发送
      tempflag=uwb_tx(uwb_sendbuffer,13+4*tag_num_tosend,1,poll_tx_en_start_u32);//立即发送
      #ifdef BOXING
      gpio_pin_set(IO_PIN_5);//测试
      #endif
@@ -411,54 +504,69 @@
      poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
      temp_count1=phy_timer_count_get();
      while(mac_is_busy());//等待发送完成
   #ifdef BOXING
      gpio_pin_clr(IO_PIN_5);//测试
      gpio_pin_set(IO_PIN_5);//测试
      temp_count2=phy_timer_count_get();
   #endif	
   temp_count2=phy_timer_count_get();
      resp_rx_en_start_u32 =  poll_tx_en_start_u32+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);//设置发送多少s后接收按target独立时间起点算+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US为发送后要多久开启接收时间
      temp_resp=resp_rx_en_start_u32;
   //tempflag=uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
   tempflag=uwb_rx(0,0, RESP_RX_TIMEOUT_US);
   tempflag=uwb_rx(1,resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);
   
   
   start_receive_count=phy_timer_count_get();
   poll_timeout=nearbase_num*US_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS)+US_TO_PHY_TIMER_COUNT(RESP_RX_TIMEOUT_US);//多一个多0.4ms默认0.4ms计算为0.125*4*100000,默认开启1mss
   //poll_timeout=nearbase_num*US_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS)+US_TO_PHY_TIMER_COUNT(RESP_RX_TIMEOUT_US);//多一个多0.4ms默认0.4ms计算为0.125*4*100000,默认开启1mss
   poll_timeout=US_TO_PHY_TIMER_COUNT(RESP_RX_TIMEOUT_US_DEFAULT)+tag_num_tosend*US_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS);
   end_receive_count=start_receive_count+poll_timeout;
   
   if(end_receive_count>=UINT32_MAX)
   {end_receive_count-=UINT32_MAX;}
   current_count=phy_timer_count_get();      
   count_index=end_receive_count+HALF_SECOND_TIME;
//   if(tt){
   while(current_count<end_receive_count||current_count>end_receive_count+HALF_SECOND_TIME)//循环接受包体,若为124.8K则是+62400000
      {
      
      current_count=phy_timer_count_get();//不断刷新当前计数器值
      temp_count2=phy_timer_count_get();
      while(mac_is_busy());//等待接收完成
      while(mac_is_busy())//等待接收完成
      {
                  IdleTask();
            current_count=phy_timer_count_get();
            if(current_count>end_receive_count&&current_count<end_receive_count+HALF_SECOND_TIME)
            {
                    break;
            }
      }
		
      temp_count3=phy_timer_count_get();
      if(receive_flag==1)//成功接收数据
      {
         receive_flag=0;
   tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
         //tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
         //接收成功则判断是否为同一组
         if (rx_buf[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)&&rx_buf[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
                     {   
                           memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                           SetANCTimestap(rec_nearbasepos,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);
                           if(rec_nearbasepos>=nearbase_num)          //发现新的基站
                           {
                              //get_newbase=1;
                              nearbase_num++;
                              nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
                           }
                           exsistbase_list[rec_nearbasepos]=KEEP_TIMES;//更新存活时间
                        rec_tag_index = CmpTagInList(rec_nearbaseid);//当前新来的这包数据插入当前表中的位置索引
                           //rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                           freqlost_count = 0;
                           temp_index =rec_tag_index -taglist_current_index;
                           SetANCTimestap(temp_index,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);
//                           if(rec_nearbasepos>=nearbase_num)          //发现新的基站
//                           {
//                              //get_newbase=1;
//                              nearbase_num++;
//                              nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
//                           }
//                           exsistbase_list[rec_nearbasepos]=KEEP_TIMES;//更新存活时间
              
                     memset(rx_buf, 0, sizeof(rx_buf));//清空接收到的用完的数组
                     rx_length=0;//清0长度
//                     uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
                     uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
//                     while(mac_is_busy());
                     //break;//成功接收就退出
					
      }
                     
//            #ifdef BOXING 
@@ -467,6 +575,9 @@
      }else if(receive_flag==2){//接收出错
      receive_flag=0;
      tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
//      sleep_timer_start(__MS_TO_32K_CNT(SLEEP_COUNT-phy_timer_count_get()%30));//测试
//         sleep_time_step=SLEEP_COUNT-phy_timer_count_get()%10;
      //tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
      temp_count3=phy_timer_count_get();
//      while(mac_is_busy());
      temp_count1=phy_timer_count_get();
@@ -475,7 +586,8 @@
      gpio_pin_clr(IO_PIN_5);//测试
      gpio_pin_set(IO_PIN_5);//测试
      #endif
      }	
      }
//   }
      #ifdef BOXING 
      gpio_pin_clr(IO_PIN_5);//测试
      #endif
@@ -483,7 +595,12 @@
      uwb_rx_force_off(1);
      //dwt_forecetrxoff();
       CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
       AnchorListUpdate();//更新存活基站列表
   //    Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
      //LOG_INFO(TRACE_MODULE_APP,"现存测距数 %d id1:%04x id2:%04x .\r\n",taglist_total_num,taglist_id[0],taglist_id[1]);
       //AnchorListUpdate();//更新存活基站列表
       TagListUpdate();
       taglist_current_index+=tag_num_tosend;
      if(taglist_current_index>=taglist_total_num||taglist_total_num<=SEND_GROUP_MAX_NUM) //如果标签当前index大于总index，或者总基站数量小于等于25，则从0开始index。一个是已经发完一轮了，一个是不够最大发送数
     taglist_current_index = 0;
       //Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
}
#endif