From 039c3029bf18daf88b546d913e488a6ae33e310b Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期四, 10 七月 2025 18:10:54 +0800
Subject: [PATCH] 增加官方滤波和计算aoa角度打印函数，距离远角度抖动未解决
---
keil/uwb_app.c | 37 +++++++++++++++++++++++++++++++++----
1 files changed, 33 insertions(+), 4 deletions(-)
diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index a2bdf68..641255b 100644
--- a/keil/uwb_app.c
+++ b/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);
@@ -829,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;
@@ -930,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();
}
@@ -951,8 +973,8 @@
}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);
+ 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);
@@ -985,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();
@@ -1030,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);
}
//主函数绑定接受逻辑
--
Gitblit v1.9.3