From c6bf7d2728868250e6b219a55bdc7bf4245b8d7c Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期二, 27 五月 2025 14:47:34 +0800
Subject: [PATCH] 加入snr打印逻辑
---
keil/uwb_app.c | 220 ++++++++++++++++++++++++++++++++++++++++++++++++++-----
1 files changed, 200 insertions(+), 20 deletions(-)
diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index cbfdfb7..a2bdf68 100644
--- a/keil/uwb_app.c
+++ b/keil/uwb_app.c
@@ -9,6 +9,7 @@
#include "dw_app_anchor.h"
#include "global_param.h"
#include "board.h"
+#include "CommMap.h"
#include "lib_aoa.h"
extern int simple_main(void);
@@ -61,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;
@@ -99,7 +101,7 @@
#define HALF_SECOND_TIME 624000000
/* Length of the common part of the message */
#define MSG_COMMON_LEN 10
-
+#define calib_len 60
#define UWB_DELAY_TIME_US 496
static uint8_t receive_flag=0;
struct mk_uwb_configure
@@ -253,8 +255,104 @@
/* RX done process handler. */
int8_t rssi;
+#define MSG_POS 0x01
+#define M_PI 3.1415927
+uint8_t usart_send[40];
+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;
+// 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]);
+ 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]);
+//LOG_INFO(TRACE_MODULE_APP, "ANT3 IQ: %f %f\r\n", iq1[6], iq1[7]);
+float iq_re = iq1[2 * 0] * iq1[2 * 3] + iq1[2 * 0 + 1] * iq1[2 * 3 + 1];
+float iq_im = iq1[2 * 0] * iq1[2 * 3 + 1] - iq1[2 * 0 + 1] * iq1[2 * 3];
+double result=atan2f(iq_im, iq_re)/ M_PI;
+double angle=asin(result);
+angle =angle*57.3;
+//offset=10;
+//angle+=offset;
+if(angle>=90)
+ angle-=180;
+else if(angle<=-90)
+{
+angle+=180;
+}
+//LOG_INFO(TRACE_MODULE_APP, "ANGLE_X: %lf \r\n", angle);
+//float iq_re2 = iq1[2 * 2] * iq1[2 * 3] + iq1[2 * 2 + 1] * iq1[2 * 3 + 1];
+//float iq_im2 = iq1[2 * 2] * iq1[2 * 3 + 1] - iq1[2 * 2 + 1] * iq1[2 * 3];
+//double result2=atan2f(iq_im2, iq_re2)/ M_PI;
+//double angle2=asin(result2);
+//angle2 =angle2*57.3;
+//if(angle2>=90)
+// angle2-=180;
+//else if(angle2<=-90)
+//{
+//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;
+ usart_send[0] = 0x55;
+ usart_send[1] = 0xAA;
+ usart_send[2] = 17+10; //length
+ usart_send[3] = MSG_POS; //MSG_TYPE
+ memcpy(&usart_send[4],&tag_id,2); //tag_id)
+ memcpy(&usart_send[6],&distance_temp,2);
+ memcpy(&usart_send[8],&angle1,2);
+ usart_send[10] = (uint8_t)rssi1;
+ usart_send[11] = 0; //BUTTON目前先不写
+ usart_send[12] = bat_percent; //BATTARY目前先不写
+ 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);
+}
+ 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,%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)
+{
+ uint16_t checksum = 0;
+ usart_send1[0] = 0x55;
+ usart_send1[1] = 0xAA;
+ usart_send1[2] = 0x0d; //length
+ usart_send1[3] = 5+10; //MSG_TYPE
+ memcpy(&usart_send1[4],&tag_id,2); //tag_id)
+ usart_send1[6] = 10;
+ //用户数据段
+ checksum = Checksum_u16(&usart_send1[2], 5+10);
+ memcpy(&usart_send1[7+10], &checksum, 2);
+}
static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
-{ uint8_t valid_sts=0;
+{
// Power off radio
power_off_radio();
@@ -271,20 +369,9 @@
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;
- valid_sts= sts_valid_check();
- if (valid_sts)
- {
- aoa_calculate(&elevation, &azimuth);
- aoa_fom_get(NULL, &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]);
-
- sts_rssi = sts_rssi_output_get();
- }
+
}
else
{
@@ -438,9 +525,12 @@
resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts_i64);
resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts_i64);//此处时间戳int64直接转换为uint64不知道会不会有错误
memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&disoffset,2);//差个修正offset,修正有符号但是这个com表为无符号的,传过去直接赋给Int16_t相当于还原了
+ //放入用户控制参数
+ buffer_construct_user(tag_id_recv);
+ memcpy(&send_buffer[40],usart_send1,20);
temp_resp_i64=resp_tx_ts_i64;
temp_count3= phy_timer_count_get();
- flag_temp2=uwb_tx(send_buffer, 40,1 ,resp_tx_en_start_u32);//立即发送测试size大小
+ flag_temp2=uwb_tx(send_buffer, 60,1 ,resp_tx_en_start_u32);//立即发送测试size大小
temp_count1=phy_timer_count_get();
while(mac_is_busy());
@@ -511,11 +601,11 @@
{
uint32_t u32LogLen,datalenth;
char acReadponse[200];
- u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "车内：%d 人 测距成功数：%d ID: ",get_in_num,success_num);
-
+ //u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "车内：%d 人 测距成功数：%d ID: ",get_in_num,success_num);
+ u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "门内：%d 人 ID: ",get_in_num);
for(uint16_t i=0;i<get_in_num;i++)
{
- datalenth = sprintf((char*)&acReadponse[u32LogLen],",%04X",anchor_id_in[i]);
+ datalenth = sprintf((char*)&acReadponse[u32LogLen],"%04X,",anchor_id_in[i]);
u32LogLen += datalenth;
}
LOG_INFO(TRACE_MODULE_APP,"%s\r\n",acReadponse);
@@ -677,6 +767,68 @@
}
}
}
+extern uint8_t calibration_start;
+ float pd_offset,offset;
+ float divide_param = 1.97; //3.25为分立天线的值
+void CalibratePdOffset(int16_t angle)
+{
+ uint16_t i;
+ static uint8_t times=0,steady=0;
+ static int16_t tmp,max,min,ave;
+ static int32_t sum;
+ static int16_t History[calib_len];
+ History[times]=angle;
+ times++;
+ if(times >= calib_len)
+ {
+ max = -900;
+ min = 900;
+ times = 0;
+ sum = 0;
+ for(i = 0;i < calib_len;i++)
+ {
+ tmp=History[i];
+ sum += tmp;
+ if (tmp > max)
+ max = tmp;
+ else if(tmp<min)
+ min = tmp;
+ }
+ ave = sum / calib_len;
+
+// if(steady)
+// {
+ if (abs(ave) < 100)
+ {
+// Save.pd_offset=pd_offset;
+ pd_offset=-ave;
+ memcpy(&g_com_map[PDOFFSET],&pd_offset,4);
+ save_com_map_to_flash();
+ i = 2;
+ WriteCtrlPara(COMM_MAP_CALIB_OK,(uint8_t*)&i,1);
+ calibration_start = 0;
+ //printf("Calibration Success.\r\n");
+ }
+// else
+// {
+// steady = 0;
+// }
+//
+// }
+// else
+// {
+// if(max - min < 200)
+// {
+// offset =- ave * M_PI * divide_param/1800;
+// pd_offset += offset;
+// steady = 1;
+// }else{
+// pd_offset -= 1;
+// }
+// }
+ }
+
+}
int UwbRange(void)
{
@@ -772,12 +924,39 @@
seize_anchor=1; //抢占anchor
Anchor_RecNearPoll(i);
}
+ uint8_t valid_sts=0;
+ valid_sts= sts_valid_check();
+ if (valid_sts)
+ {
+ aoa_calculate(&elevation, &azimuth);
+ aoa_fom_get(NULL, &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();
+ }
+
recev_error_num=0;
//range_timeout_us=5000;//恢复为5000进入range后
//LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 包序%d\r\n",g_com_receive_id,distance,frame_seq_nb2);
//LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 信号强度ANT0:%f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);
//LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 信号强度ANT0:%f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[1],sts_rssi[2]);
- check_if_in_or_out_car(g_com_receive_id,distance,sts_rssi[1],sts_rssi[2]);
+ //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d\r\n",g_com_receive_id,distance);
+ if(calibration_start)
+ {
+ 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);
+ }
+
+ //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++;
//gpio_pin_clr(SCL_PIN);
@@ -852,6 +1031,7 @@
phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
#endif
}
+
//主函数绑定接受逻辑
int UwbSearch(void)
{
--
Gitblit v1.9.3