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

chen

2025-05-06 fef76b2f74d65ae80a890bbc8ce2be8e591cdd03

 keil/uwb_app.c

@@ -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
    {
@@ -772,13 +823,30 @@
                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 包序%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);
                check_if_in_or_out_car(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);
@@ -853,6 +921,7 @@
      phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
      #endif
}

//主函数绑定接受逻辑
int UwbSearch(void)
{