通过debug已经测试功能正常，移植到正确的工卡试下

zhangbo

2 天以前 01b6f2525f47ee781d86a2495dfcd17f68ccbafd

 keil/simple_ss_twr_dw_init.c

@@ -161,7 +161,7 @@
        memcpy(rx_buf, rx_report->pkt_data, rx_length);
        /* Calculate rx timestamp */
        resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
            receive_flag=1;
        receive_flag=1;
    }
    else
    {
@@ -172,9 +172,9 @@
        /* UWB_TO_ERR      Receive timeout           */
        /* UWB_STS_ERR     STS error                 */
        rx_length = 0;
            receive_flag=2;
        receive_flag=2;
    }
		

}

/* TX done process handler. */
@@ -201,102 +201,102 @@

//注意：因为新注册的标签并不知道基站已经收到了，所以要在下一包立刻将新的标签ID发出，否则标签会一直抢占基站的poll包。
//方法是将新注册的标签添加到下次发送的标签列表头部，其他标签依次向后挪动。
uint16_t AddNewTagIntoList(uint16_t tagid)  
{                                       
     uint16_t newindex_cal = taglist_current_index+25;//当前索引+25，为下次索引要发出的值的最大值，如果总数小于这个值，即可放到最后面。
      if(newindex_cal>=taglist_total_num) //如果当前索引是最后一包，那么就加到总索引的后面，如果不是就加到下一次起始索引位置。
      {
uint16_t AddNewTagIntoList(uint16_t tagid)
{
    uint16_t newindex_cal = taglist_current_index+25;//当前索引+25，为下次索引要发出的值的最大值，如果总数小于这个值，即可放到最后面。
    if(newindex_cal>=taglist_total_num) //如果当前索引是最后一包，那么就加到总索引的后面，如果不是就加到下一次起始索引位置。
    {
        newindex_cal = taglist_total_num;
          
      }else{           
         for(uint16_t i=taglist_total_num;i>newindex_cal;i--)  //从后往前挪地方??就是插入
            {
                taglist_id[i]=taglist_id[i-1];
                taglist_dist[i]=taglist_dist[i-1];
                taglist_keeptime[i]=taglist_keeptime[i-1];
            }          
      }
      taglist_total_num++;
      taglist_id[newindex_cal] = tagid;
      taglist_dist[newindex_cal] = 0;
      taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
      return newindex_cal;

    } else {
        for(uint16_t i=taglist_total_num; i>newindex_cal; i--) //从后往前挪地方??就是插入
        {
            taglist_id[i]=taglist_id[i-1];
            taglist_dist[i]=taglist_dist[i-1];
            taglist_keeptime[i]=taglist_keeptime[i-1];
        }
    }
    taglist_total_num++;
    taglist_id[newindex_cal] = tagid;
    taglist_dist[newindex_cal] = 0;
    taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
    return newindex_cal;
}
uint8_t FindNearBasePos(uint16_t baseid)//寻找当前列表中的基站返回索引
{
   uint8_t i;
   for(i=0;i<nearbase_num;i++)
   {
      if(baseid==nearbaseid_list[i])
         return i;
   }
   if(i==nearbase_num)
      return nearbase_num;
    uint8_t i;
    for(i=0; i<nearbase_num; i++)
    {
        if(baseid==nearbaseid_list[i])
            return i;
    }
    if(i==nearbase_num)
        return nearbase_num;
}

void AnchorListUpdate(void)
{
   next_nearbase_num = 0;
         int j=0;
         for(int i=0;i<nearbase_num;i++)
         {
            if(exsistbase_list[i]>0)
            {	
          exsistbase_list[i]--;	
               next_nearbase_num++;
               true_exsistbase_list[j]=exsistbase_list[i];
               true_nearbase_idlist[j]=nearbaseid_list[i];			
               true_nearbase_distlist[j++]=nearbase_distlist[i];
					
            }
            nearbase_num = next_nearbase_num;//更新现存基站数
            last_nearbase_num = next_nearbase_num;//更新当前基站数
         }
    next_nearbase_num = 0;
    int j=0;
    for(int i=0; i<nearbase_num; i++)
    {
        if(exsistbase_list[i]>0)
        {
            exsistbase_list[i]--;
            next_nearbase_num++;
            true_exsistbase_list[j]=exsistbase_list[i];
            true_nearbase_idlist[j]=nearbaseid_list[i];
            true_nearbase_distlist[j++]=nearbase_distlist[i];

        }
        nearbase_num = next_nearbase_num;//更新现存基站数
        last_nearbase_num = next_nearbase_num;//更新当前基站数
    }
}

void Rank_ANchor_list_by_dis(void)//按照目前的基站距离进行冒泡排序用临时数组方式将最近的放到最前面，最远的放到最后面
{
for(int i=0;i<last_nearbase_num-1;i++)
         {
            for(int j=0;j<last_nearbase_num-1;j++)
            {
               if(true_nearbase_distlist[j]>true_nearbase_distlist[j+1])
               {
                  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];
                  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];
						
                  true_nearbase_distlist[j+1]=temp_dist;
                  true_nearbase_idlist[j+1]=temp_id;
                  true_exsistbase_list[j+1]=temp_exsis;
               }			
            }
         }
         for (int i=0;i<last_nearbase_num;i++)
         {
            nearbaseid_list[i]=true_nearbase_idlist[i];
            nearbase_distlist[i]=true_nearbase_distlist[i];
            exsistbase_list[i] = true_exsistbase_list[i];
         }
    for(int i=0; i<last_nearbase_num-1; i++)
    {
        for(int j=0; j<last_nearbase_num-1; j++)
        {
            if(true_nearbase_distlist[j]>true_nearbase_distlist[j+1])
            {
                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];
                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];

                true_nearbase_distlist[j+1]=temp_dist;
                true_nearbase_idlist[j+1]=temp_id;
                true_exsistbase_list[j+1]=temp_exsis;
            }
        }
    }
    for (int i=0; i<last_nearbase_num; i++)
    {
        nearbaseid_list[i]=true_nearbase_idlist[i];
        nearbase_distlist[i]=true_nearbase_distlist[i];
        exsistbase_list[i] = true_exsistbase_list[i];
    }
}
uint16_t CmpTagInList(uint16_t tagid)//找这个新一包的基站在不在现存的taglist中在的话对应位置更新存活时间，不在就增加（确保有效数量小于最大设置数）
{
    uint16_t temp;
   for(uint8_t i=0;i<taglist_total_num;i++)
   {
      if(memcmp(&tagid,&taglist_id[i],2)==0)
    for(uint8_t i=0; i<taglist_total_num; i++)
    {
        if(memcmp(&tagid,&taglist_id[i],2)==0)
        {
            taglist_keeptime[i] = TAG_KEEPTIMES;
         return i;
            return i;
        }
   }
    }
    if(taglist_total_num<MAX_TAG_LIST_NUM)
    temp = AddNewTagIntoList(tagid);
   return temp;
        temp = AddNewTagIntoList(tagid);
    return temp;
}
int simple_main(void)
{
@@ -330,70 +330,72 @@
//    // 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();
      uwb_tx(uwb_sendbuffer,sizeof(uwb_sendbuffer),0,0);//立即发送
      poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_start_time_gap());//修正时间戳
      while(mac_is_busy());//等待发送完成
		
      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为发送后要多久开启接收时间
      start_receive_count=phy_timer_count_get();
      poll_timeout=tag_num_tosend*50000+50000;//多一个多0.4ms默认0.4ms计算为0.125*4*100000
      end_receive_count=start_receive_count+poll_timeout;
      if(end_receive_count>=124800000)
   {end_receive_count-=124800000;}
   current_count=phy_timer_count_get();
   uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
      while(current_count<end_receive_count||current_count>end_receive_count+62400000)//循环接受包体,若为124.8K则是+62400000
      {
      current_count=phy_timer_count_get();//不断刷新当前计数器值
      if(receive_flag==1)//成功接收数据
      {
         receive_flag=0;
         //获取发射端时钟偏差
         test2=phy_freq_offset_get();
         //接收成功则判断是否为同一组
         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);
                                                if(last_nearbase_num==0)
                                                {
                                                   nearbaseid_list[0]=rec_nearbaseid;
                                                   nearbase_num=1;
                                                   exsistbase_list[0]=KEEP_TIMES;
                                  SetANCTimestap(0,&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);//这个是改过的参数
                                                }
    uwb_poll_buffer_construct();
    temp_tag_num=0;//临时数量为0
    poll_tx_en_start_u32 = phy_timer_count_get();
    uwb_tx(uwb_sendbuffer,sizeof(uwb_sendbuffer),0,0);//立即发送
    poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_start_time_gap());//修正时间戳
    while(mac_is_busy());//等待发送完成

    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为发送后要多久开启接收时间
    start_receive_count=phy_timer_count_get();
    poll_timeout=tag_num_tosend*50000+50000;//多一个多0.4ms默认0.4ms计算为0.125*4*100000
    end_receive_count=start_receive_count+poll_timeout;
    if(end_receive_count>=124800000)
    {
        end_receive_count-=124800000;
    }
    current_count=phy_timer_count_get();
    uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
    while(current_count<end_receive_count||current_count>end_receive_count+62400000)//循环接受包体,若为124.8K则是+62400000
    {
        current_count=phy_timer_count_get();//不断刷新当前计数器值
        if(receive_flag==1)//成功接收数据
        {
            receive_flag=0;
            //获取发射端时钟偏差
            test2=phy_freq_offset_get();
            //接收成功则判断是否为同一组
            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);
                if(last_nearbase_num==0)
                {
                    nearbaseid_list[0]=rec_nearbaseid;
                    nearbase_num=1;
                    exsistbase_list[0]=KEEP_TIMES;
                    SetANCTimestap(0,&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);//这个是改过的参数
                }
//                                                if(rec_nearbaseid==nearbaseid_list[0])
//                                                   {
//                                     exsistbase_list[0]=KEEP_TIMES;
//                                   SetANCTimestap(0,&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);//这个是改过的参数
//                                                   }
                                                else
                        {
                           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);
                           if(rec_nearbasepos>=last_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, 0);//立即开启接受并设置0超时
      }else if(receive_flag==2){//接收出错
      receive_flag=0;
      uwb_rx(0, 0, 0);//立即开启接受并设置0超时
      }
      }
      //关闭UWBapi
       uwb_close();
       CalculateDists(poll_tx_ts_i64); //计算距离
       AnchorListUpdate();//更新存活基站列表
       Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
      }
                else
                {
                    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);
                    if(rec_nearbasepos>=last_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, 0);//立即开启接受并设置0超时
            } else if(receive_flag==2) { //接收出错
                receive_flag=0;
                uwb_rx(0, 0, 0);//立即开启接受并设置0超时
            }
        }
        //关闭UWBapi
        uwb_close();
        CalculateDists(poll_tx_ts_i64); //计算距离
        AnchorListUpdate();//更新存活基站列表
        Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
    }
}
#endif