更新：1、优化TAS逻辑

2、优化宏定义

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>

data_process_class_dll/data_process.cpp

@@ -65,7 +65,6 @@ int Data_Process::data_preprocess(struct DataRev    Data_Input[150])
 
 		data_num = min(max(Data_Input[0].Point_Sum, 0), 150);
 		TAS_track_idx = Data_Input[0].TAS_track_index;
-		latest_timestamp = Data_Input[0].CPI_time;
 		for (int i=0; i<data_num;i++)
 		{
 			PointRecv Data_buffer_temp;

data_process_class_dll/parameters.h

@@ -12,7 +12,7 @@
 
 #ifdef MECHANICAL_SCANNING
 /******************************数据处理参数**************************************/
-#define DATA_RATE_TAS 0.0625
+// #define DATA_RATE_TAS 0.0625
 
 #define SIGMA_R    10.0         //测量误差
 #define SIGMA_A    0.02
@@ -52,12 +52,14 @@
 
 #define DIRECT_TRACKING_QUEUE_LENGTH 10
 
+#pragma message("MECHANICAL_SCANNING")
+
 #endif
 
 #ifdef PHASE_SCANNING
 
 /******************************数据处理参数**************************************/
-#define DATA_RATE_TAS 0.3
+// #define DATA_RATE_TAS 0.3
 
 #define SIGMA_R    10.0         //测量误差
 #define SIGMA_A    0.02
@@ -99,6 +101,8 @@
 
 #define DIRECT_TRACKING_QUEUE_LENGTH 10
 
+#pragma message("PHASE_SCANNING")
+
 #endif
 
 #define Round(x) (((x) > 0) \

data_process_class_dll/struct.h

@@ -125,7 +125,7 @@ struct Tracking_Target
 {
 	int     Index;                          //目标批号
 	int     empty_flag;                     //是否为空标志位 1非空 0空
-
+	long long last_track_time;				//最新跟踪时间
 };
 
 //引导跟踪目标结构体

data_process_class_dll/tas_ctrl.cpp

@@ -39,14 +39,14 @@ void TAS_Ctrl::tas_ctrl_process(std::vector<Trust_Track>            *trust_track
 	tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
 	tas_beam_output(trust_track,Tracking_beam,Work_Parameter,latest_timestamp);
 
-	//跟踪队列移位
+	// //跟踪队列移位
-	struct Tracking_Target tas_target_tmp;
+	// struct Tracking_Target tas_target_tmp;
-	memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target));
+	// memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target));
-	for (int i=TAS_QUEUE_LENGTH-1;i>0;i--)
+	// for (int i=TAS_QUEUE_LENGTH-1;i>0;i--)
-	{
+	// {
-		memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
+	// 	memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
-	}
+	// }
-	memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
+	// memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
 };
 
 void TAS_Ctrl::tas_target_add(std::vector<Trust_Track>          *trust_track,
@@ -81,6 +81,7 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track>          *trust_track,
 						//插入跟踪队列
 						tas_target_queue[j].Index=(*trust_track)[i].Track_Index;
 						tas_target_queue[j].empty_flag=1;
+						tas_target_queue[j].last_track_time=(*trust_track)[i].T_track;
 
 						//跟踪目标数目加一
 						tas_target_num=tas_target_num+1;
@@ -254,47 +255,62 @@ void  TAS_Ctrl::tas_beam_output(std::vector<Trust_Track>            *trust_track
                                 struct TrackingBeam             *Tracking_beam,
                                 struct  RadarPara               Work_Parameter,
                                 long long latest_timestamp)
-{
-	if(tas_target_queue[0].empty_flag==1)
 {
 
-		double H_track = 0;
-		double X_now[6] = {0};
 	long long CPI_time = 0;
-		bool found_track = false;
+	long long earliest_CPI_time = LLONG_MAX;
-		for (int i=0;i<trust_track->size();i++ )
+	float delta_T = 0;
+	double H_track = 0;
+	int earliest_track_idx=-1;
+	int earliest_tas_queue_idx=-1;
+	double X_now[6] = {0};
+
+	//查找TAS队列中CPI时间最早的目标且满足TAS数据率的目标
+	for(int tas_idx=0;tas_idx<TAS_QUEUE_LENGTH;tas_idx++)
 	{
-			if((*trust_track)[i].Track_Index == tas_target_queue[0].Index)
+		if(tas_target_queue[tas_idx].empty_flag==0)
 		{
-				H_track = (*trust_track)[i].Height;
+			continue;
-				CPI_time = (*trust_track)[i].T_track;
+		}
-				for(int ii=0;ii<6;ii++)
+
-					X_now[ii]=(*trust_track)[i].X[ii];
+		for (size_t track_idx=0;track_idx<trust_track->size();track_idx++ )
-				found_track = true;
+		{
+			if((*trust_track)[track_idx].Track_Index == tas_target_queue[tas_idx].Index)
+			{
+				CPI_time = (*trust_track)[track_idx].T_track;
+				delta_T = (latest_timestamp - CPI_time) / 1000.0f;
+				
+				//查找最早的CPI时间, 航迹时间与当前最新时间戳的差值大于 1/DATA_RATE_TAS 时才输出TAS跟踪波束
+				if(CPI_time < earliest_CPI_time && (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f > 1.0f / Work_Parameter.DATA_RATE_TAS && delta_T > 1.0f / Work_Parameter.DATA_RATE_TAS)
+				{
+					earliest_CPI_time = CPI_time;
+					H_track = (*trust_track)[track_idx].Height;
+
+					earliest_track_idx = track_idx;
+					earliest_tas_queue_idx = tas_idx;
+				
+					for(int ii=0;ii<6;ii++){
+						X_now[ii]=(*trust_track)[track_idx].X[ii];
+					}
+						
+					printf("TAS: earliest_track_idx: %d, CPI_time: %lld, delta_T: %.3f\n", earliest_track_idx, CPI_time, delta_T);
+					
+				}
+
+				break;
+			}
 		}
 	}
 
-		if (!found_track)
+	//没有满足条件的TAS目标，关闭跟踪波束
+	if(earliest_track_idx == -1)
 	{
 		Tracking_beam->open_flag = 0;
 		return;
 	}
 
-		//数据率门控：航迹时间与当前最新时间戳的差值大于 1/DATA_RATE_TAS 时才输出TAS跟踪波束，
+	//更新目标最后跟踪时间
-		//保证TAS目标数据率不随波束时间变化（使用Work_Parameter中的DATA_RATE_TAS参数值）。
+	tas_target_queue[earliest_tas_queue_idx].last_track_time = latest_timestamp;
-		//DATA_RATE_TAS<=0 时不做门控，按原逻辑每次输出（避免除零，兼容未配置该参数的调用者）。
-		if (Work_Parameter.DATA_RATE_TAS > 0)
-		{
-			double delta_T_track = (double)(latest_timestamp - CPI_time) / 1000.0;
-			if (delta_T_track <= 1.0 / Work_Parameter.DATA_RATE_TAS)
-			{
-				//更新间隔未到 不输出跟踪波束
-				Tracking_beam->open_flag = 0;
-				return;
-			}
-		}
-
-		float delta_T = (latest_timestamp - CPI_time) / 1000.0f;
 
 	//预测目标位置 计算跟踪波束波位号 俯仰角
 	double x_track=X_now[0]+X_now[1]*delta_T;
@@ -304,9 +320,9 @@ void  TAS_Ctrl::tas_beam_output(std::vector<Trust_Track>            *trust_track
 	Coor_trans.cart2polar(x_track,y_track,&range,&amzi);
 
 	//目标距离
-		Tracking_beam->Range=range;
+	Tracking_beam->Range=(float)range;
 	//目标方位
-		Tracking_beam->Azi=amzi/PI*180;
+	Tracking_beam->Azi=(float)amzi/PI*180;
 //		Tracking_beam->Azi+=6;
 	//目标俯仰角
 	double elev=asin(H_track/range)/PI*180;
@@ -318,23 +334,16 @@ void  TAS_Ctrl::tas_beam_output(std::vector<Trust_Track>            *trust_track
 //		else
 //			elev=elev;
 
-		Tracking_beam->Elev=elev;
+	Tracking_beam->Elev=(float)elev;
 
 	//跟踪波束类型
 	Tracking_beam->type=1;
 
 	//跟踪目标批号
-		Tracking_beam->TAS_track_index = tas_target_queue[0].Index;
+	Tracking_beam->TAS_track_index = earliest_track_idx;
 
 	//跟踪波束开关开启
 	Tracking_beam->open_flag=1;
 
 	//std::cout << "TAS: range: " <<Tracking_beam->Range << "azi: " <<Tracking_beam->Azi <<"pit: " <<Tracking_beam->Elev;
 }
-	else
-	{
-		//跟踪波束开关关闭
-		Tracking_beam->open_flag=0;
-	}
-
-}

data_process_class_dll/track_asso_direct_tracking.cpp

@@ -424,7 +424,7 @@ void Track_Asso_Direct_Tracking::model_filter(Trust_Track        *trust_track,
    //未关联上，航迹外推
    else
    {
-	   double delta_T = DATA_RATE_TAS;
+	   double delta_T = Work_Parameter.DATA_RATE_TAS/1000.0;
 	   double F[6][6],  Q1[6][6],  Q2[6][6],  Q3[6][6];
 	   IMM_F_Q_gen( v_track, delta_T, F, Q1,  Q2,  Q3,Work_Parameter);
 

data_process_class_dll/track_init.cpp

@@ -342,29 +342,6 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
 				double h_track_head = (*temp_track)[j][0].height;
 				double T_track_head = (*temp_track)[j][0].T;
 
-//                if(T_track_head == 30675948)
-//                {
-//                    std::cout << "r_point=" << r_point
-//                             << "h_point=" << h_point
-//                             << "T_point=" << T_point
-//                             << "x_track_head=" << x_track_head
-//                             << "y_track_head=" << y_track_head
-//                             << "v_track_head=" << v_track_head
-//                             << "T_track_head=" << T_track_head;
-//                }
-
-//                if(T_point == 30682320)
-//                {
-//                    std::cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
-//                    std::cout << "r_point=" << r_point
-//                             << "h_point=" << h_point
-//                             << "T_point=" << T_point
-//                             << "x_track_head=" << x_track_head
-//                             << "y_track_head=" << y_track_head
-//                             << "v_track_head=" << v_track_head
-//                             << "T_track_head=" << T_track_head;
-//                }
-
 				//距离差
 				double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
 
@@ -390,17 +367,6 @@ void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>>
 					&& vr_point*v_track_head>0  )
 				{
 
-//                     if(T_track_head == 30675948)
-//                     {
-//                         std::cout << "r_point=" << r_point
-//                                  << "h_point=" << h_point
-//                                  << "T_point=" << T_point
-//                                  << "x_track_head=" << x_track_head
-//                                  << "y_track_head=" << y_track_head
-//                                  << "v_track_head=" << v_track_head
-//                                  << "T_track_head=" << T_track_head;
-//                     }
-
 					struct Asso_info asso_info_tmp;
 					asso_info_tmp.point_idx = i+1;
 					asso_info_tmp.track_idx = j+1;