From 44812eee65f5e9eb47c2b01b6cf86c101083eb61 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期五, 25 七月 2025 17:41:24 +0800
Subject: [PATCH] 修改激活对应com表标志位位置，避免与其他元素耦合
---
keil/uwb_app.c | 91 ++++++++++++++++++++++++++++++++++++++-------
1 files changed, 76 insertions(+), 15 deletions(-)
diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index 78c6121..a23570b 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);
@@ -32,7 +32,7 @@
static uint8_t frame_seq_nb2,battary,button,rec_nearbase_num,ancidlist_num;
static uint16_t ancidlist_rec[TAG_NUM_IN_SYS],ancidlist_send[TAG_NUM_IN_SYS],rec_ancidlist[TAG_NUM_IN_SYS];
static int16_t rec_ancdistlist[TAG_NUM_IN_SYS];
-extern uint8_t group_id;
+extern uint16_t group_id;
static uint16_t anc_id_recv,tag_id_recv;
static int16_t rec_antdelay;
extern uint32_t dev_id;
@@ -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]);
@@ -280,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;
}
@@ -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;
}
@@ -371,7 +384,6 @@
temp_count= phy_timer_count_get();
memcpy(&rx_rpt, rx_report, sizeof(struct MAC_HW_REPORT_T));
rx_length = 0;
-
}
}
@@ -485,11 +497,12 @@
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;
- memcpy(&send_buffer[GROUP_ID_IDX],&group_id,1);//组id
+ memcpy(&send_buffer[GROUP_ID_IDX],&group_id,2);//组id变为2字节
memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);//自身id
memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);//接受者id
send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
@@ -519,6 +532,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());
}
@@ -816,7 +830,24 @@
}
}
+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
+}
+uint16_t temp_group_id;
int UwbRange(void)
{
uint8_t i;
@@ -875,8 +906,9 @@
{
gpio_pin_clr(SCL_PIN);
//if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL&&!memcmp(&rx_buf[TAG_ID_IDX],&g_com_map[BIND_DEV_ID],2))//判断是否是和自己是同一组通讯的且为poll包
- if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
+ if(memcmp(&group_id,&rx_buf[GROUP_ID_IDX],2)==0&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
{ flag_recsuccess = 1;
+
memcpy(&g_com_receive_id,&rx_buf[TAG_ID_IDX],2);
// sleep_timer_start(__MS_TO_32K_CNT(UWB_DELAY_TIME_US));//测试
temp_count2=phy_timer_count_get();
@@ -895,6 +927,14 @@
PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);
}
}
+ if(g_com_map[BIND_START_INDEX])//如果开启配对
+ {
+ if(distance<=200)//小于200m的才可配对
+ {
+ memcpy(&temp_group_id,&rx_buf[BIND_GROUP],2);
+
+ }
+ }
for(i=0; i<rec_nearbase_num; i++)
{
memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
@@ -917,13 +957,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;
@@ -937,11 +983,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++;
@@ -972,6 +1026,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 +1076,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