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

更新：1、修改参数配置方式，改为由配置文件输入；

Browse Source 
Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-27 10:36:14 +08:00
parent 0e325fb153
commit 423c200659
39 changed files with 19638 additions and 945 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/tas_ctrl.cpp
+32 -70
View File
@@ -9,15 +9,9 @@
using namespace std;
// parameters.h 中定义了 DATA_RATE_TAS 宏（相扫0.3/机扫0.0625），
// 而本模块的波束门控必须使用 RadarPara 结构体成员 Work_Parameter.DATA_RATE_TAS
// （由dll调用者经 track_process_parameters_initial 输入），故取消该宏定义，
// 否则 Work_Parameter.DATA_RATE_TAS 会被预处理器展开为非法常量表达式。
#undef DATA_RATE_TAS
TAS_Ctrl::TAS_Ctrl()
{
memset(tas_target_queue,0,TAS_QUEUE_LENGTH*sizeof(Tracking_Target));
memset(tas_target_queue,0,RDP_TAS_QUEUE_CAPACITY*sizeof(Tracking_Target));
tas_target_num=0;
}
@@ -33,19 +27,19 @@ void TAS_Ctrl::tas_ctrl_process(std::vector<Trust_Track> *trust_track
struct TrackingBeam *Tracking_beam,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
long long latest_timestamp)
{
if (Trust_track_num_Output == 0) return;
*Trust_track_num_Output = 0;
tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
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);
tas_target_add(trust_track, Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output,rdp_parameter);
tas_beam_output(trust_track,Tracking_beam,rdp_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_tmp, &tas_target_queue[RDP_TAS_QUEUE_CAPACITY-1], sizeof(Tracking_Target));
// for (int i=RDP_TAS_QUEUE_CAPACITY-1;i>0;i--)
// {
// memcpy(&tas_target_queue[i],&tas_target_queue[i-1],sizeof(Tracking_Target));
// }
@@ -55,26 +49,26 @@ void TAS_Ctrl::tas_ctrl_process(std::vector<Trust_Track> *trust_track
void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
for (size_t i=0;i<trust_track->size();i++)
{
double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
double h=(*trust_track)[i].Height+Work_Parameter.Height;
double h=(*trust_track)[i].Height+rdp_parameter.radar_height;
double azi = atan2((*trust_track)[i].X[3],(*trust_track)[i].X[0]);
if(azi<0)
azi=azi+2*PI;
azi=azi/PI*180;
//进入跟踪的条件： 1.手动跟踪的目标 或 满足速度、距离、高度条件满足 关联点数大于2 2.不在禁止跟踪区域
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, Work_Parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
&& tas_target_num < MAX_TAS_NUM
if( ( (*trust_track)[i].manual_tracking_flag == 1)//|| (tas_auto_start( v, r, h, rdp_parameter)==1 && (*trust_track)[i][k].associate_point_number>=2)
&& tas_target_num < rdp_parameter.tas_track_num_max
&& (*trust_track)[i].Track_Mode == 0
&& tas_prohibite_area( r, azi, v, h, Work_Parameter) == 0)
&& tas_prohibite_area( r, azi, v, h, rdp_parameter) == 0)
{
for(int j=0;j<TAS_QUEUE_LENGTH;j++)
for(int j=0;j<RDP_TAS_QUEUE_CAPACITY;j++)
{
if(tas_target_queue[j].empty_flag==0)// && j%2==0)
{
@@ -107,7 +101,7 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=static_cast<float>(asin((*trust_track)[i].Height/r_output)/PI*180);
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
break;
@@ -119,32 +113,16 @@ void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
};
//int TAS_Ctrl:: tas_auto_start(double v, double r, double h, struct RadarPara Work_Parameter)
//{
// 说明：TAS 自动开始（tas_auto_start）的原实现依赖改造前雷达参数结构体中的
// TAS 距离/速度/高度门限，这些参数未纳入新的 RDP_parameter，故原实现保持停用；
// 具体逻辑不再以注释代码形式保留（tas_ctrl.h 中保留声明，后续如需启用再按新参数补齐）。
//// if(r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
//// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) )
//// )
// if( r>Work_Parameter.R_TAS_min && r<Work_Parameter.R_TAS_max && v<Work_Parameter.V_TAS_max && v>Work_Parameter.V_TAS_min
// && ( (r<=1000 && h>=Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h>=Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h>=Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h>=Work_Parameter.TAS_Height_4) || (r>4000 && h>=Work_Parameter.TAS_Height_5) ) )
// {
// return 1;
// }
// else
// {
// return 0;
// }
//// return 0;
//};
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct RadarPara Work_Parameter)
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,const RDP_parameter& rdp_parameter)
{
for (int i=0;i<Work_Parameter.TAS_prohibite_area_num;i++)
for (int i=0;i<rdp_parameter.tas_prohibite_area_num;i++)
{
if(r<Work_Parameter.R_max_TAS_prohibited[i] && r>Work_Parameter.R_min_TAS_prohibited[i] && azi<Work_Parameter.Azimuth_max_TAS_prohibited[i] && azi>Work_Parameter.Azimuth_min_TAS_prohibited[i])
if(r<rdp_parameter.tas_prohibite_area_range_max[i] && r>rdp_parameter.tas_prohibite_area_range_min[i] && azi<rdp_parameter.tas_prohibite_area_azimuth_max[i] && azi>rdp_parameter.tas_prohibite_area_azimuth_min[i])
{
return 1;
}
@@ -157,11 +135,11 @@ int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h,struct
void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
struct Track Trust_Track_Output[][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
const RDP_parameter& rdp_parameter)
{
//查找TAS目标是否已经消批 若消批则移出TAS队列
for (int i=0;i<TAS_QUEUE_LENGTH;i++)
for (int i=0;i<RDP_TAS_QUEUE_CAPACITY;i++)
{
if(tas_target_queue[i].empty_flag == 1)
{
@@ -189,14 +167,14 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
// {
// double r=sqrt(pow((*trust_track)[i].X[0],2)+pow((*trust_track)[i].X[3],2));
// double v=sqrt(pow((*trust_track)[i].X[1],2)+pow((*trust_track)[i].X[4],2));
// double h=(*trust_track)[i].Height+Work_Parameter.Height;
// double h=(*trust_track)[i].Height+rdp_parameter.radar_height;
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,Work_Parameter) == 1) //满足结束跟踪条件
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h,rdp_parameter) == 1) //满足结束跟踪条件
// {
// int track_index=(*trust_track)[i].Track_Index;
// for (int ii=0;ii<TAS_QUEUE_LENGTH;ii++)
// for (int ii=0;ii<RDP_TAS_QUEUE_CAPACITY;ii++)
// {
// if(tas_target_queue[ii].Index == track_index)
// {
@@ -224,7 +202,7 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
// Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point=(*trust_track)[i].point_flag;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Mode=(*trust_track)[i].Track_Mode;
// Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation=static_cast<float>(asin((*trust_track)[i].Height/r_output)/PI*180);
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
// Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
// Trust_Track_Output[*Trust_track_num_Output-1][0].Target_Type=(*trust_track)[i].Target_Type;
// Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
// }
@@ -235,28 +213,12 @@ void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
};
//int TAS_Ctrl::tas_auto_end(double v, double r, double h, struct RadarPara Work_Parameter)
//{
// if((r<=1000 && h<Work_Parameter.TAS_Height_1) || (r<=2000 && r>1000 && h<Work_Parameter.TAS_Height_2) || (r<=3000 && r>2000 && h<Work_Parameter.TAS_Height_3) || (r<=4000 && r>3000 && h<Work_Parameter.TAS_Height_4) || (r>4000 && h<Work_Parameter.TAS_Height_5)
// || v<Work_Parameter.V_TAS_min || v>Work_Parameter.V_TAS_max
// )
// {
// return 1;
// }
// else
// {
// return 0;
// }
// return 0;
//};
// 说明：TAS 自动结束（tas_auto_end）的原实现同样依赖未纳入 RDP_parameter 的
// TAS 距离/速度/高度门限，故保持停用（tas_ctrl.h 中保留声明，实现待后续补齐）。
void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track,
struct TrackingBeam *Tracking_beam,
struct RadarPara Work_Parameter,
const RDP_parameter& rdp_parameter,
long long latest_timestamp)
{
@@ -270,7 +232,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
double X_now[6] = {0};
//查找TAS队列中CPI时间最早的目标且满足TAS数据率的目标
for(int tas_idx=0;tas_idx<TAS_QUEUE_LENGTH;tas_idx++)
for(int tas_idx=0;tas_idx<RDP_TAS_QUEUE_CAPACITY;tas_idx++)
{
if(tas_target_queue[tas_idx].empty_flag==0)
{
@@ -285,7 +247,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
delta_T_last_track = (latest_timestamp - tas_target_queue[tas_idx].last_track_time) / 1000.0f;
//查找CPI时间最早，且满足TAS数据率的目标
if(CPI_time < earliest_CPI_time && (delta_T_last_track > Work_Parameter.DATA_RATE_TAS))
if(CPI_time < earliest_CPI_time && (delta_T_last_track > rdp_parameter.data_rate_tas))
{
earliest_CPI_time = CPI_time;
H_track = (*trust_track)[track_idx].Height;
@@ -339,7 +301,7 @@ void TAS_Ctrl::tas_beam_output(std::vector<Trust_Track> *trust_track
Tracking_beam->Elev=(float)elev;
//跟踪波束类型
Tracking_beam->type=1;
Tracking_beam->type=rdp_parameter.default_track_beam_type;
//跟踪目标批号
Tracking_beam->TAS_track_index = (*trust_track)[earliest_track_idx].Track_Index;