From 945f0cc0210d2ea3339870f0e38d9be72314c4e2 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期二, 15 七月 2025 14:08:55 +0800
Subject: [PATCH] 修改PA引脚为FUN6,正确驱动PA,修改为官方demo算法,线性度需要优化

---
 keil/uwb_app.c |  458 ++++++++++++++++++++++++++++++++++++++++++++++++++------
 1 files changed, 403 insertions(+), 55 deletions(-)

diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index 8fa0ea7..c282465 100644
--- a/keil/uwb_app.c
+++ b/keil/uwb_app.c
@@ -9,8 +9,9 @@
 #include "dw_app_anchor.h"
 #include "global_param.h"
 #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);
@@ -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;
@@ -69,7 +71,7 @@
 #define RANGING_PERIOD_MS (1000)
 
 /* This is the delay from Frame RX POLL frame to send RESP Frame */
-#define POLL_RX_TO_RESP_TX_DLY_US 4000U //yuan670 with urt 3000success
+#define POLL_RX_TO_RESP_TX_DLY_US 2000U //yuan670 with urt 3000success
 
 #define RESP_TX_TO_FINAL_RX_DLY_US 500U
 
@@ -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
@@ -167,6 +169,10 @@
     .sts_key1 = 0xF86050A8,
     .sts_key2 = 0xD1D336AA,
     .sts_key3 = 0x14148674,
+};
+static struct anchor_id_car{
+ uint16_t anchor_new_id;
+ uint16_t change_num;
 };
 /* Buffer to store received frame */
 
@@ -249,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;
+while(angle>=90)
+        angle-=180;
+while(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();
 
@@ -267,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
     {
@@ -339,7 +430,7 @@
             anchordata_num++;
     }
     anchordata_id[i] = ancid;//与之通信的标签id存入当前表中
-		distance=dist*0.5+anchordata_dist[i]*0.5;
+		distance=dist*0.5+(int16_t)anchordata_dist[i]*0.5;
     anchordata_dist[i] = dist;
     anchordata_bat[i] = battary;
     tagofflinetime[i]=0;//不断更新当前TAG对应离线时间
@@ -363,6 +454,19 @@
     }
     if(i==taglist_num)
         return taglist_num;
+    //tagofflinetime[i] = 0;
+    return i;
+}
+
+uint16_t CmpCarInExistList(uint16_t tagid)
+{   uint16_t i;
+    for(i=0; i<anchordata_num; i++)
+    {
+        if(memcmp(&tagid,&anchordata_id[i],2)==0)
+            break;
+    }
+    if(i==anchordata_num)
+        return anchordata_num;
     //tagofflinetime[i] = 0;
     return i;
 }
@@ -394,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;
@@ -415,29 +520,47 @@
 //       resp_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(20*rec_nearbase_num+POLL_RX_TO_RESP_TX_DLY_US+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS);//后面的需要根据已有基站数量进行更改,如果是抢占自己最后一个回复,要有底数
 //				//这里应该有问题问一下钟工
 //		}//此处设置绝对时间将poll u32改为phy_timer_count_get()
-		resp_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US);
+		resp_tx_en_start_u32 = poll_rx_en_start_u32+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US);
     resp_tx_ts_i64 = ranging_tx_time_correct(resp_tx_en_start_u32 + phy_shr_duration());//修正时间戳
     /* Write all timestamps in the final message. See NOTE 8 below. */
     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();
+		tx_num++;
     while(mac_is_busy());
-    gpio_pin_clr(SCL_PIN);
+    
 }
-uint32_t range_timeout_us = 5000;//yuan5000
+uint32_t range_timeout_us = 1000000;//yuan5000
 uint16_t uwb_searchcount;
 uint8_t flag_recsuccess;
 
 uint32_t start_receive_count_calibration;
 uint32_t current_count_calibration;
 uint32_t get_in_num,get_out_num;
+struct anchor_id_car anchor_id_in_car[TAG_NUM_IN_SYS],anchor_id_out_car[TAG_NUM_IN_SYS];
+int16_t change_count[TAG_NUM_IN_SYS];
 uint16_t anchor_id_in[TAG_NUM_IN_SYS],anchor_id_out[TAG_NUM_IN_SYS];
 uint8_t secondtask_search_count,secondtask_search_flag;
 extern uint8_t g_start_send_flag,search_open_flag = 1,link_success_flag,flag_secondtask;
+uint16_t CmpCarInTable(uint16_t tagid)
+{   uint16_t i;
+    for(i=0; i<get_in_num; i++)
+    {
+        if(memcmp(&tagid,&anchor_id_in[i],2)==0)
+            break;
+    }
+    if(i==get_in_num)
+        return get_in_num;
+    //tagofflinetime[i] = 0;
+    return i;
+}
 void TagListUpdate_person_num(void)
 {
    uint8_t i,j=0;
@@ -475,7 +598,37 @@
     anchordata_num=j;
 		
 }
+int success_num=0;
+void in_table_log(void)
+{
+	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 人 ID: ",get_in_num);
+					for(uint16_t i=0;i<get_in_num;i++)
+            {
+                datalenth = sprintf((char*)&acReadponse[u32LogLen],"%04X,",anchor_id_in[i]);
+                u32LogLen += datalenth;
+            }
+		LOG_INFO(TRACE_MODULE_APP,"%s\r\n",acReadponse);
+						success_num=0;
+}
+void TagListUpdate_person_num_car(void)
+{
+uint8_t i,j=0;
+    for(i=0; i<anchordata_num; i++)
+    {
+        if(tagofflinetime[i]++<QUIT_SLOT_TIME)
+        {
+            anchordata_id[j]=anchordata_id[i];
+            tagofflinetime[j++]=tagofflinetime[i];
+        }else{
+						change_count[j]=0;
+			}
+    }
+    anchordata_num=j;
 
+}
 void Calibration_Time(void)
 {	
 	current_count_calibration=phy_timer_count_get();
@@ -506,9 +659,25 @@
 	}
 }
 uint16_t g_com_receive_id;
-int Uwb_One_Shot(void)
+int add_in_car_table(uint16_t position_anchor_in_table,uint16_t receive_success_id)
 {
-		
+		 if(position_anchor_in_table==get_in_num)//新增
+				{
+        if(get_in_num<TAG_NUM_IN_SYS-1)
+            get_in_num++;
+				}
+				anchor_id_in[position_anchor_in_table]=receive_success_id;
+}
+int delete_in_car_table(uint16_t position_anchor_in_table)
+{
+			if(position_anchor_in_table<get_in_num)//若在列表里
+			{
+			for(int i=position_anchor_in_table+1;i<get_in_num;i++)
+							{
+								anchor_id_in[i-1]=anchor_id_in[i];//缺个数量--
+							}
+							get_in_num--;//更新列表现存数
+			}
 }
 void check_if_in_or_out(uint16_t receive_success_id,int32_t distance_from_tag,float rssi_ant0,float rssi_ant2)
 {
@@ -569,13 +738,123 @@
     }
 	}
 }
+
+void check_if_in_or_out_car(uint16_t receive_success_id,int32_t distance_from_tag,float rssi_ant0,float rssi_ant2)
+{
+    uint8_t i;
+		uint16_t position_anchor_exist,position_anchor_in_table;
+		position_anchor_exist=CmpCarInExistList(receive_success_id);
+	if(rssi_ant0>rssi_ant2&&distance_from_tag<200)
+	{
+		change_count[position_anchor_exist]++;
+		if(change_count[position_anchor_exist]>0)
+		{
+		position_anchor_in_table=CmpCarInTable(receive_success_id);
+		add_in_car_table(position_anchor_in_table,receive_success_id);
+		}
+		if(change_count[position_anchor_exist]>=3)
+		{	change_count[position_anchor_exist]=3;
+			
+		}
+	}else if(rssi_ant0<rssi_ant2&&distance_from_tag<200)
+	{
+		change_count[position_anchor_exist]--;
+		if(change_count[position_anchor_exist]<0)
+		{
+		position_anchor_in_table=CmpCarInTable(receive_success_id);
+		delete_in_car_table(position_anchor_in_table);
+		}
+		if(change_count[position_anchor_exist]<=-3)
+		{change_count[position_anchor_exist]=-3;
+		}	
+	}
+}
+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;
+//				}
+//			}
+	}
+
+}
+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;
     uint16_t tempid;
     // The following peripherals will be initialized in the uwb_open function
     // phy/mac/aes/lsp/phy timers initialized
-    uwb_open();
+  
 
 //    // Set calibration parameters
 //    uwb_calibration_params_set(config.phy_cfg.ch_num);
@@ -589,7 +868,7 @@
 //    };
 //    phy_adv_params_configure(&adv_config);
 
-//    // uwb configure
+//    // uwb configure                                                        
 //    uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
 
 //    ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
@@ -602,6 +881,7 @@
     temp_count3=phy_timer_count_get();
     gpio_pin_set(SCL_PIN);
 		sts_lsp_store();
+		
     flag_temp1=uwb_rx(0, 0, range_timeout_us);//开启接收
 	
    // while(mac_is_busy());
@@ -612,25 +892,19 @@
 	if(end_receive_count>=UINT32_MAX)
 	{end_receive_count-=UINT32_MAX;}
 	current_count=phy_timer_count_get();
-		while(current_count<end_receive_count||current_count>end_receive_count+HALF_SECOND_TIME)//循环接受包体,若为124.8K则是+62400000
-		{
+
 			current_count=phy_timer_count_get();
 			while(mac_is_busy())
         {
-						Calibration_Time();
+						//Calibration_Time();
             IdleTask();
-            current_count=phy_timer_count_get();
-            if(current_count>end_receive_count&&current_count<end_receive_count+HALF_SECOND_TIME)
-            {
-                    break;
-            }
         }
 			sts_lsp_store_stop();
 			
 			
     if(receive_flag==1)//成功接收
     {
-
+		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包
         {    flag_recsuccess = 1;
@@ -668,25 +942,63 @@
                 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);
+				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=-mk_q7_to_s16(azimuth);
+					//sts_rssi = sts_rssi_output_get();
+			}
+		
 						recev_error_num=0;
            	//range_timeout_us=5000;//恢复为5000进入range后
-           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]);
-					 check_if_in_or_out(g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);
+           //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]);
+						//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;
+			
+			
+//			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++;
 					
-					
-						gpio_pin_clr(SCL_PIN);
+						//gpio_pin_clr(SCL_PIN);
         //break;去掉break变为一对多
 				}
     } 
-			sts_lsp_store();
-			
-			break;
-		//失败或者接受被高发射机打断都会再次开启接收
-			//flag_temp1=uwb_rx(0, 0, range_timeout_us);
-	}
-		delay_us(1);
-		sts_lsp_store_stop();
-		uwb_rx_force_off(1);
+//			sts_lsp_store();
+			gpio_pin_clr(SCL_PIN);
+//		sts_lsp_store_stop();
+//		uwb_rx_force_off(1);
 
 //		if(!flag_recsuccess)
 //		{
@@ -705,39 +1017,60 @@
 			
 			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();
+	#ifdef STS_MODE
  // Set STS key and IV
     phy_sts_key_configure(&sts_iv_key);
 	
 	// which RX ports will be used for AoA/PDoA
     phy_rx_ant_mode_set(RX_ANT_PORTS_COMBINATION);
-
+	#endif
     // Set calibration parameters
     uwb_calibration_params_set(config.phy_cfg.ch_num);
-
+	#ifndef STS_MODE
    // set advanced parameters
+    struct PHY_ADV_CONFIG_T adv_config = {
+        .thres_fap_detect = 40,
+        .nth_scale_factor = 4,
+        .ranging_performance_mode = 0,
+        .skip_weakest_port_en = 0,
+    };
+	#else
+	 // set advanced parameters
     struct PHY_ADV_CONFIG_T adv_config = {
         .thres_fap_detect = 40,
         .nth_scale_factor = 4,
         .ranging_performance_mode = 3,
         .skip_weakest_port_en = 0,
     };
+	#endif
     phy_adv_params_configure(&adv_config);
 
     // uwb configure
     uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
-		
+		#ifdef STS_MODE
 		ranging_lib_init();
+		#endif
     ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
-		
+		#ifdef STS_MODE
 		aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
     aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
 
     aoa_param_config();
 		
 		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);
 }
+
 //主函数绑定接受逻辑
 int UwbSearch(void)
 {
@@ -746,36 +1079,51 @@
     // The following peripherals will be initialized in the uwb_open function
     // phy/mac/aes/lsp/phy timers initialized
     uwb_open();
+	#ifdef STS_MODE
   // Set STS key and IV
     phy_sts_key_configure(&sts_iv_key);
 	
 	// which RX ports will be used for AoA/PDoA
     phy_rx_ant_mode_set(RX_ANT_PORTS_COMBINATION);
-
+	#endif
     // Set calibration parameters
     uwb_calibration_params_set(config.phy_cfg.ch_num);
 
+	#ifndef STS_MODE
    // set advanced parameters
+    struct PHY_ADV_CONFIG_T adv_config = {
+        .thres_fap_detect = 40,
+        .nth_scale_factor = 4,
+        .ranging_performance_mode = 0,
+        .skip_weakest_port_en = 0,
+    };
+		#else
+		  // set advanced parameters
     struct PHY_ADV_CONFIG_T adv_config = {
         .thres_fap_detect = 40,
         .nth_scale_factor = 4,
         .ranging_performance_mode = 3,
         .skip_weakest_port_en = 0,
     };
+		#endif
     phy_adv_params_configure(&adv_config);
 
     // uwb configure
     uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
 		
+		#ifdef STS_MODE
 		ranging_lib_init();
+		#endif
     ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
 		
+		#ifdef STS_MODE
 		aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
     aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
 
     aoa_param_config();
 		
 		phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+		#endif
     // Register rx interrupt callback function
     mac_register_process_handler(tx_int_callback, rx_int_callback);
 		
@@ -875,9 +1223,9 @@
     case LINK_SUCCESS:
     {   //连接成功进行轮询测距
 
-        uwb_led_on();
+        //uwb_led_on();
         UwbRange();
-        uwb_led_off();
+        //uwb_led_off();
 
     }
     break;

--
Gitblit v1.9.3