修改PA引脚为FUN6，正确驱动PA，修改为官方demo算法,线性度需要优化

chen

2025-07-15 945f0cc0210d2ea3339870f0e38d9be72314c4e2

 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);
@@ -284,9 +284,9 @@
angle =angle*57.3;
//offset=10;
//angle+=offset;
if(angle>=90)
while(angle>=90)
        angle-=180;
else if(angle<=-90)
while(angle<=-90)
{
angle+=180;
}
@@ -498,6 +498,7 @@


uint8_t flag_temp2,flag_temp1;
uint8_t tx_num;
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos)//根据自己是否为新基站定制消息去发送，根据是否抢占判断
{
    temp_count2=poll_rx_en_start_u32;
@@ -532,6 +533,7 @@
    temp_count3= phy_timer_count_get();
    flag_temp2=uwb_tx(send_buffer, 60,1 ,resp_tx_en_start_u32);//立即发送测试size大小
    temp_count1=phy_timer_count_get();
      tx_num++;
    while(mac_is_busy());
    
}
@@ -829,7 +831,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;
@@ -930,13 +948,19 @@
      {
            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();
               sts_rssi = sts_rssi_output_get();
               //angle_temp=angle_calculate();
            angle_temp=-mk_q7_to_s16(azimuth);
               //sts_rssi = sts_rssi_output_get();
         }
      
                  recev_error_num=0;
@@ -950,11 +974,19 @@
         CalibratePdOffset(angle_temp);
         }else{
         angle_temp+=pd_offset;
         buffer_construct(tag_id_recv,distance,(int16_t)angle_temp,rssi);
         //uart_send(TRACE_PORT_UART0,usart_send,10+17,NULL);
         buffer_log_send(tag_id_recv,distance,(int16_t)angle_temp);
         }
         
			
//         while(angle_temp>=90)
//         angle_temp-=180;
//         while(angle_temp<=-90)
//         {
//         angle_temp+=180;
//         }
         buffer_construct(tag_id_recv,distance,(int16_t)angle_temp,rssi);
                  uart_send(TRACE_PORT_UART0,usart_send,10+17,NULL);
         //buffer_log_send(tag_id_recv,distance,(int16_t)angle_temp);
                  }

         //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++;
@@ -985,6 +1017,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();
@@ -1030,6 +1067,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);
}

//主函数绑定接受逻辑