From f438ac53d98368590f20fedd82429e450cb433f4 Mon Sep 17 00:00:00 2001 From: waiwaylee Date: Mon, 7 Sep 2026 09:48:58 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=9A1=E3=80=81=E4=BC=98?= =?UTF-8?q?=E5=8C=96TAS=E9=80=BB=E8=BE=91=202=E3=80=81=E4=BC=98=E5=8C=96?= =?UTF-8?q?=E5=AE=8F=E5=AE=9A=E4=B9=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: waiwaylee --- data_process_class_dll/data_process.cpp | 1 - data_process_class_dll/parameters.h | 8 +- data_process_class_dll/struct.h | 2 +- data_process_class_dll/tas_ctrl.cpp | 145 ++++++++++-------- .../track_asso_direct_tracking.cpp | 2 +- data_process_class_dll/track_init.cpp | 72 +++------ 6 files changed, 104 insertions(+), 126 deletions(-) diff --git a/data_process_class_dll/data_process.cpp b/data_process_class_dll/data_process.cpp index 978bc2c..dfe3b1c 100644 --- a/data_process_class_dll/data_process.cpp +++ b/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 0) \ diff --git a/data_process_class_dll/struct.h b/data_process_class_dll/struct.h index 896bc26..1f7dc4a 100644 --- a/data_process_class_dll/struct.h +++ b/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; //最新跟踪时间 }; //引导跟踪目标结构体 diff --git a/data_process_class_dll/tas_ctrl.cpp b/data_process_class_dll/tas_ctrl.cpp index 5b61332..0f95e62 100644 --- a/data_process_class_dll/tas_ctrl.cpp +++ b/data_process_class_dll/tas_ctrl.cpp @@ -39,14 +39,14 @@ void TAS_Ctrl::tas_ctrl_process(std::vector *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; - memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target)); - 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[0], &tas_target_tmp, sizeof(Tracking_Target)); + // //跟踪队列移位 + // struct Tracking_Target tas_target_tmp; + // memcpy(&tas_target_tmp, &tas_target_queue[TAS_QUEUE_LENGTH-1], sizeof(Tracking_Target)); + // 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[0], &tas_target_tmp, sizeof(Tracking_Target)); }; void TAS_Ctrl::tas_target_add(std::vector *trust_track, @@ -81,6 +81,7 @@ void TAS_Ctrl::tas_target_add(std::vector *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; @@ -255,61 +256,76 @@ void TAS_Ctrl::tas_beam_output(std::vector *trust_track struct RadarPara Work_Parameter, long long latest_timestamp) { - if(tas_target_queue[0].empty_flag==1) + + long long CPI_time = 0; + long long earliest_CPI_time = LLONG_MAX; + 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_idxsize();i++ ) + if(tas_target_queue[tas_idx].empty_flag==0) { - if((*trust_track)[i].Track_Index == tas_target_queue[0].Index) + continue; + } + + for (size_t track_idx=0;track_idxsize();track_idx++ ) + { + if((*trust_track)[track_idx].Track_Index == tas_target_queue[tas_idx].Index) { - H_track = (*trust_track)[i].Height; - CPI_time = (*trust_track)[i].T_track; - for(int ii=0;ii<6;ii++) - X_now[ii]=(*trust_track)[i].X[ii]; - found_track = true; + 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) - { - Tracking_beam->open_flag = 0; - return; - } + //没有满足条件的TAS目标，关闭跟踪波束 + if(earliest_track_idx == -1) + { + Tracking_beam->open_flag = 0; + return; + } - //数据率门控：航迹时间与当前最新时间戳的差值大于 1/DATA_RATE_TAS 时才输出TAS跟踪波束， - //保证TAS目标数据率不随波束时间变化（使用Work_Parameter中的DATA_RATE_TAS参数值）。 - //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; - } - } + //更新目标最后跟踪时间 + tas_target_queue[earliest_tas_queue_idx].last_track_time = latest_timestamp; - float delta_T = (latest_timestamp - CPI_time) / 1000.0f; + //预测目标位置 计算跟踪波束波位号 俯仰角 + double x_track=X_now[0]+X_now[1]*delta_T; + double y_track=X_now[3]+X_now[4]*delta_T; + double amzi,range; + coor_trans Coor_trans; + Coor_trans.cart2polar(x_track,y_track,&range,&amzi); - //预测目标位置 计算跟踪波束波位号 俯仰角 - double x_track=X_now[0]+X_now[1]*delta_T; - double y_track=X_now[3]+X_now[4]*delta_T; - double amzi,range; - coor_trans Coor_trans; - Coor_trans.cart2polar(x_track,y_track,&range,&amzi); - - //目标距离 - Tracking_beam->Range=range; - //目标方位 - Tracking_beam->Azi=amzi/PI*180; + //目标距离 + Tracking_beam->Range=(float)range; + //目标方位 + Tracking_beam->Azi=(float)amzi/PI*180; // Tracking_beam->Azi+=6; - //目标俯仰角 - double elev=asin(H_track/range)/PI*180; + //目标俯仰角 + double elev=asin(H_track/range)/PI*180; // if(elev<=0) // elev=0; @@ -318,23 +334,16 @@ void TAS_Ctrl::tas_beam_output(std::vector *trust_track // else // elev=elev; - Tracking_beam->Elev=elev; + Tracking_beam->Elev=(float)elev; - //跟踪波束类型 - Tracking_beam->type=1; + //跟踪波束类型 + 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: " <Range << "azi: " <Azi <<"pit: " <Elev; - } - else - { - //跟踪波束开关关闭 - Tracking_beam->open_flag=0; - } + //跟踪波束开关开启 + Tracking_beam->open_flag=1; + //std::cout << "TAS: range: " <Range << "azi: " <Azi <<"pit: " <Elev; } diff --git a/data_process_class_dll/track_asso_direct_tracking.cpp b/data_process_class_dll/track_asso_direct_tracking.cpp index c25dab7..a8df218 100644 --- a/data_process_class_dll/track_asso_direct_tracking.cpp +++ b/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); diff --git a/data_process_class_dll/track_init.cpp b/data_process_class_dll/track_init.cpp index 0aff6d2..699ca0e 100644 --- a/data_process_class_dll/track_init.cpp +++ b/data_process_class_dll/track_init.cpp @@ -342,64 +342,30 @@ void Track_Init::point_track_head_asso( std::vector > 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; -// } + //距离差 + double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); -// 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 vmax; + if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点 + { + vmax = Work_Parameter.V_MAX; + } + else //中远程模式 最大速度正常用 + { + vmax = Work_Parameter.V_MAX; + } - //距离差 - double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2)); - - double vmax; - if(Work_Parameter.work_mode == 0) //近程模式 最大速度减小一点 - { - vmax = Work_Parameter.V_MAX; - } - else //中远程模式 最大速度正常用 - { - vmax = Work_Parameter.V_MAX; - } - - double delta_T = (T_point - T_track_head)/1000.0; + double delta_T = (T_point - T_track_head)/1000.0; //满足关联条件的点航 // if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0) ) // && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E - if( - ( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) - || (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) - ) - && 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; -// } + if( + ( (Work_Parameter.work_mode == 0&&( (r_point>=1000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) + || (Work_Parameter.work_mode != 0&&( (r_point>=2000 && dis<=vmax*delta_T && dis>=Work_Parameter.V_MIN*delta_T) || (r_point<2000 && dis<=vmax*delta_T/3.0 && dis>=Work_Parameter.V_MIN*delta_T/2.0) )) + ) + && vr_point*v_track_head>0 ) + { struct Asso_info asso_info_tmp; asso_info_tmp.point_idx = i+1; @@ -408,7 +374,7 @@ void Track_Init::point_track_head_asso( std::vector > (*temp_track)[j][0].asso_flag = 1; point_process[i].Use_Flag = 1; //std::cout << "v_track_head: " << v_track_head << "vr_point: " << vr_point; - } + } } }