From edee1ca562c68dff166682848afa762be6783748 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期二, 08 七月 2025 16:52:07 +0800
Subject: [PATCH] 成功加入滤波算法，将角度波动控制在2-4度左右
---
keil/uwb_app.c | 67 ++++++++++++++++++++++++++++++---
1 files changed, 61 insertions(+), 6 deletions(-)
diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index a72330d..52f9718 100644
--- a/keil/uwb_app.c
+++ b/keil/uwb_app.c
@@ -10,6 +10,8 @@
#include "global_param.h"
#include "board.h"
#include "lib_aoa.h"
+#include "lib_kf.h"
+#include "MK8000_kf_top.h"
#if PDOA_3D_EN
#include "lib_pdoa_3d.h"
#endif
@@ -26,6 +28,7 @@
int Anchor_App(void);
extern void IO_LED_control_change(uint8_t data);
extern void IO_control_init(void);
+extern uint8_t loc_kf_filter(float data_meas, enum KF_DATA_TYPE_T data_type, uint8_t *mac_addr, float *data_post);
extern void updata_led_power_state(void);
/*receive buffer*/
@@ -213,7 +216,7 @@
#elif defined STS_MODE_SQUARE
static struct mk_uwb_configure config = {
.phy_work_mode = (uint8_t)(PHY_TX | PHY_RX),
- .phy_cfg.ch_num = 9, /* Channel number. */
+ .phy_cfg.ch_num = 5, /* Channel number. */
.phy_cfg.code_index = 9, /* TRX preamble code */
.phy_cfg.mean_prf = MEAN_PRF_64M, /* Mean prf 64/128/256M */
.phy_cfg.data_bit_rate = DATA_BR_6M8, /* Data rate 6.8M */
@@ -332,6 +335,43 @@
uint8_t allow_flag;
uint8_t resp_tx_flag,poll_rx_error_num;
uint8_t tag_rssi[TAG_NUM_IN_SYS];
+uint8_t tag_show_location[TAG_NUM_IN_SYS];
+void construct_show_buffer(uint8_t*tag_show_location,uint32_t tag_pos)
+{
+//if(angle_azimuth[tag_pos])
+//{
+//
+//}elseif(angle_azimuth[tag_pos])
+//{
+
+//}elseif(angle_azimuth[tag_pos])
+//{
+
+//}elseif(angle_azimuth[tag_pos])
+//{
+
+//}
+}
+float temp1,temp2;
+static void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type)
+{
+ if (angle == NULL)
+ {
+ return;
+ }
+
+ float post_angle;
+ float angle_meas = mk_q7_to_f32(*angle);
+ temp1=angle_meas;
+
+ // call filter
+ loc_kf_filter(angle_meas, (enum KF_DATA_TYPE_T)type, mac_addr, &post_angle);
+ temp2=post_angle;
+ // update angle
+ *angle = mk_f32_to_q7(post_angle);
+ temp1=*angle;
+ // LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%d %d;\r\n", READ_SHORT(mac_addr), (int16_t)angle_meas,(int16_t)post_angle);
+}
static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
{ uint8_t valid_sts=0;
// Power off radio
@@ -363,11 +403,20 @@
pdoa_fom_get(NULL, &fom);
if(allow_flag)
{
- tag_rssi[taglist_pos]=rssi;
+ tag_rssi[taglist_pos]=rssi;
+ uint8_t mac_addr=0;
+ angle_result_filter(&mac_addr,&azimuth, KF_DATA_TYPE_AZIMUTH);
+ angle_result_filter(&mac_addr,&elevation, KF_DATA_TYPE_ELEVATION);
angle_azimuth[taglist_pos]=(uint16_t)mk_q7_to_s16(azimuth);
angle_elevation[taglist_pos]=(uint16_t)mk_q7_to_s16(elevation);
- buffer_message_send(taglist_pos,0,0,0);
- allow_flag=0;
+ //buffer_message_send(taglist_pos,0,0,0);
+ //int16_t azimth=mk_q7_to_s16(azimuth),elevate=mk_q7_to_s16(elevation);
+
+// // filter process
+
+ LOG_INFO(TRACE_MODULE_APP, "PDoA 0x%04x Azimuth %d Elevation %d FoM %u\r\n",tag_id_recv, mk_q7_to_s16(azimuth),
+ mk_q7_to_s16(elevation), fom);
+ allow_flag=0;
}
}
if(resp_tx_flag==0)
@@ -1027,8 +1076,7 @@
}
}
- LOG_INFO(TRACE_MODULE_APP, "PDoA 0x%04x Azimuth %d Elevation %d FoM %u\r\n",tag_id_recv, mk_q7_to_s16(azimuth),
- mk_q7_to_s16(elevation), fom);
+
}
int UwbRange(void)
{
@@ -1202,6 +1250,11 @@
static struct PDOA_3D_MAC_ADDR_T mac_addr_cache[PDOA_3D_SUPPORT_NUM];
static struct PDOA_3D_PDOA_DATA_T pdoa_data_cache[PDOA_3D_SUPPORT_NUM];
#endif
+#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 Anchor_uwb_aoa_square_init(void)
{
uwb_open();
@@ -1261,6 +1314,8 @@
phy_sts_key_configure(&sts_iv_key);
phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+ //初始化滤波空间
+ loc_post_kf_config(1000, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM, KF_TIMEOUT_MS);
}
--
Gitblit v1.9.3