新测距算法加入，稳定测距版本

chen

2024-12-31 5e02aaa74bd161d42cd3a48dc5d6f89491ea29ef

 keil/uwb_app.c

@@ -78,7 +78,7 @@
#define RANGING_PERIOD_MS (1000)

/* This is the delay from Frame RX POLL frame to send RESP Frame */
#define POLL_RX_TO_RESP_TX_DLY_US 550U //yuan600 success 并且不卡busy
#define POLL_RX_TO_RESP_TX_DLY_US 550U //yuan550极限

#define RESP_TX_TO_FINAL_RX_DLY_US 500U

@@ -392,17 +392,35 @@
//      }
      
}
//uint16_t CmpTagInList(uint16_t tagid)
//{   uint16_t i;
//    for(i=0; i<taglist_num; i++)
//    {
//        if(memcmp(&tagid,&anchordata_id[i],2)==0)
//            break;
//    }
//    if(i==taglist_num)
//        return taglist_num;
//    //tagofflinetime[i] = 0;
//    return i;
//}yuan
uint16_t CmpTagInList(uint16_t tagid)
{   uint16_t i;
    for(i=0; i<taglist_num; i++)
    {
        if(memcmp(&tagid,&anchordata_id[i],2)==0)
        if(memcmp(&tagid,&tagid_list[i],2)==0)
            break;
    }
    if(i==taglist_num)
        return taglist_num;
    //tagofflinetime[i] = 0;
    return i;
}
uint16_t ExistInTagList(uint16_t tagid)
{   uint16_t i;
    for(i=0; i<taglist_num; i++)
    {
        if(memcmp(&tagid,&tagid_list[i],2)==0)
            return 1;
    }
    return 0;
}

uint16_t CmpCarInExistList(uint16_t tagid)
@@ -429,20 +447,33 @@
    }
}

//void TagListUpdate(void)
//{
//    uint8_t i,j=0;
//    for(i=0; i<anchordata_num; i++)
//    {
//        if(tagofflinetime[i]++<QUIT_SLOT_TIME)
//        {
//            anchordata_id[j]=anchordata_id[i];
//            tagofflinetime[j++]=tagofflinetime[i];
//        }
//    }
//    anchordata_num=j;
//}
void TagListUpdate(void)
{
    uint8_t i,j=0;
    for(i=0; i<anchordata_num; i++)
uint16_t i,j=0,k=0;
    for(i=0; i<taglist_num; i++)
    {
        if(tagofflinetime[i]++<QUIT_SLOT_TIME)
          if(tagofflinetime[i]++<TAG_KEEPTIMES)
        {
            anchordata_id[j]=anchordata_id[i];
            tagid_list[j]=tagid_list[i];
            tagdist_list[j] = tagdist_list[i];
            tagofflinetime[j++]=tagofflinetime[i];
        }
    }
    anchordata_num=j;
      }
       taglist_num=j;
}


uint8_t position;
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos)//根据自己是否为新基站定制消息去发送，根据是否抢占判断
@@ -476,6 +507,7 @@
    temp_resp_i64=resp_tx_ts_i64;
    temp_count3= phy_timer_count_get();
    flag_temp2=uwb_tx(send_buffer, 40,1 ,resp_tx_en_start_u32);//立即发送测试size大小
      tagofflinetime[taglist_pos] = 0;//更新标签通信
//    temp_count1=phy_timer_count_get();
    //while(mac_is_busy());
    //gpio_pin_clr(SCL_PIN);
@@ -581,32 +613,32 @@
//            wltag_uwbtimer = HIDO_TimerGetTick();
//            CloseUWB();
//      }
////      if(uwb_losttimer>g_com_map[UWB_RNAGE_TIME])
////      { 
////       wltag_statetimer = HIDO_TimerGetTick();
////         if(uwb_rx_flag==0)//若要切换到搜索模式时没有开启则开启UWB
////         {
////            OpenUWB();
////         }
////     wltag_state = SEARCH;
////      }
//      if(uwb_losttimer>g_com_map[UWB_RNAGE_TIME])
//      { 
//       wltag_statetimer = HIDO_TimerGetTick();
//         if(uwb_rx_flag==0)//若要切换到搜索模式时没有开启则开启UWB
//         {
//            OpenUWB();
//         }
//     wltag_state = SEARCH;
//      }
//      break;
////   case SEARCH:
////      if(HIDO_TimerGetTick()-wltag_statetimer>1)
////      {
////      wltag_statetimer = HIDO_TimerGetTick();
////    wltag_state = CLOSE;
////    CloseUWB();
////      }
////      break;
////   case CLOSE:
////      if(HIDO_TimerGetTick()-wltag_statetimer>g_com_map[UWB_WAIT_TIME])
////      {
////      wltag_statetimer = HIDO_TimerGetTick();
////    wltag_state = SEARCH;
////    OpenUWB();
////      }
////      break;
//   case SEARCH:
//      if(HIDO_TimerGetTick()-wltag_statetimer>1)
//      {
//      wltag_statetimer = HIDO_TimerGetTick();
//    wltag_state = CLOSE;
//    CloseUWB();
//      }
//      break;
//   case CLOSE:
//      if(HIDO_TimerGetTick()-wltag_statetimer>g_com_map[UWB_WAIT_TIME])
//      {
//      wltag_statetimer = HIDO_TimerGetTick();
//    wltag_state = SEARCH;
//    OpenUWB();
//      }
//      break;
//   }
    UWBIdleTask();
}
@@ -627,34 +659,55 @@
            memcpy(rec_ancidlist,&rx_buf[FZ_NEARBASEID_INDEX],2*rec_nearbase_num);
            memcpy(rec_ancdistlist,&rx_buf[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num);
            memcpy(&rec_antdelay,&rx_buf[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2);
            for(i=0; i<rec_nearbase_num; i++)
            {
                if(rec_ancidlist[i] == dev_id)
                {      temp_recdist_before_offset=rec_ancdistlist[i];
                    rec_ancdistlist[i]+=(int16_t)g_com_map[DIST_OFFSET];
                    PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);
                }
            }
            for(i=0; i<rec_nearbase_num; i++)
            {
                memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
                if(tempid==dev_id)//也是比id如果有就更新没有就抢占
                {
                    seize_anchor=0;  //非抢占。已存在列表中
                    Anchor_RecNearPoll(i);
                    break;
                }
            }
//            for(i=0; i<rec_nearbase_num; i++)
//            {
//                if(rec_ancidlist[i] == dev_id)
//                {      temp_recdist_before_offset=rec_ancdistlist[i];
//                    rec_ancdistlist[i]+=(int16_t)g_com_map[DIST_OFFSET];
//                    PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);
//                }
//            }
//            for(i=0; i<rec_nearbase_num; i++)
//            {
//                memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
//                if(tempid==dev_id)//也是比id如果有就更新没有就抢占
//                {
//                    seize_anchor=0;  //非抢占。已存在列表中
//                    Anchor_RecNearPoll(i);
//                    break;
//                }
//            }

            if(i==rec_nearbase_num)
            {
                seize_anchor=1;   //抢占anchor
                Anchor_RecNearPoll(i);
            }
            }else{
         //   OpenUWB();//接收到不是自己的包再次开启UWB接收
//            if(i==rec_nearbase_num)
//            {
//                seize_anchor=1;   //抢占anchor
//                Anchor_RecNearPoll(i);
//            }
                        taglist_pos=CmpTagInList(tag_id_recv);
                if(taglist_num>=ANC_MAX_NUM)
                              return 0;
                         for(i=0; i<rec_nearbase_num; i++)
                        {
                              memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
                              if(tempid==dev_id)
                              {
                                    Anchor_RecNearPoll(i);
                                    tagdist_list[taglist_pos]=rec_ancdistlist[i];
                                    if(taglist_pos==taglist_num)// taglist_pos==taglist_num 说明这个基站不在当前列表中
                                    {   //tempid==dev_id 说明基站下发测距报文有这个标签的信息
                                          taglist_num++;          //满足上述两种情况才会添加基站ID进入列表中，否则会出现标签不在基站列表中，标签也不响应的情况
                                          tagid_list[taglist_pos] = tag_id_recv;
                                    }
                                    break;
                              }
                        }
                         if((i==rec_nearbase_num&&taglist_pos==taglist_num)||(rec_nearbase_num==0&&!ExistInTagList(tag_id_recv)))//当基站不在标签列表中，标签也不在基站列表中时，随机找个时间片回复基站。
                {
                       Anchor_RecNearPoll(rec_nearbase_num);
                }
            }
            
				
}
int UwbRange(void)
{