Merge branch 'feature'

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

data_process_class_dll/data_process.cpp

@@ -160,6 +160,7 @@ int         model)
 		                              &trust_track,
 		                              Trust_Track_Output,
 		                              Trust_track_num_Output,
+									  latest_timestamp,
 		                              Work_Parameter
 		                              );
 

data_process_class_dll/parameters.h

@@ -10,7 +10,7 @@
 /**********************************版本信息*************************************************/
 #define MAJOR_VERSION 1
 #define MINOR_VERSION 5
-#define PATCH_VERSION 6
+#define PATCH_VERSION 7
 #ifdef PHASE_SCANNING
 #define VERSION_INFO "PS"
 #else

data_process_class_dll/rdp_record.cpp

@@ -260,7 +260,7 @@ RDP_RECORD_DEFINE(RdpRecTrustTrack);
 //关联信息
 YLT_REFL(RdpRecAssoInfo, id,point_id,track_id,circle_id,d1,d2,d3,d_min,point_index,track_index,
 	delta_T,delta_h,R,VT,
-	thd_d,thd_h,thd_r,
+	tas_flag,thd_d,thd_h,thd_r,
 	min_d_flag);
 RDP_RECORD_DEFINE(RdpRecAssoInfo);
 

data_process_class_dll/rdp_record_entity.h

@@ -407,6 +407,8 @@ struct RdpRecAssoInfo
 	double R;
 	double VT;
 
+	bool tas_flag;  //TAS关联标志 1是 0否
+
 	bool thd_d;
 	bool thd_h;
 	bool thd_r;
@@ -432,6 +434,7 @@ struct RdpRecAssoInfo
 		delta_h = p.delta_h;
 		R = p.R;
 		VT = p.VT;
+		tas_flag = p.tas_flag;
 		thd_d = p.thd_d;
 		thd_h = p.thd_h;
 		thd_r = p.thd_r;

data_process_class_dll/struct.h

@@ -157,6 +157,8 @@ struct Asso_Info
 	double R;
 	double VT;
 
+	bool tas_flag;  //TAS关联标志 1是 0否
+
 	bool thd_d;
 	bool thd_h;
 	bool thd_r;

data_process_class_dll/track_asso.cpp

@@ -20,6 +20,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv>               *point_
 									std::vector <Trust_Track>            *trust_track,                            //航迹文件
 									struct  Track                    Trust_Track_Output[MAX_TRACK_NUM][10],   //更新航迹信息
 									int                              *Trust_track_num_Output,                 //更新航迹数
+									long long 								latest_timestamp,                        //最新时间戳
 									struct  RadarPara                Work_Parameter                           //雷达参数
 									)
 {
@@ -35,7 +36,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv>               *point_
 	//IMM算法
 	model_interaction(trust_track);
 
-	model_filter(trust_track, Work_Parameter);
+	model_filter(trust_track, Work_Parameter, latest_timestamp);
 
 	model_output(trust_track);
 
@@ -429,7 +430,7 @@ void Track_Asso::IMM_d_cal(double v_track,
 
 }
 
-void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  RadarPara Work_Parameter)
+void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  RadarPara Work_Parameter, long long latest_timestamp)
 {
 
 	
@@ -538,6 +539,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 			associated_info_tmp.delta_h=delta_h;
 			associated_info_tmp.R=R;
 			associated_info_tmp.VT=VT;
+			associated_info_tmp.tas_flag=point_process[loop_of_point].point_type;
 			associated_info_tmp.thd_d=thd_d;
 			associated_info_tmp.thd_h=thd_h;
 			associated_info_tmp.thd_r=thd_r;
@@ -728,7 +730,11 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct  Ra
 			if((*trust_track)[i].Extrapolate_round <= MAX_EXTRAPOLATE_NUM)
 			{
 				double v_track = sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
-				double delta_T;
+				double delta_T = 0;
+				// double delta_T = (latest_timestamp - (*trust_track)[i].T_track)/1000.0;
+				// if (delta_T <= 0)
+				// 	continue;
+				
 				if(Work_Parameter.work_mode == 0)
 				{
 				delta_T = Work_Parameter.DATA_RATE_SHORT;

data_process_class_dll/track_asso.h

@@ -16,6 +16,7 @@ public:
 							   std::vector <Trust_Track>            *trust_track,                            //航迹文件
 							   struct  Track                    Trust_Track_Output[MAX_TRACK_NUM][10],   //更新航迹信息
 							   int                              *Trust_track_num_Output,                 //更新航迹数
+							   long long 								latest_timestamp,                        //最新时间戳
 							   struct  RadarPara                Work_Parameter                             //工作参数
 							   );
 
@@ -30,7 +31,8 @@ private:
 	void model_interaction(std::vector <Trust_Track>    *trust_track);               //模型交互
 
 	void model_filter(std::vector <Trust_Track>          *trust_track,           //滤波
-					  struct  RadarPara              Work_Parameter
+					  struct  RadarPara              Work_Parameter,
+					  long long							latest_timestamp
 					  );
 
 	void model_output(std::vector <Trust_Track>         *trust_track );             //模型输出

data_process_class_dll/track_asso_tas.cpp

@@ -6,6 +6,10 @@
 #include <cstring>
 #include <vector>
 #include <Eigen/Dense>
+
+#include "rdp_record.h"
+#include "rdp_record_entity.h"
+
 using namespace Eigen;
 using namespace std;
 
@@ -109,6 +113,13 @@ long long								latest_timestamp		//最新时间戳
 
 			std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim, (*trust_track)[i].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].speed_dim));
 			std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim));
+
+			(*trust_track)[i].Range = Trust_Track_Output[*Trust_track_num_Output-1][0].Range;
+			(*trust_track)[i].Azimuth = Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth;
+			(*trust_track)[i].Elevation = Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation;
+			(*trust_track)[i].Velocity = Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V;
+
+			RDP_RECORD(RdpRecTrustTrack((*trust_track)[i]));
 		}
 
 	}
@@ -240,16 +251,7 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
                                     long long									latest_timestamp		//最新时间戳
                                     )
 {
-	//存储关联信息
+	std::vector <Asso_Info> associated_info;
-	struct asso_info
-	{
-		double d1;
-		double d2;
-		double d3;
-		double d_min;
-		int point_index;
-	};
-	std::vector <asso_info> associated_info;
 
 	//航迹信息
 	double X1[6] = {0},X2[6] = {0},X3[6] = {0},P1[6][6] = {{0}},P2[6][6] = {{0}},P3[6][6] = {{0}};
@@ -327,24 +329,38 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 			thd_h = true;
 		}
 
+		struct Asso_Info associated_info_tmp;
+		associated_info_tmp.d1=d1;
+		associated_info_tmp.d2=d2;
+		associated_info_tmp.d3=d3;
+		associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
+		associated_info_tmp.track_index=tas_track_idx;
+		if(d1<=d2&&d1<=d3)
+			associated_info_tmp.d_min=d1;
+		else if(d2<=d1&& d2<=d3)
+			associated_info_tmp.d_min=d2;
+		else
+			associated_info_tmp.d_min=d3;
+		associated_info_tmp.point_id=point_process[loop_of_point].point_id;
+		associated_info_tmp.track_id=(*trust_track)[tas_track_idx].track_id;
+		associated_info_tmp.circle_id=point_process[loop_of_point].circle_id;
+		associated_info_tmp.delta_T=delta_T;
+		associated_info_tmp.delta_h=delta_h;
+		associated_info_tmp.R=0;
+		associated_info_tmp.VT=0;
+		associated_info_tmp.tas_flag=point_process[loop_of_point].point_type;
+		associated_info_tmp.thd_d=thd_d;
+		associated_info_tmp.thd_h=thd_h;
+		associated_info_tmp.thd_r=true;
+		associated_info_tmp.min_d_flag=false;
+
+
 		//小于关联门限  保存关联信息
 		if(thd_d && thd_h)
 		{
-
-			//            std::cout << "TAS asoooooo111" ;
-			struct asso_info associated_info_tmp;
-			associated_info_tmp.d1=d1;
-			associated_info_tmp.d2=d2;
-			associated_info_tmp.d3=d3;
-			associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
-			if(d1<=d2&&d1<=d3)
-				associated_info_tmp.d_min=d1;
-			else if(d2<=d1&& d2<=d3)
-				associated_info_tmp.d_min=d2;
-			else
-				associated_info_tmp.d_min=d3;
-
 			associated_info.push_back(associated_info_tmp);
+		}else{
+			RDP_RECORD(RdpRecAssoInfo(associated_info_tmp));
 		}
 	}
 
@@ -370,6 +386,8 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 		double d2=associated_info[min_index-1].d2;
 		double d3=associated_info[min_index-1].d3;
 
+		associated_info[min_index-1].min_d_flag=true;
+
 		//卡尔曼滤波
 		double Z[3]={point_process[point_index-1].range,point_process[point_index-1].azimuth,point_process[point_index-1].velocity};
 		double prt = point_process[point_index-1].PRF_index;
@@ -406,7 +424,7 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 		double Possibility3=(det_S3>1e-12)?(1.0/sqrt(pow(2*PI,3)*det_S3)*exp(-0.5*d3)):0.0;
 
 		//更新航迹
-		for(size_t i=0;i<trust_track->size();i++)
+		for(size_t i=0;i<trust_track->size();i++){
 
 			if((*trust_track)[i].Track_Index == tas_track_idx)
 			{
@@ -450,6 +468,8 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 				(*trust_track)[i].GNSS_time = point_process[point_index-1].GNSS_time;
 
 				//更新关联上的点迹信息
+				(*trust_track)[i].point_id=point_process[point_index-1].point_id;
+				(*trust_track)[i].circle_id=point_process[point_index-1].circle_id;
 				(*trust_track)[i].range_point=point_process[point_index-1].range;
 				(*trust_track)[i].azi_point=point_process[point_index-1].azimuth/PI*180;
 				(*trust_track)[i].elev_point= asin(point_process[point_index-1].height/point_process[point_index-1].range)/PI*180;
@@ -472,7 +492,12 @@ void Track_Asso_Tas::model_filter(  std::vector<Trust_Track>                  *t
 				//更新高度
 				track_hight_update(tas_track_idx,point_index,trust_track);
 			}
+		}
 
+		for(auto& info : associated_info)
+		{
+			RDP_RECORD(RdpRecAssoInfo(info));
+		}
 	}
 	//未关联上，航迹外推
 	else

data_process_class_dll/track_init.cpp

@@ -107,6 +107,15 @@ int Track_Init::track_init_process_logic(   std::vector <PointRecv>
 		std::vector<PointRecv>().swap(point_process);
 		std::vector<PointRecv>().swap((*point_recv));
 
+#ifdef RDP_RECORD_ENABLE
+		//记录临时航迹
+		for(auto& vec_t : *temp_track){
+			for(auto& t : vec_t){
+				RDP_RECORD(RdpRecTempTrack(t));
+			}
+		}
+#endif	
+
 		//临时航迹满足起始长度 转为可靠航迹
 		tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
 
@@ -115,14 +124,6 @@ int Track_Init::track_init_process_logic(   std::vector <PointRecv>
 	//消亡临时航迹
 	tmp_track_die(temp_track);
 
-#ifdef RDP_RECORD_ENABLE
-	//记录临时航迹
-	for(auto& vec_t : *temp_track){
-		for(auto& t : vec_t){
-			RDP_RECORD(RdpRecTempTrack(t));
-		}
-	}
-#endif	
 	return 0;
 
 }