1111111

WXK

2025-02-11 e328ebef585cea2351b37117b2d5ac4978ecd3c0

 keil/include/drivers/uwb_tag_.c

copy from keil/uwb_tag.c
copy to keil/include/drivers/uwb_tag_.c
Îļþ´Ó keil/uwb_tag.c ¸´ÖÆ
@@ -5,14 +5,14 @@
#include "mk_power.h"
#include "mk_sleep_timer.h"
#include "lib_ranging.h"
#include "dw_tag.h"
#include "dw_app.h"
#include "board.h"
#include "mk_calib.h"

extern int TagRange(void);
extern Operation_step UWB_work_state;
#if defined(MK_SS_TWR_DW_INIT)

#define CALCULATE_TIMECOUNT
extern int TagRange(void);

/* Ranging period: 1s */
#define RANGING_PERIOD_MS (1000)

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

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

/* 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 1000
#define DELAY_DEFAULT 50000
#define DELAY_BETWEEN_TWO_FRAME_UUS 600  //yuan1400
#define HALF_SECOND_TIME 62400000

static uint16_t CmpTagInList(uint16_t tagid);
uint16_t CmpTagInList(uint16_t tagid);
uint16_t AddNewTagIntoList(uint16_t tagid);
void AnchorListUpdate(void);
void Rank_ANchor_list_by_dis(void);
@@ -45,8 +45,8 @@
    uint8_t phy_work_mode; /* PHY_TX / PHY_RX / PHT_TX|PHY_RX */
    struct UWB_CONFIG_T phy_cfg;
};
extern uint32_t dev_id;
extern uint8_t group_id;
uint32_t dev_id;
uint8_t group_id;
/* Default communication configuration. */
static struct mk_uwb_configure config = {//yuan
    .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
@@ -124,7 +124,7 @@
static uint8_t receive_flag=0;
static uint8_t rec_tag_index,tag_num_tosend;

static uint32_t start_receive_count,end_receive_count,poll_timeout,current_count,temp_resp;
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
@@ -133,16 +133,15 @@
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;
uint16_t nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],rec_nearbaseid,rec_nearbasepos;
uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM];
uint32_t temp_tag_count1=0;
uint32_t temp_tag_count2=0;
uint32_t temp_tag_count3=0;
uint32_t temp_count1=0;
uint32_t temp_count2=0;
uint32_t temp_count3=0;
uint32_t temp_count4=0;
uint32_t tempflag=0;
int32_t freq_offset,freq_offset_filter; 
int temp_flag,poll_tx_num,resp_rx_num;
extern double distance;
uint32_t count1,count2;
extern double distance_tag;
enum SIMPLE_FSM_T
{
    SIMPLE_IDLE = 0,
@@ -217,13 +216,14 @@
        memcpy(rx_buf, rx_report->pkt_data, rx_length);
        /* Calculate rx timestamp */
        resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
            temp_tag_count1=phy_timer_count_get();
            temp_count1=phy_timer_count_get();
               //获取发射端时钟偏差
         resp_rx_num++;
         freq_offset=phy_freq_offset_get();
         freq_offset_filter=average_filter(freq_offset);//获取频偏
//         int32_t ppm = freq_offset_filter / (int32_t)(ch_center_freq_map[UWB_CH_NUM] * 1e-6);
//         calib_xtal38m4_load_cap_auto_tune(ppm);//利用电容调整晶振适配频偏应在完整的一包之后调整，需要关闭XTAL_AUTO_TUNE_EN å®å®šä¹‰é¿å…æ”¶åŒ…中途校准导致测距错误
			
      //   freq_offset_filter=average_filter(freq_offset);//获取频偏
         //int32_t ppm = freq_offset_filter / (int32_t)(ch_center_freq_map[UWB_CH_NUM] * 1e-6);
         //calib_xtal38m4_load_cap_auto_tune(ppm);//利用电容调整晶振适配频偏应在完整的一包之后调整，需要关闭XTAL_AUTO_TUNE_EN å®å®šä¹‰é¿å…æ”¶åŒ…中途校准导致测距错误
         //LOG_INFO(TRACE_MODULE_APP, "poll_tx_num is %d,resp_rx_num is %d,distance is %lf\r\n",poll_tx_num,resp_rx_num,distance);   
         receive_flag=1;
         
@@ -239,7 +239,7 @@
            rx_state1=rx_report->err_code;   
        rx_length = 0;
            receive_flag=2;
         temp_tag_count2=phy_timer_count_get();
         temp_count2=phy_timer_count_get();
         
    }
      
@@ -261,7 +261,7 @@
            temp_flag=0;
            return;
         }
         count1=temp_tag_count1;
         count1=temp_count1;
            temp_flag=1;
    }
}
@@ -318,9 +318,9 @@
               true_nearbase_distlist[j++]=nearbase_distlist[i];
               
            }
            nearbase_num = next_nearbase_num;//更新现存基站数
            last_nearbase_num = next_nearbase_num;//更新当前基站数
         }
         nearbase_num = next_nearbase_num;//更新现存基站数
         last_nearbase_num = next_nearbase_num;//更新当前基站数
}

void Rank_ANchor_list_by_dis(void)//按照目前的基站距离进行冒泡排序用临时数组方式将最近的放到最前面，最远的放到最后面
@@ -352,7 +352,7 @@
            exsistbase_list[i] = true_exsistbase_list[i];
         }
}
static uint16_t CmpTagInList(uint16_t tagid)//找这个新一包的基站在不在现存的taglist中在的话对应位置更新存活时间，不在就增加（确保有效数量小于最大设置数）
uint16_t CmpTagInList(uint16_t tagid)//找这个新一包的基站在不在现存的taglist中在的话对应位置更新存活时间，不在就增加（确保有效数量小于最大设置数）
{
    uint16_t temp;
   for(uint8_t i=0;i<taglist_total_num;i++)
@@ -367,7 +367,8 @@
    temp = AddNewTagIntoList(tagid);
   return temp;
}

uint32_t count_index;
int tt=1;
int TagRange(void)
{
    // The following peripherals will be initialized in the uwb_open function
@@ -404,104 +405,88 @@
      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);//立即发送
		
      //gpio_pin_set(IO_PIN_5);//测试
      #ifdef BOXING
      gpio_pin_set(IO_PIN_5);//测试
      #endif
      //LOG_INFO(TRACE_MODULE_APP, "进入测距",poll_tx_num,resp_rx_num,distance);   
      poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
      temp_tag_count1=phy_timer_count_get();
      temp_count1=phy_timer_count_get();
      while(mac_is_busy());//等待发送完成
      temp_tag_count2=phy_timer_count_get();
      gpio_pin_clr(IO_PIN_5);//测试
      gpio_pin_set(IO_PIN_5);//测试
      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);
   
   receive_flag =0;
	
   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
   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_tag_count2=phy_timer_count_get();
      temp_count2=phy_timer_count_get();
      while(mac_is_busy());//等待接收完成
      temp_tag_count3=phy_timer_count_get();
      temp_count3=phy_timer_count_get();
      if(receive_flag==1)//成功接收数据
      {
         receive_flag=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);
                                       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,(uint32_t)poll_tx_ts_i64);//这个是改过的参数
                                       }
//                                                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
                        {
                           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>=last_nearbase_num)          //发现新的基站
                           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超时
//                     while(mac_is_busy());
                     //break;//成功接收就退出
					
      }
      tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
      //gpio_pin_clr(IO_PIN_5);//测试
			
							
//            #ifdef BOXING 
//      gpio_pin_clr(IO_PIN_5);//测试
//         #endif
      }else if(receive_flag==2){//接收出错
      receive_flag=0;
      tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
      temp_tag_count3=phy_timer_count_get();
      temp_count3=phy_timer_count_get();
//      while(mac_is_busy());
      temp_tag_count1=phy_timer_count_get();
      temp_count1=phy_timer_count_get();
      }
      //gpio_pin_clr(IO_PIN_5);//测试
   #ifdef BOXING 
      gpio_pin_clr(IO_PIN_5);//测试
      gpio_pin_set(IO_PIN_5);//测试
      #endif
      }
//   }
      #ifdef BOXING 
      gpio_pin_clr(IO_PIN_5);//测试
      #endif
      delay_us(1);
      uwb_rx_force_off(1);
      //dwt_forecetrxoff();
       CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
       AnchorListUpdate();//更新存活基站列表
       Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
       LOG_INFO(TRACE_MODULE_APP,"基站ID：%X,距离: %d\r\n",rec_nearbaseid,nearbase_distlist[0]);
   //    Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
}


void UWB_Tag_Poll(void)
{
  switch(UWB_work_state)
   {
    case LINK_SUCCESS:
    case SEARCH_DEV:
    case UN_BIND:
   {
   uwb_led_on();
    TagRange();
   uwb_led_off();
   } 
         break;
   }
// update_led_power_state();//更新灯状态
}
#endif