ChinaUWBProject.git

			@@ -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);
			}