能够SS测距与MK8000_SS_demo_Anchor配合使用

chen

2024-07-29 55579b9ada2fc22edcae817e7d6e65710c47edd3

 keil/simple_ss_twr_dw_init.c

@@ -7,6 +7,7 @@
#include "lib_ranging.h"
#include "dw_app.h"
#include "board.h"
#include "mk_calib.h"

#if defined(MK_SS_TWR_DW_INIT)

@@ -16,12 +17,15 @@
#define RANGING_PERIOD_MS (1000)

/* 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 700U
#define POLL_TX_TO_RESP_RX_DLY_US 750U  //yuan700

#define RESP_RX_TO_FINAL_TX_DLY_US 550U

/* Poll delay: 1s */
#define POLL_DELAY  100U

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

/* Field index in frame */
#define MSG_SEQ_NUM_IDX 2
@@ -37,12 +41,12 @@
    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 = {
    .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
    .phy_cfg.ch_num = 5,                      /* Channel number.                           */
    .phy_cfg.ch_num = 9,                      /* 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                            */
@@ -53,11 +57,11 @@
    .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_ANTENNA_3,    /* UWB RX antenna port                       */
    .phy_cfg.rx_ant_id = UWB_RX_ANT_3,    /* UWB RX antenna port                       */
};

/* Buffer to store received frame */
static uint8_t rx_buf[128];
static uint8_t rx_buf[150];
static uint8_t uwb_sendbuffer[150];
/*     Frames used in the ranging process
 *     Poll message:
@@ -92,13 +96,29 @@
static uint8_t frame_seq_nb = 0;

static volatile uint16_t rx_state;
static volatile uint16_t rx_state1;
static volatile uint16_t rx_length;
//DW
uint16_t rec_nearbaseid,temp_index;
uint16_t rec_nearbaseid;
uint8_t temp_tag_num;
int32_t test2;
static uint8_t receive_flag=0;
static uint8_t rec_tag_index,tag_num_tosend;

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

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;
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_count1=0;
uint32_t temp_count2=0;
uint32_t temp_count3=0;
uint32_t tempflag=0;
int32_t freq_offset,freq_offset_filter; 
enum SIMPLE_FSM_T
{
    SIMPLE_IDLE = 0,
@@ -108,7 +128,19 @@
};
void uwb_poll_buffer_construct(void)
{

   for(uint8_t i=0;i<nearbase_num;i++)   //标签距离32位转成16位
{
    u16_nearbase_distlist[i] = nearbase_distlist[i];
}
      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[MESSAGE_TYPE_IDX] = MBX_POLL;	
      memcpy(&uwb_sendbuffer[ANCHOR_ID_IDX],&mainbase_id,2);
}

static enum SIMPLE_FSM_T state = SIMPLE_IDLE;
@@ -161,7 +193,14 @@
        memcpy(rx_buf, rx_report->pkt_data, rx_length);
        /* Calculate rx timestamp */
        resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
            temp_count2=phy_timer_count_get();
               //获取发射端时钟偏差
         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 宏定义避免收包中途校准导致测距错误
            receive_flag=1;
			
    }
    else
    {
@@ -171,8 +210,10 @@
        /* UWB_BD_ERR      Preamble detection error  */
        /* UWB_TO_ERR      Receive timeout           */
        /* UWB_STS_ERR     STS error                 */
            rx_state1=rx_report->err_code;	
        rx_length = 0;
            receive_flag=2;
         temp_count2=phy_timer_count_get();
    }
      
}
@@ -186,18 +227,11 @@
    /** UWB TX success */
    if (tx_report->err_code == UWB_TX_OK)
    {
         temp_count1=phy_timer_count_get();//测试
    }
}


uint32_t start_receive_count,end_receive_count,poll_timeout,current_count;
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
uint8_t rec_tag_num,temp_tag_num,kp1;
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;
uint16_t nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],rec_nearbaseid,rec_nearbasepos;

//注意：因为新注册的标签并不知道基站已经收到了，所以要在下一包立刻将新的标签ID发出，否则标签会一直抢占基站的poll包。
//方法是将新注册的标签添加到下次发送的标签列表头部，其他标签依次向后挪动。
@@ -298,6 +332,7 @@
    temp = AddNewTagIntoList(tagid);
   return temp;
}

int simple_main(void)
{
    // The following peripherals will be initialized in the uwb_open function
@@ -312,7 +347,7 @@
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .ranging_performance_mode = 0,
        .dynamic_port_sel_en = 0,
        .skip_weakest_port_en = 0,
    };
    phy_adv_params_configure(&adv_config);

@@ -332,27 +367,31 @@
//#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());//修正时间戳
      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);//立即发送
      poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
      while(mac_is_busy());//等待发送完成
		
      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);
   temp_count3=phy_timer_count_get();
   while(mac_is_busy());//等待接收完成
   temp_count2=phy_timer_count_get();
      start_receive_count=phy_timer_count_get();
      poll_timeout=tag_num_tosend*50000+50000;//多一个多0.4ms默认0.4ms计算为0.125*4*100000
   poll_timeout=nearbase_num*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;}
   if(end_receive_count>=UINT32_MAX)
   {end_receive_count-=UINT32_MAX;}
   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数据
                     {   
@@ -362,7 +401,7 @@
                                                   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);//这个是改过的参数
                                  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])
//                                                   {
@@ -372,7 +411,7 @@
                                                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);
                           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)          //发现新的基站
                           {
                              //get_newbase=1;
@@ -383,15 +422,19 @@
              }
                      memset(rx_buf, 0, sizeof(rx_buf));//清空接收到的用完的数组
                        rx_length=0;//清0长度
                        uwb_rx(0, 0, 0);//立即开启接受并设置0超时
                     uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
                     while(mac_is_busy());
      }
      }else if(receive_flag==2){//接收出错
      receive_flag=0;
      uwb_rx(0, 0, 0);//立即开启接受并设置0超时
      }
      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();
      }
      //关闭UWBapi
       uwb_close();
       CalculateDists(poll_tx_ts_i64); //计算距离
       //uwb_close();
       CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
       AnchorListUpdate();//更新存活基站列表
       Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
      }