ChinaUWBProject.git

			@@ -121,7 +121,7 @@
			uint16_t rec_nearbaseid;
			uint8_t temp_tag_num;
			int32_t test2;
			static uint8_t receive_flag=0;
			//static uint8_t receive_flag=0;
			static uint8_t rec_tag_index,tag_num_tosend;

			static uint32_t start_receive_count,end_receive_count,poll_timeout,current_count,temp_resp;
			@@ -201,70 +201,70 @@
			return rx_timestamp;
			}

			/* RX done process handler. */
			static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
			{
			// Power off radio
			power_off_radio();
			///* RX done process handler. */
			//static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)
			//{
			// // Power off radio
			// power_off_radio();

			rx_state = rx_report->err_code;
			// rx_state = rx_report->err_code;

			/** UWB RX success */
			if (rx_state == UWB_RX_OK)
			{
			/* Received data does not contain FCS */
			rx_length = rx_report->pkt_len;
			memcpy(rx_buf, rx_report->pkt_data, rx_length);
			/* Calculate rx timestamp */
			resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
			temp_tag_count1=phy_timer_count_get();
			//获取发射端时钟偏差
			resp_rx_num++;
			freq_offset=phy_freq_offset_get();
			freq_offset_filter=average_filter(freq_offset);//获取频偏
			// int32_t ppm = freq_offset_filter / (int32_t)(ch_center_freq_map[UWB_CH_NUM] * 1e-6);
			// calib_xtal38m4_load_cap_auto_tune(ppm);//利用电容调整晶振适配频偏应在完整的一包之后调整，需要关闭XTAL_AUTO_TUNE_EN 宏定义避免收包中途校准导致测距错误
			//LOG_INFO(TRACE_MODULE_APP, "poll_tx_num is %d,resp_rx_num is %d,distance is %lf\r\n",poll_tx_num,resp_rx_num,distance);
			receive_flag=1;

			}
			else
			{
			/* UWB_PLD_ERR payload error */
			/* UWB_PHR_ERR PHR error */
			/* UWB_SFD_ERR Sfd error */
			/* UWB_BD_ERR Preamble detection error */
			/* UWB_TO_ERR Receive timeout */
			/* UWB_STS_ERR STS error */
			rx_state1=rx_report->err_code;
			rx_length = 0;
			receive_flag=2;
			temp_tag_count2=phy_timer_count_get();

			}

			}
			// /** UWB RX success */
			// if (rx_state == UWB_RX_OK)
			// {
			// /* Received data does not contain FCS */
			// rx_length = rx_report->pkt_len;
			// memcpy(rx_buf, rx_report->pkt_data, rx_length);
			// /* Calculate rx timestamp */
			// resp_rx_ts_i64 = ranging_rx_time_correct(rx_report);
			// temp_tag_count1=phy_timer_count_get();
			// //获取发射端时钟偏差
			// resp_rx_num++;
			// freq_offset=phy_freq_offset_get();
			// freq_offset_filter=average_filter(freq_offset);//获取频偏
			//// int32_t ppm = freq_offset_filter / (int32_t)(ch_center_freq_map[UWB_CH_NUM] * 1e-6);
			//// calib_xtal38m4_load_cap_auto_tune(ppm);//利用电容调整晶振适配频偏应在完整的一包之后调整，需要关闭XTAL_AUTO_TUNE_EN 宏定义避免收包中途校准导致测距错误
			// //LOG_INFO(TRACE_MODULE_APP, "poll_tx_num is %d,resp_rx_num is %d,distance is %lf\r\n",poll_tx_num,resp_rx_num,distance);
			//// receive_flag=1;
			//
			// }
			// else
			// {
			// /* UWB_PLD_ERR payload error */
			// /* UWB_PHR_ERR PHR error */
			// /* UWB_SFD_ERR Sfd error */
			// /* UWB_BD_ERR Preamble detection error */
			// /* UWB_TO_ERR Receive timeout */
			// /* UWB_STS_ERR STS error */
			// rx_state1=rx_report->err_code;
			// rx_length = 0;
			// receive_flag=2;
			// temp_tag_count2=phy_timer_count_get();
			//
			// }
			//
			//}

			/* TX done process handler. */
			static void tx_int_callback(struct MAC_HW_REPORT_T *tx_report)
			{
			// Power off radio
			power_off_radio();
			///* TX done process handler. */
			//static void tx_int_callback(struct MAC_HW_REPORT_T *tx_report)
			//{
			// // Power off radio
			// power_off_radio();

			/** UWB TX success */
			if (tx_report->err_code == UWB_TX_OK)
			{
			poll_tx_num++;
			temp_count4=phy_timer_count_get();//测试
			if(temp_flag){
			count2=phy_timer_count_get();
			temp_flag=0;
			return;
			}
			count1=temp_tag_count1;
			temp_flag=1;
			}
			}
			// /** UWB TX success */
			// if (tx_report->err_code == UWB_TX_OK)
			// {
			// poll_tx_num++;
			// temp_count4=phy_timer_count_get();//测试
			// if(temp_flag){
			// count2=phy_timer_count_get();
			// temp_flag=0;
			// return;
			// }
			// count1=temp_tag_count1;
			// temp_flag=1;
			// }
			//}



			@@ -370,122 +370,124 @@

			int TagRange(void)
			{
			// The following peripherals will be initialized in the uwb_open function
			// phy/mac/aes/lsp/phy timers initialized
			uwb_open();
			// // 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);
			// // Set calibration parameters
			// uwb_calibration_params_set(config.phy_cfg.ch_num);

			// 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,
			};
			phy_adv_params_configure(&adv_config);
			// // 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,
			// };
			// 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);
			// // 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);
			// ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);

			// Register rx interrupt callback function
			mac_register_process_handler(tx_int_callback, rx_int_callback);
			// // Register rx interrupt callback function
			//// mac_register_process_handler(tx_int_callback, rx_int_callback);

			//#if LOW_POWER_EN
			// // Initialize low power mode
			// power_init();
			// // Enable sleep timer
			// sleep_timer_open(true, SLEEP_TIMER_MODE_ONESHOT, NULL);
			//#endif
			uwb_poll_buffer_construct();
			temp_tag_num=0;//临时数量为0
			poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
			tempflag=uwb_tx(uwb_sendbuffer,13+4*nearbase_num,1,poll_tx_en_start_u32);//立即发送

			//gpio_pin_set(IO_PIN_5);//测试
			//LOG_INFO(TRACE_MODULE_APP, "进入测距",poll_tx_num,resp_rx_num,distance);
			poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
			temp_tag_count1=phy_timer_count_get();
			while(mac_is_busy());//等待发送完成
			temp_tag_count2=phy_timer_count_get();
			resp_rx_en_start_u32 = poll_tx_en_start_u32+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);//设置发送多少s后接收按target独立时间起点算+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US为发送后要多久开启接收时间
			temp_resp=resp_rx_en_start_u32;
			//tempflag=uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
			tempflag=uwb_rx(0,0, RESP_RX_TIMEOUT_US);

			receive_flag =0;
			start_receive_count=phy_timer_count_get();
			poll_timeout=nearbase_numUS_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS)+US_TO_PHY_TIMER_COUNT(RESP_RX_TIMEOUT_US);//多一个多0.4ms默认0.4ms计算为0.1254*100000,默认开启1mss
			end_receive_count=start_receive_count+poll_timeout;
			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();//不断刷新当前计数器值
			temp_tag_count2=phy_timer_count_get();
			while(mac_is_busy());//等待接收完成
			temp_tag_count3=phy_timer_count_get();
			if(receive_flag==1)//成功接收数据
			{
			receive_flag=0;

			//接收成功则判断是否为同一组
			if (rx_buf[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)&&rx_buf[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
			{
			memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
			if(last_nearbase_num==0)
			{
			nearbaseid_list[0]=rec_nearbaseid;
			nearbase_num=1;
			exsistbase_list[0]=KEEP_TIMES;
			SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);//这个是改过的参数
			}
			// if(rec_nearbaseid==nearbaseid_list[0])
			// {
			// exsistbase_list[0]=KEEP_TIMES;
			// SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);//这个是改过的参数
			// }
			else
			{
			rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
			SetANCTimestap(rec_nearbasepos,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);
			if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
			{
			//get_newbase=1;
			nearbase_num++;
			nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
			}
			exsistbase_list[rec_nearbasepos]=KEEP_TIMES;//更新存活时间
			}
			memset(rx_buf, 0, sizeof(rx_buf));//清空接收到的用完的数组
			rx_length=0;//清0长度
			// uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
			// while(mac_is_busy());
			//break;//成功接收就退出
			}
			tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
			//gpio_pin_clr(IO_PIN_5);//测试

			}else if(receive_flag==2){//接收出错
			receive_flag=0;
			tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
			temp_tag_count3=phy_timer_count_get();
			// while(mac_is_busy());
			temp_tag_count1=phy_timer_count_get();
			}
			//gpio_pin_clr(IO_PIN_5);//测试
			}
			delay_us(1);
			uwb_rx_force_off(1);
			CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
			AnchorListUpdate();//更新存活基站列表
			Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
			LOG_INFO(TRACE_MODULE_APP,"基站ID：%X,距离: %d\r\n",rec_nearbaseid,nearbase_distlist[0]);
			////#if LOW_POWER_EN
			//// // Initialize low power mode
			//// power_init();
			//// // Enable sleep timer
			//// sleep_timer_open(true, SLEEP_TIMER_MODE_ONESHOT, NULL);
			////#endif
			// uwb_poll_buffer_construct();
			// temp_tag_num=0;//临时数量为0
			// poll_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_DELAY);//发送必须要延时发送才可以用于测距否则立即发送会获取时间戳不对，需要计算程序运行时间，避免设置过去时间
			// tempflag=uwb_tx(uwb_sendbuffer,13+4*nearbase_num,1,poll_tx_en_start_u32);//立即发送
			//
			// //gpio_pin_set(IO_PIN_5);//测试
			// //LOG_INFO(TRACE_MODULE_APP, "进入测距",poll_tx_num,resp_rx_num,distance);
			// poll_tx_ts_i64 = ranging_tx_time_correct(poll_tx_en_start_u32 + phy_shr_duration());//修正时间戳
			// temp_tag_count1=phy_timer_count_get();
			// while(mac_is_busy());//等待发送完成
			// temp_tag_count2=phy_timer_count_get();
			// resp_rx_en_start_u32 = poll_tx_en_start_u32+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US);//设置发送多少s后接收按target独立时间起点算+ US_TO_PHY_TIMER_COUNT(POLL_TX_TO_RESP_RX_DLY_US为发送后要多久开启接收时间
			// temp_resp=resp_rx_en_start_u32;
			// //tempflag=uwb_rx(1, resp_rx_en_start_u32, RESP_RX_TIMEOUT_US);//开启接受并设置接收超时
			// tempflag=uwb_rx(0,0, RESP_RX_TIMEOUT_US);
			//
			//// receive_flag =0;
			// start_receive_count=phy_timer_count_get();
			// poll_timeout=nearbase_numUS_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS)+US_TO_PHY_TIMER_COUNT(RESP_RX_TIMEOUT_US);//多一个多0.4ms默认0.4ms计算为0.1254*100000,默认开启1mss
			// end_receive_count=start_receive_count+poll_timeout;
			// 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();//不断刷新当前计数器值
			// temp_tag_count2=phy_timer_count_get();
			// while(mac_is_busy());//等待接收完成
			// temp_tag_count3=phy_timer_count_get();
			//// if(receive_flag==1)//成功接收数据
			// {
			//// receive_flag=0;
			//
			// //接收成功则判断是否为同一组
			// if (rx_buf[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buf[TAG_ID_IDX],&dev_id,2)&&rx_buf[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据
			// {
			// memcpy(&rec_nearbaseid,&rx_buf[ANCHOR_ID_IDX],2);
			// if(last_nearbase_num==0)
			// {
			// nearbaseid_list[0]=rec_nearbaseid;
			// nearbase_num=1;
			// exsistbase_list[0]=KEEP_TIMES;
			// SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);//这个是改过的参数
			// }
			//// if(rec_nearbaseid==nearbaseid_list[0])
			//// {
			//// exsistbase_list[0]=KEEP_TIMES;
			//// SetANCTimestap(0,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2);//这个是改过的参数
			//// }
			// else
			// {
			// rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
			// SetANCTimestap(rec_nearbasepos,&rx_buf[RESP_MSG_POLL_RX_TS_IDX],&rx_buf[RESP_MSG_RESP_TX_TS_IDX],(uint32_t)resp_rx_ts_i64,&rx_buf[RESP_MSG_ANC_DISTOFFSET],test2,(uint32_t)poll_tx_ts_i64);
			// if(rec_nearbasepos>=last_nearbase_num) //发现新的基站
			// {
			// //get_newbase=1;
			// nearbase_num++;
			// nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
			// }
			// exsistbase_list[rec_nearbasepos]=KEEP_TIMES;//更新存活时间
			// }
			// memset(rx_buf, 0, sizeof(rx_buf));//清空接收到的用完的数组
			// rx_length=0;//清0长度
			//// uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
			//// while(mac_is_busy());
			// //break;//成功接收就退出
			// }
			// tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
			// //gpio_pin_clr(IO_PIN_5);//测试
			//
			// }else
			//// if(receive_flag==2)
			// {//接收出错
			//// receive_flag=0;
			// tempflag=uwb_rx(0, 0, RESP_RX_TIMEOUT_US);//立即开启接受并设置0超时
			// temp_tag_count3=phy_timer_count_get();
			//// while(mac_is_busy());
			// temp_tag_count1=phy_timer_count_get();
			// }
			// //gpio_pin_clr(IO_PIN_5);//测试
			// }
			// delay_us(1);
			// uwb_rx_force_off(1);
			// CalculateDists(poll_tx_ts_i64); //计算距离并更新距离表
			// AnchorListUpdate();//更新存活基站列表
			// Rank_ANchor_list_by_dis();//冒泡排序重新按距离重新排列基站列表最小距离存货基站放到最前面
			// LOG_INFO(TRACE_MODULE_APP,"基站ID：%X,距离: %d\r\n",rec_nearbaseid,nearbase_distlist[0]);
			}