修改为AOA跟随模块测试基站，成功输出角度和距离一直测距版本

chen

2025-05-06 fef76b2f74d65ae80a890bbc8ce2be8e591cdd03

 keil/uwb_app.c

@@ -69,7 +69,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 4000U //yuan670 with urt 3000success
#define POLL_RX_TO_RESP_TX_DLY_US 2000U //yuan670 with urt 3000success

#define RESP_TX_TO_FINAL_RX_DLY_US 500U

@@ -253,8 +253,71 @@

/* RX done process handler. */
int8_t rssi;
#define MSG_POS          0x01
#define M_PI      3.1415927
uint8_t usart_send[40];
extern uint8_t bat_percent;
double angle_temp;
double angle_calculate(void)
{
 double offset; 
// float pdoa[3];
// pdoa[0] = pdoa_select_get(0, 3);
// pdoa[1] = pdoa_select_get(1, 3);
// pdoa[2] = pdoa_select_get(2, 3);
//LOG_INFO(TRACE_MODULE_APP, "PDOA:%f,%f,%f\r\n", pdoa[0], pdoa[1], pdoa[2]);
 float *iq1 = sts_first_path_iq_get();
//LOG_INFO(TRACE_MODULE_APP, "ANT0 IQ: %f %f\r\n", iq1[0], iq1[1]);
//LOG_INFO(TRACE_MODULE_APP, "ANT1 IQ: %f %f\r\n", iq1[2], iq1[3]);
//LOG_INFO(TRACE_MODULE_APP, "ANT2 IQ: %f %f\r\n", iq1[4], iq1[5]);
//LOG_INFO(TRACE_MODULE_APP, "ANT3 IQ: %f %f\r\n", iq1[6], iq1[7]);
float iq_re = iq1[2 * 0] * iq1[2 * 1] + iq1[2 * 0 + 1] * iq1[2 * 1 + 1];
float iq_im = iq1[2 * 0] * iq1[2 * 1 + 1] - iq1[2 * 0 + 1] * iq1[2 * 1];
double result=atan2f(iq_im, iq_re)/ M_PI;
double angle=asin(result);
angle =angle*57.3;
offset=30;
angle+=offset;
if(angle>=90)
        angle-=180;
else if(angle<=-90)
{
angle+=180;
}
//LOG_INFO(TRACE_MODULE_APP, "ANGLE_X: %lf \r\n", angle);
float iq_re2 = iq1[2 * 2] * iq1[2 * 3] + iq1[2 * 2 + 1] * iq1[2 * 3 + 1];
float iq_im2 = iq1[2 * 2] * iq1[2 * 3 + 1] - iq1[2 * 2 + 1] * iq1[2 * 3];
double result2=atan2f(iq_im2, iq_re2)/ M_PI;
double angle2=asin(result2);
angle2 =angle2*57.3;
if(angle2>=90)
        angle2-=180;
else if(angle2<=-90)
{
angle2+=180;
}
return angle;
}
void buffer_construct(uint16_t tag_id,uint16_t distance_temp,int16_t angle1,uint8_t rssi1)
{
        uint16_t checksum = 0;
        usart_send[0] = 0x55;
        usart_send[1] = 0xAA;
        usart_send[2] = 17+10;                        //length
        usart_send[3] = MSG_POS;                        //MSG_TYPE
        memcpy(&usart_send[4],&tag_id,2);                //tag_id)
        memcpy(&usart_send[6],&distance_temp,2);
        memcpy(&usart_send[8],&angle1,2);
        usart_send[10] = (uint8_t)rssi1;
        usart_send[11] = 0;  //BUTTON目前先不写
        usart_send[12] = bat_percent;         //BATTARY目前先不写
        usart_send[13] = 0;  //RESERVE1
        usart_send[14] = 0;  //RESERVE2
        checksum = Checksum_u16(&usart_send[2], 13+10);
        memcpy(&usart_send[15+10], &checksum, 2);
}
static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
{      uint8_t valid_sts=0;
{		
    // Power off radio
    power_off_radio();

@@ -272,19 +335,7 @@
        poll_rx_num++;
        rssi = rx_report->rssi;
        receive_flag=1;
            valid_sts= sts_valid_check();
      if (valid_sts)
      {
            aoa_calculate(&elevation, &azimuth);
            aoa_fom_get(NULL, &fom);
//            float pdoa[3];
//            pdoa[0] = pdoa_select_get(0, 3);
//            pdoa[1] = pdoa_select_get(1, 3);
//            pdoa[2] = pdoa_select_get(2, 3);
//            LOG_INFO(TRACE_MODULE_APP, "PDOA: %f %f %f\r\n", pdoa[0], pdoa[1], pdoa[2]);

               sts_rssi = sts_rssi_output_get();
         }
				
    }
    else
    {
@@ -432,7 +483,7 @@
//       resp_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(20*rec_nearbase_num+POLL_RX_TO_RESP_TX_DLY_US+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS);//后面的需要根据已有基站数量进行更改，如果是抢占自己最后一个回复，要有底数
//            //这里应该有问题问一下钟工
//      }//此处设置绝对时间将poll u32改为phy_timer_count_get()
      resp_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US);
      resp_tx_en_start_u32 = poll_rx_en_start_u32+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US);
    resp_tx_ts_i64 = ranging_tx_time_correct(resp_tx_en_start_u32 + phy_shr_duration());//修正时间戳
    /* Write all timestamps in the final message. See NOTE 8 below. */
    resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts_i64);
@@ -443,9 +494,9 @@
    flag_temp2=uwb_tx(send_buffer, 40,1 ,resp_tx_en_start_u32);//立即发送测试size大小
    temp_count1=phy_timer_count_get();
    while(mac_is_busy());
    gpio_pin_clr(SCL_PIN);
    
}
uint32_t range_timeout_us = 5000;//yuan5000
uint32_t range_timeout_us = 1000000;//yuan5000
uint16_t uwb_searchcount;
uint8_t flag_recsuccess;

@@ -506,17 +557,20 @@
    anchordata_num=j;
      
}
int success_num=0;
void in_table_log(void)
{
   uint32_t u32LogLen,datalenth;
   char acReadponse[200];
   u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "车内：%d 人 ID:",get_in_num,get_out_num);
   //u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "车内：%d 人 测距成功数：%d ID: ",get_in_num,success_num);
   u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "门内：%d 人 ID: ",get_in_num);
               for(uint16_t i=0;i<get_in_num;i++)
            {
                datalenth = sprintf((char*)&acReadponse[u32LogLen],",%04X",anchor_id_in[i]);
                datalenth = sprintf((char*)&acReadponse[u32LogLen],"%04X,",anchor_id_in[i]);
                u32LogLen += datalenth;
            }
      LOG_INFO(TRACE_MODULE_APP,"%s\r\n",acReadponse);
                  success_num=0;
}
void TagListUpdate_person_num_car(void)
{
@@ -652,28 +706,36 @@
   if(rssi_ant0>rssi_ant2&&distance_from_tag<200)
   {
      change_count[position_anchor_exist]++;
      if(change_count[position_anchor_exist]>0)
      {
      position_anchor_in_table=CmpCarInTable(receive_success_id);
      add_in_car_table(position_anchor_in_table,receive_success_id);
      }
      if(change_count[position_anchor_exist]>=3)
      {   change_count[position_anchor_exist]=3;
         position_anchor_in_table=CmpCarInTable(receive_success_id);
         add_in_car_table(position_anchor_in_table,receive_success_id);
			
      }
   }else if(rssi_ant0<rssi_ant2&&distance_from_tag<200)
   {
      change_count[position_anchor_exist]--;
      if(change_count[position_anchor_exist]<=-3)
      {change_count[position_anchor_exist]=-3;
      if(change_count[position_anchor_exist]<0)
      {
      position_anchor_in_table=CmpCarInTable(receive_success_id);
      delete_in_car_table(position_anchor_in_table);
      }
      if(change_count[position_anchor_exist]<=-3)
      {change_count[position_anchor_exist]=-3;
      }   
   }
}

int UwbRange(void)
{
    uint8_t i;
    uint16_t tempid;
    // The following peripherals will be initialized in the uwb_open function
    // phy/mac/aes/lsp/phy timers initialized
    uwb_open();
  

//    // Set calibration parameters
//    uwb_calibration_params_set(config.phy_cfg.ch_num);
@@ -687,7 +749,7 @@
//    };
//    phy_adv_params_configure(&adv_config);

//    // uwb configure
//    // uwb configure                                                        
//    uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);

//    ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
@@ -700,6 +762,7 @@
    temp_count3=phy_timer_count_get();
    gpio_pin_set(SCL_PIN);
      sts_lsp_store();
		
    flag_temp1=uwb_rx(0, 0, range_timeout_us);//开启接收
   
   // while(mac_is_busy());
@@ -710,25 +773,19 @@
   if(end_receive_count>=UINT32_MAX)
   {end_receive_count-=UINT32_MAX;}
   current_count=phy_timer_count_get();
      while(current_count<end_receive_count||current_count>end_receive_count+HALF_SECOND_TIME)//循环接受包体,若为124.8K则是+62400000
      {

         current_count=phy_timer_count_get();
         while(mac_is_busy())
        {
                  Calibration_Time();
                  //Calibration_Time();
            IdleTask();
            current_count=phy_timer_count_get();
            if(current_count>end_receive_count&&current_count<end_receive_count+HALF_SECOND_TIME)
            {
                    break;
            }
        }
         sts_lsp_store_stop();
         
         
    if(receive_flag==1)//成功接收
    {

      gpio_pin_clr(SCL_PIN);
        //if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL&&!memcmp(&rx_buf[TAG_ID_IDX],&g_com_map[BIND_DEV_ID],2))//判断是否是和自己是同一组通讯的且为poll包
         if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
        {    flag_recsuccess = 1;
@@ -766,25 +823,40 @@
                seize_anchor=1;   //抢占anchor
                Anchor_RecNearPoll(i);
            }
                  uint8_t valid_sts=0;
                  valid_sts= sts_valid_check();
      if (valid_sts)
      {
            aoa_calculate(&elevation, &azimuth);
            aoa_fom_get(NULL, &fom);
//            float pdoa[3];
//            pdoa[0] = pdoa_select_get(0, 3);
//            pdoa[1] = pdoa_select_get(1, 3);
//            pdoa[2] = pdoa_select_get(2, 3);
//            LOG_INFO(TRACE_MODULE_APP, "PDOA: %f %f %f\r\n", pdoa[0], pdoa[1], pdoa[2]);
               angle_temp=angle_calculate();
               sts_rssi = sts_rssi_output_get();
         }
       buffer_construct(tag_id_recv,distance,(int16_t)angle_temp,rssi);
                  recev_error_num=0;
              //range_timeout_us=5000;//恢复为5000进入range后
           LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 信号强度ANT0: %f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);
                check_if_in_or_out_car(g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);
           //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 包序%d\r\n",g_com_receive_id,distance,frame_seq_nb2);
                  //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 信号强度ANT0:%f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);
                  //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 信号强度ANT0:%f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[1],sts_rssi[2]);
                  //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d\r\n",g_com_receive_id,distance);
         uart_send(TRACE_PORT_UART0,usart_send,10+17,NULL);
         //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 角度 :%lf\r\n",g_com_receive_id,distance,angle_temp);
                //check_if_in_or_out_car(g_com_receive_id,distance,sts_rssi[1],sts_rssi[2]);
               success_num++;
               
					
                  gpio_pin_clr(SCL_PIN);
                  //gpio_pin_clr(SCL_PIN);
        //break;去掉break变为一对多
            }
    } 
         sts_lsp_store();
			
         break;
      //失败或者接受被高发射机打断都会再次开启接收
         //flag_temp1=uwb_rx(0, 0, range_timeout_us);
   }
      delay_us(1);
      sts_lsp_store_stop();
      uwb_rx_force_off(1);
//         sts_lsp_store();
         gpio_pin_clr(SCL_PIN);
//      sts_lsp_store_stop();
//      uwb_rx_force_off(1);

//      if(!flag_recsuccess)
//      {
@@ -805,37 +877,51 @@
}
void Uwb_init(void)
{
     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 = {
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .ranging_performance_mode = 0,
        .skip_weakest_port_en = 0,
    };
   #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);
		
      #ifdef STS_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
}

//主函数绑定接受逻辑
int UwbSearch(void)
{
@@ -844,36 +930,51 @@
    // The following peripherals will be initialized in the uwb_open function
    // phy/mac/aes/lsp/phy timers initialized
    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 = {
        .thres_fap_detect = 40,
        .nth_scale_factor = 4,
        .ranging_performance_mode = 0,
        .skip_weakest_port_en = 0,
    };
      #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);
      
      #ifdef STS_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
    // Register rx interrupt callback function
    mac_register_process_handler(tx_int_callback, rx_int_callback);
      
@@ -973,9 +1074,9 @@
    case LINK_SUCCESS:
    {   //连接成功进行轮询测距

        uwb_led_on();
        //uwb_led_on();
        UwbRange();
        uwb_led_off();
        //uwb_led_off();

    }
    break;