ChinaUWBProject.git

			@@ -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);
			@@ -99,7 +100,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 +254,93 @@

			/* 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;
			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;
			//}
			return angle;
			}
			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] = 0; //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\n",tag_id,cnt_log++,angle1,distance_temp);
			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();

			@@ -272,19 +358,7 @@
			poll_rx_num++;
			rssi = rx_report->rssi;
			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,14 +512,17 @@
			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());
			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;

			@@ -506,17 +583,20 @@
			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 人 ID:",get_in_num,get_out_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);
			success_num=0;
			}
			void TagListUpdate_person_num_car(void)
			{
			@@ -674,13 +754,76 @@
			}
			}
			}
			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)
			{
			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);
			@@ -694,7 +837,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);
			@@ -707,6 +850,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());
			@@ -717,25 +861,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;
			@@ -773,25 +911,49 @@
			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);
			check_if_in_or_out_car(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;
			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)
			// {
			@@ -812,37 +974,51 @@
			}
			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
			}

			//主函数绑定接受逻辑
			int UwbSearch(void)
			{
			@@ -851,36 +1027,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);

			@@ -980,9 +1171,9 @@
			case LINK_SUCCESS:
			{ //连接成功进行轮询测距

			uwb_led_on();
			//uwb_led_on();
			UwbRange();
			uwb_led_off();
			//uwb_led_off();

			}
			break;