更新:1、数据处理增加版本号输出;
2、增加Debug模式下可靠航迹、关联信息等数据写入数据库; Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
@@ -67,6 +67,8 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
|
|||||||
{
|
{
|
||||||
last_beam_num = Data_Input[0].Beam_index_aiz;
|
last_beam_num = Data_Input[0].Beam_index_aiz;
|
||||||
circle_id++;
|
circle_id++;
|
||||||
|
// std::cout << Data_Input[0].CPI_time << std::endl;
|
||||||
|
// RDP_LOG<<"CPI_time: "<<Data_Input[0].CPI_time<<std::endl;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@@ -133,35 +135,6 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150])
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef RDP_RECORD_ENABLE
|
|
||||||
//数据记录示例②:把本圈输出的航迹批量写入表 RdpRecTrackOut(TWS/TAS 共用一张表,用 track_mode 区分)。
|
|
||||||
//该辅助函数只在记录功能开启时编译(RDP_RECORD_ENABLE 由构建系统定义),交付构建完全不参与编译
|
|
||||||
static void rdp_record_output_tracks(struct Track Trust_Track_Output[][10], int track_num)
|
|
||||||
{
|
|
||||||
if (track_num <= 0)
|
|
||||||
{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (track_num > MAX_TRACK_NUM) //防御:输出行数理论上不应超过航迹容量(见 README 11.2 BUG-08)
|
|
||||||
{
|
|
||||||
track_num = MAX_TRACK_NUM;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<RdpRecTrackOut> recs;
|
|
||||||
recs.reserve(static_cast<size_t>(track_num));
|
|
||||||
for (int i = 0; i < track_num; ++i)
|
|
||||||
{
|
|
||||||
const struct Track &t = Trust_Track_Output[i][0]; //每行第 0 列为该航迹的主记录
|
|
||||||
recs.push_back(RdpRecTrackOut{0, t.Track_Index, t.Track_Mode, t.Point_Sum, t.Flag_Point,
|
|
||||||
t.Flag_Direction, t.GNSS_time, t.track_time,
|
|
||||||
t.x, t.y, t.z, t.v_x, t.v_y, t.v_z,
|
|
||||||
t.Range, t.Azimuth, t.Elevation, t.Range_V,
|
|
||||||
t.track_snr, t.track_rcs});
|
|
||||||
}
|
|
||||||
RDP_RECORD_BATCH(recs);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//数据处理
|
//数据处理
|
||||||
int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||||||
int *Trust_track_num_Output,
|
int *Trust_track_num_Output,
|
||||||
@@ -231,14 +204,6 @@ int model)
|
|||||||
//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
|
//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
|
||||||
}
|
}
|
||||||
|
|
||||||
//数据记录示例②:本圈输出的航迹(仅 TWS/TAS 处理帧有输出;记录功能关闭时不产生任何代码)
|
|
||||||
#ifdef RDP_RECORD_ENABLE
|
|
||||||
if (model == 1 || model == 3)
|
|
||||||
{
|
|
||||||
rdp_record_output_tracks(Trust_Track_Output, *Trust_track_num_Output);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
#include "data_process.h"
|
#include "data_process.h"
|
||||||
#include "rdp_log.h"
|
#include "rdp_log.h"
|
||||||
#include "rdp_record.h"
|
#include "rdp_record.h"
|
||||||
|
#include "parameters.h"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Data_process_class_dll::Data_process_class_dll()
|
Data_process_class_dll::Data_process_class_dll()
|
||||||
@@ -26,6 +27,7 @@ Data_process_class_dll *Data_Process_Factory::GetB()
|
|||||||
rdp_record::Init();
|
rdp_record::Init();
|
||||||
|
|
||||||
p=new Data_Process();
|
p=new Data_Process();
|
||||||
|
p->version_info = VERSION_STRING;
|
||||||
}
|
}
|
||||||
|
|
||||||
return p;
|
return p;
|
||||||
|
|||||||
@@ -3,6 +3,7 @@
|
|||||||
#define DATA_PROCESS_CLASS_DLL_H
|
#define DATA_PROCESS_CLASS_DLL_H
|
||||||
|
|
||||||
#include "data_process_class_dll_global.h"
|
#include "data_process_class_dll_global.h"
|
||||||
|
#include <string>
|
||||||
|
|
||||||
/***************************************************** 结构体 ********************************************************/
|
/***************************************************** 结构体 ********************************************************/
|
||||||
// 输入点迹的数据结构 用户可见
|
// 输入点迹的数据结构 用户可见
|
||||||
@@ -176,6 +177,7 @@ class DATA_PROCESS_CLASS_DLLSHARED_EXPORT Data_process_class_dll
|
|||||||
|
|
||||||
public:
|
public:
|
||||||
// Data_process_class_dll();
|
// Data_process_class_dll();
|
||||||
|
const char* version_info;
|
||||||
|
|
||||||
//数据预处理函数
|
//数据预处理函数
|
||||||
virtual int data_preprocess(struct DataRev Data_Input[150]) //雷达的输入点迹 主程序创建全局变量 输入数据
|
virtual int data_preprocess(struct DataRev Data_Input[150]) //雷达的输入点迹 主程序创建全局变量 输入数据
|
||||||
|
|||||||
@@ -7,13 +7,32 @@
|
|||||||
//频点(GHz)
|
//频点(GHz)
|
||||||
#define FREQ0 9.2
|
#define FREQ0 9.2
|
||||||
|
|
||||||
|
/**********************************版本信息*************************************************/
|
||||||
|
#define MAJOR_VERSION 1
|
||||||
|
#define MINOR_VERSION 5
|
||||||
|
#define PATCH_VERSION 6
|
||||||
|
#ifdef PHASE_SCANNING
|
||||||
|
#define VERSION_INFO "PS"
|
||||||
|
#else
|
||||||
|
#define VERSION_INFO "MS"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define STRINGIFY_IMPL(x) #x
|
||||||
|
#define STRINGIFY(x) STRINGIFY_IMPL(x)
|
||||||
|
|
||||||
|
#define VERSION_STRING \
|
||||||
|
"V" STRINGIFY(MAJOR_VERSION) "." \
|
||||||
|
STRINGIFY(MINOR_VERSION) "." \
|
||||||
|
STRINGIFY(PATCH_VERSION) "." \
|
||||||
|
VERSION_INFO
|
||||||
|
|
||||||
/**********************************日志输出开关**********************************************/
|
/**********************************日志输出开关**********************************************/
|
||||||
// 日志文件输出功能开关:
|
// 日志文件输出功能开关:
|
||||||
// 1) 定义(默认)RDP_LOG_ENABLE:启用日志文件输出 —— 调用 Data_Process_Factory::GetB()
|
// 1) 定义(默认)RDP_LOG_ENABLE:启用日志文件输出 —— 调用 Data_Process_Factory::GetB()
|
||||||
// 时自动将 std::cout 重定向到日志文件,日志文件存放于 RDP_LOG_DIR 目录,
|
// 时自动将 std::cout 重定向到日志文件,日志文件存放于 RDP_LOG_DIR 目录,
|
||||||
// 文件名按当前日期+时间自动命名(如 rdp_20240826_153000.log);
|
// 文件名按当前日期+时间自动命名(如 rdp_20240826_153000.log);
|
||||||
// 2) 注释掉 RDP_LOG_ENABLE:关闭 —— 不做重定向,日志仍输出到标准输出(控制台)。
|
// 2) 注释掉 RDP_LOG_ENABLE:关闭 —— 不做重定向,日志仍输出到标准输出(控制台)。
|
||||||
//#define RDP_LOG_ENABLE
|
// #define RDP_LOG_ENABLE
|
||||||
|
|
||||||
//日志文件输出目录(相对当前工作目录的路径,目录不存在时自动创建)
|
//日志文件输出目录(相对当前工作目录的路径,目录不存在时自动创建)
|
||||||
#define RDP_LOG_DIR ".\\rdp_logs\\"
|
#define RDP_LOG_DIR ".\\rdp_logs\\"
|
||||||
|
|||||||
@@ -249,10 +249,25 @@ RDP_RECORD_DEFINE(RdpRecPointCohesionTAS);
|
|||||||
YLT_REFL(RdpRecTempTrack, id, temp_track_id,point_id,circle_id,X,P,r,azi,vr,height,SNR,RCS,amplitude, CPI_time,GNSS_time,d,buff_round,temp_track_section_idx,asso_flag,pitch_num);
|
YLT_REFL(RdpRecTempTrack, id, temp_track_id,point_id,circle_id,X,P,r,azi,vr,height,SNR,RCS,amplitude, CPI_time,GNSS_time,d,buff_round,temp_track_section_idx,asso_flag,pitch_num);
|
||||||
RDP_RECORD_DEFINE(RdpRecTempTrack);
|
RDP_RECORD_DEFINE(RdpRecTempTrack);
|
||||||
|
|
||||||
// 示例②:本圈输出的航迹
|
//可靠航迹
|
||||||
YLT_REFL(RdpRecTrackOut, id, track_index, track_mode, point_sum, flag_point, flag_direction,
|
YLT_REFL(RdpRecTrustTrack, id,track_id,point_id,circle_id,Track_Index,Range,Azimuth,Elevation,Velocity,Amplitude,Height,T_track,GNSS_time,Track_Sum,Point_Index,Track_Mode,
|
||||||
gnss_time_ms, track_time_s, x, y, z, v_x, v_y, v_z, range_m, azi_deg, elev_deg,
|
Track_Update_Flag,Extrapolate_round,point_flag,approach_flag,manual_delete_flag,manual_tracking_flag,
|
||||||
range_v_mps, snr, rcs);
|
associate_point_number,primary_target_flag,Target_Type,Track_section_idx,
|
||||||
RDP_RECORD_DEFINE(RdpRecTrackOut);
|
range_point,azi_point,elev_point,vr_point,prf_point,point_type,snr_point,
|
||||||
|
amp_asv_point,prn_fftn_point,RCS,pitch_num,X,P,X1,X2,X3,P2,P3,u);
|
||||||
|
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,
|
||||||
|
min_d_flag);
|
||||||
|
RDP_RECORD_DEFINE(RdpRecAssoInfo);
|
||||||
|
|
||||||
|
// // 示例②:本圈输出的航迹
|
||||||
|
// YLT_REFL(RdpRecTrackOut, id, track_index, track_mode, point_sum, flag_point, flag_direction,
|
||||||
|
// gnss_time_ms, track_time_s, x, y, z, v_x, v_y, v_z, range_m, azi_deg, elev_deg,
|
||||||
|
// range_v_mps, snr, rcs);
|
||||||
|
// RDP_RECORD_DEFINE(RdpRecTrackOut);
|
||||||
|
|
||||||
#endif // RDP_RECORD_ENABLE
|
#endif // RDP_RECORD_ENABLE
|
||||||
|
|||||||
@@ -280,33 +280,165 @@ struct RdpRecTempTrack
|
|||||||
};
|
};
|
||||||
RDP_RECORD_DECLARE(RdpRecTempTrack);
|
RDP_RECORD_DECLARE(RdpRecTempTrack);
|
||||||
|
|
||||||
// 示例②:本圈输出的航迹 —— 记录点位于 data_process.cpp::track_process()「数据记录示例②」处,
|
// 可靠航迹
|
||||||
// TWS/TAS 共用一张表并用 track_mode 区分;每行取输出数组的 [i][0]
|
struct RdpRecTrustTrack
|
||||||
struct RdpRecTrackOut
|
|
||||||
{
|
{
|
||||||
int64_t id; //自增主键(写入时固定填 0)
|
int64_t id; //自增主键(写入时固定填 0)
|
||||||
int track_index; //航迹号
|
long long track_id; //航迹ID
|
||||||
int track_mode; //跟踪模式 TAS 1/TWS 0
|
long long point_id; //关联上的点迹ID
|
||||||
int point_sum; //该行点数(起批输出为历史点数 L)
|
long long circle_id; //处理圈数ID
|
||||||
int flag_point; //1实点/0补点
|
int Track_Index; //航迹号
|
||||||
int flag_direction; //1靠近/0远离
|
double Range; //距离
|
||||||
int64_t gnss_time_ms; //GNSS 时间(ms)
|
double Azimuth; //方位
|
||||||
float track_time_s; //航迹时间(s)
|
double Elevation; //俯仰
|
||||||
float x; //x坐标(m)
|
double Velocity; //航向速度
|
||||||
float y; //y坐标(m)
|
double Amplitude; //幅度
|
||||||
float z; //z坐标(m)
|
double Height; //高度
|
||||||
float v_x; //x速度(m/s)
|
long long T_track; //航迹时间
|
||||||
float v_y; //y速度(m/s)
|
long long GNSS_time; //GNSS时间
|
||||||
float v_z; //z速度(m/s)
|
int Track_Sum; //航迹总数
|
||||||
float range_m; //径向距离(m)
|
int Point_Index; //该航迹上的第几个点
|
||||||
float azi_deg; //方位(度)
|
int Track_Mode; //跟踪模式
|
||||||
float elev_deg; //俯仰(度)
|
int Track_Update_Flag; //航迹更新标志 1已更新/0未更新
|
||||||
float range_v_mps; //速度(m/s,接口字段 Range_V)
|
int Extrapolate_round; //航迹持续外推时间
|
||||||
float snr; //信噪比
|
int point_flag; //是否实点 1实点 0外推
|
||||||
float rcs; //RCS
|
int approach_flag; //接近标志 1接近 0远离
|
||||||
};
|
int manual_delete_flag; //手动航迹删除
|
||||||
RDP_RECORD_DECLARE(RdpRecTrackOut);
|
int manual_tracking_flag; //手动TAS
|
||||||
|
int associate_point_number; //关联的点数
|
||||||
|
int primary_target_flag; //重点目标标记
|
||||||
|
int Target_Type; //目标类型 搜索目标 低空目标 地面目标
|
||||||
|
int Track_section_idx; //航迹区号
|
||||||
|
double range_point; //关联上的点信息(距离、方位、俯仰)
|
||||||
|
double azi_point;
|
||||||
|
double elev_point;
|
||||||
|
double vr_point;
|
||||||
|
double prf_point;
|
||||||
|
int point_type; //TWS TAS
|
||||||
|
double snr_point; //信噪比
|
||||||
|
float amp_asv_point; //关联上的点迹信息(归一化幅度)
|
||||||
|
float prn_fftn_point; //点迹速度维FFT点数
|
||||||
|
double RCS;
|
||||||
|
int pitch_num; // 俯仰波位号
|
||||||
|
|
||||||
|
std::string X; //IMM
|
||||||
|
std::string P;
|
||||||
|
|
||||||
|
std::string X1;
|
||||||
|
std::string X2;
|
||||||
|
std::string X3;
|
||||||
|
std::string P2;
|
||||||
|
std::string P3;
|
||||||
|
std::string u;
|
||||||
|
|
||||||
|
RdpRecTrustTrack(){
|
||||||
|
id = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
RdpRecTrustTrack(Trust_Track& p){
|
||||||
|
id = 0;
|
||||||
|
track_id = p.track_id;
|
||||||
|
point_id = p.point_id;
|
||||||
|
circle_id = p.circle_id;
|
||||||
|
Track_Index = p.Track_Index;
|
||||||
|
Range = p.Range;
|
||||||
|
Azimuth = p.Azimuth;
|
||||||
|
Elevation = p.Elevation;
|
||||||
|
Velocity = p.Velocity;
|
||||||
|
Amplitude = p.Amplitude;
|
||||||
|
Height = p.Height;
|
||||||
|
T_track = p.T_track;
|
||||||
|
GNSS_time = p.GNSS_time;
|
||||||
|
Track_Sum = p.Track_Sum;
|
||||||
|
Point_Index = p.Point_Index;
|
||||||
|
Track_Mode = p.Track_Mode;
|
||||||
|
Track_Update_Flag = p.Track_Update_Flag;
|
||||||
|
Extrapolate_round = p.Extrapolate_round;
|
||||||
|
point_flag = p.point_flag;
|
||||||
|
approach_flag = p.approach_flag;
|
||||||
|
manual_delete_flag = p.manual_delete_flag;
|
||||||
|
manual_tracking_flag = p.manual_tracking_flag;
|
||||||
|
associate_point_number = p.associate_point_number;
|
||||||
|
primary_target_flag = p.primary_target_flag;
|
||||||
|
Target_Type = p.Target_Type; //目标类型 搜索目标 低空目标 地面目标
|
||||||
|
Track_section_idx = p.Track_section_idx; //航迹区号
|
||||||
|
range_point = p.range_point; //关联上的点信息(距离、方位、俯仰)
|
||||||
|
azi_point = p.azi_point;
|
||||||
|
elev_point = p.elev_point;
|
||||||
|
vr_point = p.vr_point;
|
||||||
|
prf_point = p.prf_point;
|
||||||
|
point_type = p.point_type; //TWS TAS
|
||||||
|
snr_point = p.snr_point; //信噪比
|
||||||
|
amp_asv_point = p.amp_asv_point; //关联上的点迹信息(归一化幅度)
|
||||||
|
prn_fftn_point = p.prn_fftn_point; //点迹速度维FFT点数
|
||||||
|
RCS = p.RCS;
|
||||||
|
pitch_num = p.pitch_num; // 俯仰波位号
|
||||||
|
|
||||||
|
X = arrayToString(p.X); //IMM
|
||||||
|
P = arrayToString(p.P);
|
||||||
|
|
||||||
|
X1 = arrayToString(p.X1);
|
||||||
|
X2 = arrayToString(p.X2);
|
||||||
|
X3 = arrayToString(p.X3);
|
||||||
|
P2 = arrayToString(p.P2);
|
||||||
|
P3 = arrayToString(p.P3);
|
||||||
|
u = arrayToString(p.u);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
RDP_RECORD_DECLARE(RdpRecTrustTrack);
|
||||||
|
|
||||||
|
// 关联信息
|
||||||
|
struct RdpRecAssoInfo
|
||||||
|
{
|
||||||
|
int64_t id; //自增主键(写入时固定填 0)
|
||||||
|
long long point_id;
|
||||||
|
long long track_id;
|
||||||
|
long long circle_id;
|
||||||
|
|
||||||
|
double d1;
|
||||||
|
double d2;
|
||||||
|
double d3;
|
||||||
|
double d_min;
|
||||||
|
int point_index;
|
||||||
|
int track_index;
|
||||||
|
|
||||||
|
double delta_T;
|
||||||
|
double delta_h;
|
||||||
|
double R;
|
||||||
|
double VT;
|
||||||
|
|
||||||
|
bool thd_d;
|
||||||
|
bool thd_h;
|
||||||
|
bool thd_r;
|
||||||
|
|
||||||
|
bool min_d_flag;
|
||||||
|
|
||||||
|
RdpRecAssoInfo(){
|
||||||
|
id = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
RdpRecAssoInfo(Asso_Info& p){
|
||||||
|
id = 0;
|
||||||
|
point_id = p.point_id;
|
||||||
|
track_id = p.track_id;
|
||||||
|
circle_id = p.circle_id;
|
||||||
|
d1 = p.d1;
|
||||||
|
d2 = p.d2;
|
||||||
|
d3 = p.d3;
|
||||||
|
d_min = p.d_min;
|
||||||
|
point_index = p.point_index;
|
||||||
|
track_index = p.track_index;
|
||||||
|
delta_T = p.delta_T;
|
||||||
|
delta_h = p.delta_h;
|
||||||
|
R = p.R;
|
||||||
|
VT = p.VT;
|
||||||
|
thd_d = p.thd_d;
|
||||||
|
thd_h = p.thd_h;
|
||||||
|
thd_r = p.thd_r;
|
||||||
|
min_d_flag = p.min_d_flag;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
RDP_RECORD_DECLARE(RdpRecAssoInfo);
|
||||||
|
|
||||||
// ============================ 数组转换函数 ============================
|
// ============================ 数组转换函数 ============================
|
||||||
|
|
||||||
|
|||||||
@@ -39,13 +39,21 @@ struct PointRecv
|
|||||||
// 可靠航迹数据结构
|
// 可靠航迹数据结构
|
||||||
struct Trust_Track
|
struct Trust_Track
|
||||||
{
|
{
|
||||||
|
long long track_id; //航迹ID
|
||||||
|
long long point_id; //关联上的点迹ID
|
||||||
|
long long circle_id; //处理圈数ID
|
||||||
|
|
||||||
int Track_Index; //航迹号
|
int Track_Index; //航迹号
|
||||||
|
|
||||||
|
double Range; //距离
|
||||||
|
double Azimuth; //方位
|
||||||
|
double Elevation; //俯仰
|
||||||
|
double Velocity; //航向速度
|
||||||
double Amplitude; //幅度
|
double Amplitude; //幅度
|
||||||
double Height; //高度
|
double Height; //高度
|
||||||
|
|
||||||
long long T_track; //航迹时间
|
long long T_track; //航迹时间
|
||||||
long long GNSS_time; //GNSS时间
|
long long GNSS_time; //GNSS时间
|
||||||
|
|
||||||
int Track_Sum; //航迹总数
|
int Track_Sum; //航迹总数
|
||||||
int Point_Index; //该航迹上的第几个点
|
int Point_Index; //该航迹上的第几个点
|
||||||
@@ -58,8 +66,8 @@ struct Trust_Track
|
|||||||
int approach_flag; //接近标志 1接近 0远离
|
int approach_flag; //接近标志 1接近 0远离
|
||||||
|
|
||||||
int manual_delete_flag; //手动航迹删除
|
int manual_delete_flag; //手动航迹删除
|
||||||
int manual_tracking_flag; //手动TAS
|
int manual_tracking_flag; //手动TAS
|
||||||
int associate_point_number; //关联的点数
|
int associate_point_number; //关联的点数
|
||||||
|
|
||||||
int primary_target_flag; //重点目标标记
|
int primary_target_flag; //重点目标标记
|
||||||
|
|
||||||
@@ -95,7 +103,6 @@ struct Trust_Track
|
|||||||
double P2[6][6];
|
double P2[6][6];
|
||||||
double P3[6][6];
|
double P3[6][6];
|
||||||
double u[3];
|
double u[3];
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
//临时航迹结构体
|
//临时航迹结构体
|
||||||
@@ -131,6 +138,32 @@ struct Temp_track
|
|||||||
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
|
float range_dim[8]; // 目标的十字星数据,目标点距离维8个点
|
||||||
};
|
};
|
||||||
|
|
||||||
|
//存储关联信息
|
||||||
|
struct Asso_Info
|
||||||
|
{
|
||||||
|
long long point_id;
|
||||||
|
long long track_id;
|
||||||
|
long long circle_id;
|
||||||
|
|
||||||
|
double d1;
|
||||||
|
double d2;
|
||||||
|
double d3;
|
||||||
|
double d_min;
|
||||||
|
int point_index;
|
||||||
|
int track_index;
|
||||||
|
|
||||||
|
double delta_T;
|
||||||
|
double delta_h;
|
||||||
|
double R;
|
||||||
|
double VT;
|
||||||
|
|
||||||
|
bool thd_d;
|
||||||
|
bool thd_h;
|
||||||
|
bool thd_r;
|
||||||
|
|
||||||
|
bool min_d_flag;
|
||||||
|
};
|
||||||
|
|
||||||
// TAS跟踪目标结构体
|
// TAS跟踪目标结构体
|
||||||
struct Tracking_Target
|
struct Tracking_Target
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -6,6 +6,10 @@
|
|||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <Eigen/Dense>
|
#include <Eigen/Dense>
|
||||||
|
|
||||||
|
#include "rdp_record.h"
|
||||||
|
#include "rdp_record_entity.h"
|
||||||
|
|
||||||
using namespace Eigen;
|
using namespace Eigen;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
@@ -101,6 +105,11 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
|||||||
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
|
if(Direction_Angle<0){Direction_Angle=Direction_Angle+2*PI;}
|
||||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Direction_Angle=static_cast<float>(Direction_Angle/PI*180);
|
||||||
|
|
||||||
|
(*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;
|
||||||
|
|
||||||
//关联点信息
|
//关联点信息
|
||||||
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=static_cast<float>((*trust_track)[i].range_point);
|
Trust_Track_Output[*Trust_track_num_Output-1][0].range_point=static_cast<float>((*trust_track)[i].range_point);
|
||||||
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=static_cast<float>((*trust_track)[i].azi_point);
|
Trust_Track_Output[*Trust_track_num_Output-1][0].azi_point=static_cast<float>((*trust_track)[i].azi_point);
|
||||||
@@ -143,12 +152,13 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
|
|||||||
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=0;
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*trust_track)[i].RCS);
|
Trust_Track_Output[*Trust_track_num_Output-1][0].track_rcs = static_cast<float>((*trust_track)[i].RCS);
|
||||||
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
|
Trust_Track_Output[*Trust_track_num_Output-1][0].pitch_num = (*trust_track)[i].pitch_num;
|
||||||
|
|
||||||
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].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));
|
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));
|
||||||
|
|
||||||
|
RDP_RECORD(RdpRecTrustTrack((*trust_track)[i]));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -422,17 +432,8 @@ 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)
|
||||||
{
|
{
|
||||||
|
|
||||||
//存储关联信息
|
|
||||||
struct asso_info
|
std::vector <Asso_Info> associated_info;
|
||||||
{
|
|
||||||
double d1;
|
|
||||||
double d2;
|
|
||||||
double d3;
|
|
||||||
double d_min;
|
|
||||||
int point_index;
|
|
||||||
int track_index;
|
|
||||||
};
|
|
||||||
std::vector <asso_info> associated_info;
|
|
||||||
|
|
||||||
//遍历所有点迹航迹 计算点迹和航迹的统计距离
|
//遍历所有点迹航迹 计算点迹和航迹的统计距离
|
||||||
for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
|
for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
|
||||||
@@ -518,28 +519,38 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
|||||||
thd_r = false;
|
thd_r = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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=static_cast<int>(loop_of_track)+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_tmp.point_id=point_process[loop_of_point].point_id;
|
||||||
|
associated_info_tmp.track_id=(*trust_track)[loop_of_track].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=R;
|
||||||
|
associated_info_tmp.VT=VT;
|
||||||
|
associated_info_tmp.thd_d=thd_d;
|
||||||
|
associated_info_tmp.thd_h=thd_h;
|
||||||
|
associated_info_tmp.thd_r=thd_r;
|
||||||
|
associated_info_tmp.min_d_flag=false;
|
||||||
|
|
||||||
//小于关联门限 保存关联信息
|
//小于关联门限 保存关联信息
|
||||||
if(thd_d && thd_h && thd_r)
|
if(thd_d && thd_h && thd_r)
|
||||||
{
|
{
|
||||||
//std::cout << "asooooooooooo";
|
|
||||||
|
|
||||||
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=static_cast<int>(loop_of_track)+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);
|
associated_info.push_back(associated_info_tmp);
|
||||||
|
}else{
|
||||||
|
RDP_RECORD(RdpRecAssoInfo(associated_info_tmp));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//最近邻法关联
|
//最近邻法关联
|
||||||
@@ -582,11 +593,10 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
|||||||
double F[6][6], Q1[6][6], Q2[6][6], Q3[6][6];
|
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);
|
IMM_F_Q_gen( v_track, delta_T,F, Q1, Q2, Q3,Work_Parameter);
|
||||||
|
|
||||||
if (delta_T <= 0)
|
if (delta_T > 0)
|
||||||
{
|
{
|
||||||
// RDP_LOG<<"delta_T <= 0, track_index: "<<track_index<<", point_index: "<<point_index<<", delta_T: "<<delta_T<<std::endl;
|
associated_info[min_index-1].min_d_flag=true;
|
||||||
// 乱序/重复时间戳:不作为有效关联,不标记点迹已使用、不刷新航迹新鲜度
|
|
||||||
}else{
|
|
||||||
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
double X1_filter[6],X2_filter[6],X3_filter[6],P1_filter[6][6],P2_filter[6][6],P3_filter[6][6];
|
||||||
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
|
double S1[3][3] = {{0}},S2[3][3] = {{0}},S3[3][3] = {{0}};
|
||||||
kalman Kalman;
|
kalman Kalman;
|
||||||
@@ -649,6 +659,8 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
|||||||
(*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_time;
|
(*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_time;
|
||||||
|
|
||||||
//更新关联上的点迹信息
|
//更新关联上的点迹信息
|
||||||
|
(*trust_track)[track_index-1].point_id = point_process[point_index-1].point_id;
|
||||||
|
(*trust_track)[track_index-1].circle_id = point_process[point_index-1].circle_id;
|
||||||
(*trust_track)[track_index-1].range_point=point_process[point_index-1].range;
|
(*trust_track)[track_index-1].range_point=point_process[point_index-1].range;
|
||||||
(*trust_track)[track_index-1].azi_point=point_process[point_index-1].azimuth/PI*180;
|
(*trust_track)[track_index-1].azi_point=point_process[point_index-1].azimuth/PI*180;
|
||||||
(*trust_track)[track_index-1].elev_point= asin(point_process[point_index-1].height/point_process[point_index-1].range)/PI*180;
|
(*trust_track)[track_index-1].elev_point= asin(point_process[point_index-1].height/point_process[point_index-1].range)/PI*180;
|
||||||
@@ -681,6 +693,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
|||||||
{
|
{
|
||||||
if(associated_info[ii].track_index==track_index)
|
if(associated_info[ii].track_index==track_index)
|
||||||
{
|
{
|
||||||
|
RDP_RECORD(RdpRecAssoInfo(associated_info[ii]));
|
||||||
associated_info.erase(associated_info.begin()+ii);
|
associated_info.erase(associated_info.begin()+ii);
|
||||||
associated_num=static_cast<int>(associated_info.size());
|
associated_num=static_cast<int>(associated_info.size());
|
||||||
}
|
}
|
||||||
@@ -692,6 +705,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
|||||||
{
|
{
|
||||||
if(associated_info[ii].point_index==point_index)
|
if(associated_info[ii].point_index==point_index)
|
||||||
{
|
{
|
||||||
|
RDP_RECORD(RdpRecAssoInfo(associated_info[ii]));
|
||||||
associated_info.erase(associated_info.begin()+ii);
|
associated_info.erase(associated_info.begin()+ii);
|
||||||
associated_num=static_cast<int>(associated_info.size());
|
associated_num=static_cast<int>(associated_info.size());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -552,7 +552,10 @@ void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_tra
|
|||||||
Trust_Track trust_track_tmp = {};
|
Trust_Track trust_track_tmp = {};
|
||||||
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
|
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
|
||||||
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
|
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
|
||||||
|
|
||||||
|
trust_track_tmp.track_id = Track_to_start[i][L-1].temp_track_id;
|
||||||
|
trust_track_tmp.point_id = Track_to_start[i][L-1].point_id;
|
||||||
|
trust_track_tmp.circle_id = Track_to_start[i][L-1].circle_id;
|
||||||
trust_track_tmp.Amplitude=Track_to_start[i][L-1].amplitude;
|
trust_track_tmp.Amplitude=Track_to_start[i][L-1].amplitude;
|
||||||
trust_track_tmp.snr_point = Track_to_start[i][L-1].SNR;
|
trust_track_tmp.snr_point = Track_to_start[i][L-1].SNR;
|
||||||
trust_track_tmp.RCS = Track_to_start[i][L-1].RCS;
|
trust_track_tmp.RCS = Track_to_start[i][L-1].RCS;
|
||||||
|
|||||||
Reference in New Issue
Block a user