waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新：1、优化TAS逻辑

Browse Source 
2、优化宏定义

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-09-07 09:48:58 +08:00
parent e729c5baee
commit f438ac53d9
6 changed files with 104 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/data_process.cpp
-1
View File
@@ -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
+6 -2
View File
@@ -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
+1 -1
View File
@@ -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
+56 -47
View File
@@ -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;
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> *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;
for (int i=0;i<trust_track->size();i++ )
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_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;
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;
continue;
}
for (size_t track_idx=0;track_idx<trust_track->size();track_idx++ )
{
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参数值）。
//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;
//更新目标最后跟踪时间
tas_target_queue[earliest_tas_queue_idx].last_track_time = latest_timestamp;
//预测目标位置 计算跟踪波束波位号 俯仰角
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
+1 -1
View File
@@ -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
-34
View File
@@ -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;