0.6.8移植完成版本，初步调好两边sts配置，但只能接受一包，基站因为中断中多加了些sts校验计算的语句导致耗时tx时间卡在过去问题未解决

chen

2025-07-04 147b7f967cb84086a53c8bee7d4acf81c49a2bb1

 keil/uwb_app.c

@@ -10,7 +10,9 @@
#include "global_param.h"
#include "board.h"
#include "lib_aoa.h"

#if PDOA_3D_EN
#include "lib_pdoa_3d.h"
#endif
extern int simple_main(void);
extern int temp_main(void);
void Calibration_Time(void);
@@ -150,7 +152,9 @@
    .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                       */
    .phy_cfg.rx_main_ant = UWB_RX_ANT_3,             /* UWB RX main antenna port                  */
    .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode                       */
    .phy_cfg.pulse_shape = 0x2,                      /* 0x0: CCC pulse, 0x2: FiRa pulse           */
};
#elif defined STS_MODE
static struct mk_uwb_configure config = {
@@ -166,7 +170,9 @@
    .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                       */
     .phy_cfg.rx_main_ant = UWB_RX_ANT_3,             /* UWB RX main antenna port                  */
    .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode                       */
    .phy_cfg.pulse_shape = 0x2,                      /* 0x0: CCC pulse, 0x2: FiRa pulse           */
};
/* Use the default key and IV specified in the IEEE 802.15.4z attachment */
static struct UWB_STS_KEY_CONFIG_T sts_iv_key = {
@@ -193,10 +199,40 @@
    .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                       */
    .phy_cfg.rx_main_ant = UWB_RX_ANT_3,             /* UWB RX main antenna port                  */
    .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode                       */
    .phy_cfg.pulse_shape = 0x2,                      /* 0x0: CCC pulse, 0x2: FiRa pulse           */
};
#elif defined STS_MODE_SQUARE
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,                  /* 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_main_ant = UWB_RX_ANT_3,             /* UWB RX main antenna port                  */
    .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode                       */
    .phy_cfg.pulse_shape = 0x2,                      /* 0x0: CCC pulse, 0x2: FiRa pulse           */
};
/* 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,
};
#endif

static struct anchor_id_car{
 uint16_t anchor_new_id;
 uint16_t change_num;
@@ -259,7 +295,7 @@
    int64_t tx_timestamp = ranging_tx_time(timestamp);

    // correct antenna delay (TX using the same antenna as RX)
    tx_timestamp += ranging_ant_delays_get(config.phy_cfg.rx_ant_id) / 2;
    tx_timestamp += ranging_ant_delays_get(config.phy_cfg.rx_main_ant) / 2;

    return tx_timestamp;
}
@@ -275,7 +311,7 @@
    int64_t rx_timestamp = ranging_rx_time(ind);

    // correct antenna delay
    rx_timestamp -= ranging_ant_delays_get(config.phy_cfg.rx_ant_id) / 2;
    rx_timestamp -= ranging_ant_delays_get(config.phy_cfg.rx_main_ant) / 2;

    return rx_timestamp;
}
@@ -285,14 +321,21 @@
uint32_t range_timeout_us = 2000000;//yuan5000
uint8_t flag_temp2,flag_temp1;
uint16_t uwb_losttimer;
uint8_t mac_error;
uint8_t resp_tx_flag,poll_rx_error_num;
static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
{      uint8_t valid_sts=0;
    // Power off radio
    power_off_radio();

      sts_lsp_store_stop();
    /** UWB RX success */
    if (rx_report->err_code == UWB_RX_OK)
    {
//          /* Received data does not contain FCS */
//        rx_length = rx_report->pkt_len;
//        memcpy(rx_buf, rx_report->pkt_data, rx_length);
//          if (sts_valid_check())
//        {
        /* Received data does not contain FCS */
        rx_length = rx_report->pkt_len;
        memcpy(rx_buf, rx_report->pkt_data, rx_length);
@@ -303,8 +346,19 @@
        poll_rx_ts_i64 = ranging_rx_time_correct(&rx_rpt);
        poll_rx_num++;
        rssi = rx_report->rssi;
        receive_flag=1;
        //receive_flag=1;
            Anchor_App();
//         //    // PDoA caculation
//         pdoa_3d_calculate(0, &elevation, &azimuth);
//         pdoa_fom_get(NULL, &fom);
//         if(resp_tx_flag==0)
//         {
//           OpenUWB();
//         }
//            }else{
//               poll_rx_error_num++;
//            rx_report->err_code |= UWB_STS_ERR;
//            }
         #ifdef STS_MODE
            valid_sts= sts_valid_check();
      if (valid_sts)
@@ -332,7 +386,7 @@
                 temp_count= phy_timer_count_get();
        memcpy(&rx_rpt, rx_report, sizeof(struct MAC_HW_REPORT_T));
        rx_length = 0;
         //   OpenUWB();//再次开启UWB接收
//            OpenUWB();//再次开启UWB接收
    }
   OpenUWB();//再次开启UWB接收
}
@@ -349,9 +403,12 @@
        temp_count= phy_timer_count_get();
        temp_internal=temp_count;
        resp_tx_num++;
      //      OpenUWB();//再次开启UWB接收
			
//            OpenUWB();//再次开启UWB接收
        //LOG_INFO(TRACE_MODULE_APP, "poll_rx_num is %d,resp_tx_num is %d\r\n",poll_rx_num,resp_tx_num);
    }
    }else{
      mac_error=1;
      }
}
uint32_t start_receive_count,end_receive_count,poll_timeout,current_count,temp_resp;
 uint16_t anchordata_id[TAG_NUM_IN_SYS],anchordata_dist[TAG_NUM_IN_SYS];
@@ -584,7 +641,9 @@
    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大小
      resp_tx_flag=1;//限制重复开启
      tagofflinetime[taglist_pos] = 0;//更新标签通信
      //phy_update_sts_iv_counter(0x00, sts_iv_key.sts_vcounter);//重置sts
//    temp_count1=phy_timer_count_get();
    //while(mac_is_busy());
    //gpio_pin_clr(SCL_PIN);
@@ -841,6 +900,7 @@
{
            uint8_t i;
            uint16_t tempid;
            //phy_update_sts_iv_counter(0x01, 0x00);//接受到后更新sts时间
         if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
        {    
                  flag_recsuccess = 1;
@@ -848,6 +908,17 @@
                  memcpy(&g_com_receive_id,&rx_buf[TAG_ID_IDX],2);
                 //temp_count2=phy_timer_count_get();
                  uwb_losttimer=0;//成功测距清0 lost时间
					
//               #if AOA_EN
//                        // AOA caculation
//                        aoa_calculate(&elevation, &azimuth);
//                        aoa_fom_get(NULL, &fom);
//#elif PDOA_3D_EN
//                        // PDoA caculation
//                        pdoa_3d_calculate(0, &elevation, &azimuth);
//                        pdoa_fom_get(NULL, &fom);
//#endif
					
            frame_seq_nb2 = rx_buf[SEQUENCE_IDX];//获取包序
            battary = rx_buf[BATTARY_IDX];
            rec_nearbase_num=rx_buf[FZ_NEARBASENUM_INDEX];  //标签传过来的他与基站交互的基站id数目
@@ -855,30 +926,6 @@
            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;
//                }
//            }

//            if(i==rec_nearbase_num)
//            {
//                seize_anchor=1;   //抢占anchor
//                Anchor_RecNearPoll(i);
//            }

                        taglist_pos=CmpTagInList(tag_id_recv);
                if(taglist_num>=ANC_MAX_NUM)
@@ -888,7 +935,6 @@
                              memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
                              if(tempid==dev_id)
                              {
                                    if(tag_authorized_List[taglist_pos]==1||bind_flag==1)//授权后才会通信
                                    Anchor_RecNearPoll(i);
                                    tagdist_list[taglist_pos]=rec_ancdistlist[i];
                                    anchordata_bat[taglist_pos] = battary;//保存该基站电量
@@ -905,8 +951,6 @@
                                    {   //tempid==dev_id 说明基站下发测距报文有这个标签的信息
                                          taglist_num++;          //满足上述两种情况才会添加基站ID进入列表中，否则会出现标签不在基站列表中，标签也不响应的情况
                                          tagid_list[taglist_pos] = tag_id_recv;//新增基站
                                          tag_authorized_List[taglist_pos]=2;
                                          find_flag=1;
                                    }
                                    break;
                              }
@@ -917,7 +961,9 @@
                           Anchor_RecNearPoll(rec_nearbase_num);
                }
            }
				
//             //phy_update_sts_iv_counter(0x00, sts_iv_key.sts_vcounter);//新增sts
//             LOG_INFO(TRACE_MODULE_APP, "PDoA Azimuth %d Elevation %d FoM %u\r\n", mk_q7_to_s16(azimuth),
//                                 mk_q7_to_s16(elevation), fom);
            
}
int UwbRange(void)
@@ -1055,53 +1101,105 @@
         
         return 0;
}
void Uwb_init(void)
void Uwb_init(void)//uwbinit修改了
{
      uwb_open();
   #ifdef STS_MODE
 // Set STS key and IV
    phy_sts_key_configure(&sts_iv_key);
	
   // which RX ports will be used for AoA/PDoA
    phy_rx_ant_mode_set(RX_ANT_PORTS_COMBINATION);
   #endif
    // Set calibration parameters
    uwb_calibration_params_set(config.phy_cfg.ch_num);
   #ifndef STS_MODE
   // set advanced parameters
    struct PHY_ADV_CONFIG_T adv_config = {

    // set advanced parameters
    struct PHY_ADV_CONFIG_T adv_config =
    {
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .nth_scale_factor = 1,
        .ranging_performance_mode = 0,
#if RX_ANT_PORTS_NUM == 4
        .skip_weakest_port_en = 1,
#else
        .skip_weakest_port_en = 0,
#endif
    };
   #else
    // 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,
    };
   #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);
      #ifdef STS_MODE
      aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
    aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));

    aoa_param_config();
		
      phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
      #endif
      mac_register_process_handler(tx_int_callback, rx_int_callback);//初始化回调
    ranging_lib_init(config.phy_cfg.sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM);

    // Register rx interrupt callback function
    mac_register_process_handler(tx_int_callback, rx_int_callback);

    // Reduce inter frame spacing (IFS) for non encrypted packets
    mac_ifs_set(0, 0);
}
#if PDOA_3D_EN
#define PDOA_3D_SUPPORT_NUM 10
#define PDOA_3D_TIMEOUT_MS 2000
static struct PDOA_3D_MAC_ADDR_T mac_addr_cache[PDOA_3D_SUPPORT_NUM];
static struct PDOA_3D_PDOA_DATA_T pdoa_data_cache[PDOA_3D_SUPPORT_NUM];
#endif
void Anchor_uwb_aoa_square_init(void)
{
     uwb_open();
    // Set calibration parameters
    uwb_calibration_params_set(config.phy_cfg.ch_num);

    // set advanced parameters
    struct PHY_ADV_CONFIG_T adv_config =
    {
        .thres_fap_detect = 40,
        .nth_scale_factor = 1,
        .ranging_performance_mode = 0,
#if RX_ANT_PORTS_NUM == 4
        .skip_weakest_port_en = 1,
#else
        .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);

    ranging_lib_init(config.phy_cfg.sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM);

    // Register rx interrupt callback function
    mac_register_process_handler(tx_int_callback, rx_int_callback);

    // Reduce inter frame spacing (IFS) for non encrypted packets
    mac_ifs_set(0, 0);

#if (ANT_PATTERN == ANT_PATTERN_SQUARE)
    angle_span_t aoa_span;
    aoa_span.Ndim = 2;
    aoa_span.el_low = 90;
    aoa_span.el_high = 90;
    aoa_span.el_step = 1;
    aoa_span.az_low = 0;
    aoa_span.az_high = 359;
    aoa_span.az_step = 1;
    aoa_angle_search_span_set(&aoa_span);
#endif

#if AOA_EN
    sts_param_config(config.phy_cfg.sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM, STS_BUF_NUM, STS_BUF_SIZE);
    aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
#else
    sts_param_config(config.phy_cfg.sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI, STS_BUF_NUM, STS_BUF_SIZE);
#endif

#if PDOA_3D_EN
    pdoa_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_NONE, mac_addr_cache, pdoa_data_cache, PDOA_3D_SUPPORT_NUM, PDOA_3D_TIMEOUT_MS);
    // pdoa_angle_reverse_set(1, 0);
#endif

    // Set STS key and IV
    phy_sts_key_configure(&sts_iv_key);

    phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
}


//主函数绑定接受逻辑
int UwbSearch(void)
{
@@ -1145,8 +1243,8 @@
      #ifdef STS_MODE
      ranging_lib_init();
      #endif
    ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
		
    //ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);//新版本修改
         ranging_lib_init(config.phy_cfg.sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM);
      #ifdef STS_MODE
      aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
    aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));