增加官方滤波和计算aoa角度打印函数，距离远角度抖动未解决

chen

2025-07-10 039c3029bf18daf88b546d913e488a6ae33e310b

 keil/uwb_app.c

@@ -11,7 +11,7 @@
#include "board.h"
#include "CommMap.h"
#include "lib_aoa.h"

#include "lib_kf.h"
extern int simple_main(void);
extern int temp_main(void);
void Calibration_Time(void);
@@ -62,6 +62,7 @@
int32_t distance;
uint8_t taglist_num;
float *sts_rssi=NULL;
float *sts_rssi1=NULL;
extern uint8_t recev_error_num;

extern Operation_step UWB_work_state;
@@ -260,6 +261,8 @@
uint8_t usart_send1[40];
extern uint8_t bat_percent;
double angle_temp;
float sts_first_rssi_result[4];
float rssi_result[4];
double angle_calculate(void)
{
 double offset; 
@@ -269,6 +272,7 @@
// 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]);
@@ -298,8 +302,15 @@
//{
//angle2+=180;
//}

//计算端口0和3 rssi和STS 第一路径rssi
sts_first_rssi_result[0]=iq1[0]*iq1[0]+iq1[1]*iq1[1];//计算天线0的sts rssi
sts_first_rssi_result[1]=iq1[6]*iq1[6]+iq1[7]*iq1[7];//计算天线3的sts rssi
rssi_result[0]=20 * log10(sts_first_rssi_result[0] / sts_first_rssi_result[1]) + rssi;//根据相对rssi计算天线0的rssi
rssi_result[1]=rssi;
return angle;
}
int8_t snr_result[2];
void buffer_construct(uint16_t tag_id,uint16_t distance_temp,int16_t angle1,uint8_t rssi1)
{
        uint16_t checksum = 0;
@@ -313,7 +324,7 @@
        usart_send[10] = (uint8_t)rssi1;
        usart_send[11] = 0;  //BUTTON目前先不写
        usart_send[12] = bat_percent;         //BATTARY目前先不写
        usart_send[13] = 0;  //RESERVE1
        usart_send[13] = (uint8_t)snr_result[1];  //RESERVE1
        usart_send[14] = 0;  //RESERVE2
        checksum = Checksum_u16(&usart_send[2], 13+10);
        memcpy(&usart_send[15+10], &checksum, 2);
@@ -321,9 +332,10 @@
 char log_buf[300];
 uint32_t u32LogLen1,dataloglenth;
 uint32_t cnt_log;

void buffer_log_send(uint16_t tag_id,uint16_t distance_temp,int16_t angle1)
{
   dataloglenth = snprintf((char*)&log_buf,sizeof(log_buf), "DATA,%x,%d,%d,%d\n",tag_id,cnt_log++,angle1,distance_temp);
   dataloglenth = snprintf((char*)&log_buf,sizeof(log_buf), "DATA,%x,%d,%d,%d,%d,%d\n",tag_id,cnt_log++,angle1,distance_temp,(int32_t)rssi_result[1],snr_result[1]);
   uart_send(TRACE_PORT_UART0,log_buf,dataloglenth,NULL);
}
void buffer_construct_user(uint16_t tag_id)
@@ -357,6 +369,7 @@
        poll_rx_ts_i64 = ranging_rx_time_correct(&rx_rpt);
        poll_rx_num++;
        rssi = rx_report->rssi;
            snr_result[1]=rx_report->snr;
        receive_flag=1;
            
    }
@@ -816,7 +829,23 @@
   }

}
void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type)
{
    if (angle == NULL)
    {
        return;
    }

#if KF_EN
    float post_angle;
    float angle_meas = mk_q7_to_f32(*angle);
    // call filter
    loc_kf_filter(angle_meas, (enum KF_DATA_TYPE_T)type, mac_addr, &post_angle);
    // update angle
    *angle = mk_f32_to_q7(post_angle);
    // LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%d %d;\r\n", READ_SHORT(mac_addr), (int16_t)angle_meas,(int16_t)post_angle);
#endif
}
int UwbRange(void)
{
    uint8_t i;
@@ -917,12 +946,18 @@
      {
            aoa_calculate(&elevation, &azimuth);
            aoa_fom_get(NULL, &fom);
            angle_result_filter(NULL, &azimuth, KF_DATA_TYPE_AZIMUTH);
    
//             LOG_INFO(TRACE_MODULE_APP, "Azimuth %d Elevation %d Azimuth FoM %d\r\n",
//                        mk_q7_to_s16(azimuth), mk_q7_to_s16(elevation),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();
               //angle_temp=angle_calculate();
            angle_temp=-mk_q7_to_s16(azimuth);
               sts_rssi = sts_rssi_output_get();
         }
      
@@ -972,6 +1007,11 @@
         
         return 0;
}
#define KF_SUPPORT_NUM 3
#define KF_TIMEOUT_MS 2000
static struct KF_MAC_ADDR_T kf_mac_addr_cache[KF_SUPPORT_NUM];
static struct KF_CHANNEL_CACHE_T kf_channel_cache[KF_SUPPORT_NUM];
static struct KF_MAT_VALUE_CACHE_T kf_mat_value_cache[KF_SUPPORT_NUM];
void Uwb_init(void)
{
     uwb_open();
@@ -1017,6 +1057,8 @@
      
      phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
      #endif
       loc_post_kf_config(100, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM,
                           KF_TIMEOUT_MS);
}

//主函数绑定接受逻辑