372 lines
12 KiB
C++
372 lines
12 KiB
C++
#include "data_process.h"
|
||
#include "rdp_log.h"
|
||
#include "rdp_record.h"
|
||
#include "rdp_parameter_loader.h"
|
||
#include <cstring>
|
||
#include <string>
|
||
#include <vector>
|
||
#include <cmath>
|
||
#include <iostream>
|
||
|
||
using namespace std;
|
||
|
||
// 全局工作参数对象（声明见 struct.h）。所有处理模块直接访问，由 parameters_reload() 加载。
|
||
RDP_parameter rdp_parameter{};
|
||
/*******************************************************************************/
|
||
/*******************************************************************************/
|
||
/************************************用户接口函数*******************************/
|
||
/*******************************************************************************/
|
||
/*******************************************************************************/
|
||
|
||
//数据预处理
|
||
int Data_Process::data_preprocess(struct DataRev Data_Input[150])
|
||
{
|
||
// 更新当前系统时间
|
||
latest_timestamp = Data_Input[0].CPI_time;
|
||
frame_id++;
|
||
|
||
//数据存入缓存区域 Data_buffer
|
||
if(Data_Input[0].point_type == 0) //tws数据
|
||
{
|
||
data_num=min(Data_Input[0].Point_Sum, 150);
|
||
//std::cout << "TARGET azi beam idx:" <<Data_Input[0].Beam_index_aiz;
|
||
#ifdef RDP_RECORD_ENABLE
|
||
//记录输入点迹：处理线程内先收集源结构体，循环结束后一次入队（构造实体与写库由记录写线程完成）
|
||
std::vector<PointRecv> rec_points;
|
||
rec_points.reserve(static_cast<std::size_t>(data_num));
|
||
#endif
|
||
for (int i=0; i<data_num;i++)
|
||
{
|
||
PointRecv Data_buffer_temp;
|
||
Data_buffer_temp.point_id = point_id++;
|
||
Data_buffer_temp.frame_id = frame_id;
|
||
Data_buffer_temp.circle_id = circle_id;
|
||
Data_buffer_temp.amplitude=Data_Input[i].Amplitude;
|
||
Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI;
|
||
Data_buffer_temp.SNR = Data_Input[i].Snr;
|
||
Data_buffer_temp.RCS = Data_Input[i].RCS;
|
||
Data_buffer_temp.CPI_time = Data_Input[i].CPI_time;
|
||
Data_buffer_temp.GNSS_time = Data_Input[i].GNSS_time;
|
||
Data_buffer_temp.freq_index = Data_Input[i].Freq_index;
|
||
Data_buffer_temp.PRF_index = Data_Input[i].PRI;
|
||
Data_buffer_temp.range = Data_Input[i].Range;
|
||
Data_buffer_temp.height= Data_Input[i].Range*sin(Data_Input[i].Elevation/180*PI);
|
||
Data_buffer_temp.use_flag = 0;
|
||
Data_buffer_temp.velocity = Data_Input[i].Velocity;
|
||
Data_buffer_temp.encoder_value = Data_Input[i].Encoder_value;
|
||
Data_buffer_temp.pitch_beam_index = Data_Input[i].pitch_num;
|
||
Data_buffer_temp.azimuth_beam_index = Data_Input[i].Beam_index_aiz;
|
||
Data_buffer_temp.point_type = Data_Input[i].point_type;
|
||
Data_buffer_temp.amp_asv_point = Data_Input[i].amp_asv;
|
||
Data_buffer_temp.prn_fftn_point = Data_Input[i].prn_fftn;
|
||
|
||
std::memcpy(Data_buffer_temp.speed_dim, Data_Input[i].speed_dim, sizeof(Data_buffer_temp.speed_dim));
|
||
std::memcpy(Data_buffer_temp.range_dim, Data_Input[i].range_dim, sizeof(Data_buffer_temp.range_dim));
|
||
|
||
#ifdef RDP_RECORD_ENABLE
|
||
rec_points.push_back(Data_buffer_temp);
|
||
#endif
|
||
|
||
Data_buffer.push_back(Data_buffer_temp);
|
||
}
|
||
|
||
#ifdef RDP_RECORD_ENABLE
|
||
//判圈之前（所有 return 之前）把本帧收集的点迹一次入队
|
||
RDP_RECORD_POINTS(rec_points);
|
||
#endif
|
||
|
||
// 判圈：扫描模式由配置参数 scan_mode 决定（0=相扫，1=机扫），替代原编译期 PHASE_SCANNING/MECHANICAL_SCANNING 分支。
|
||
// 相扫：判断前后两帧波位号的大小，来判断是否扫描一周完成（当前波位号 <= 上一帧波位号即判满圈）；
|
||
// 机扫：可能存在转台反转导致波位号变小但并未扫描一周，因此改为波位号为 0 时判满圈。
|
||
const bool circle_done = (rdp_parameter.scan_mode == 0)
|
||
? (Data_Input[0].Beam_index_aiz <= last_beam_num) //TWS 相扫
|
||
: (Data_Input[0].Beam_index_aiz == 0); //TWS 机扫
|
||
|
||
last_beam_num = Data_Input[0].Beam_index_aiz;
|
||
if(circle_done)
|
||
{
|
||
circle_id++;
|
||
// std::cout << Data_Input[0].CPI_time << std::endl;
|
||
// RDP_LOG<<"CPI_time: "<<Data_Input[0].CPI_time<<std::endl;
|
||
return 1;
|
||
}
|
||
else
|
||
{
|
||
return 2;
|
||
}
|
||
}
|
||
else if(Data_Input[0].point_type == 1) //tas数据
|
||
{
|
||
// std::cout << "TAS TARGET :" <<Data_Input[0].TAS_track_index;
|
||
//std::cout << "TAS time :" <<Data_Input[0].CPI_time;
|
||
|
||
data_num = min(max(Data_Input[0].Point_Sum, 0), 150);
|
||
TAS_track_idx = Data_Input[0].TAS_track_index;
|
||
#ifdef RDP_RECORD_ENABLE
|
||
//记录输入点迹：处理线程内先收集源结构体，循环结束后一次入队（构造实体与写库由记录写线程完成）
|
||
std::vector<PointRecv> rec_points;
|
||
rec_points.reserve(static_cast<std::size_t>(data_num));
|
||
#endif
|
||
for (int i=0; i<data_num;i++)
|
||
{
|
||
PointRecv Data_buffer_temp;
|
||
Data_buffer_temp.point_id = point_id++;
|
||
Data_buffer_temp.frame_id = frame_id;
|
||
Data_buffer_temp.circle_id = circle_id;
|
||
Data_buffer_temp.amplitude=Data_Input[i].Amplitude;
|
||
Data_buffer_temp.azimuth=Data_Input[i].Azimuth/180*PI;
|
||
Data_buffer_temp.SNR = Data_Input[i].Snr;
|
||
Data_buffer_temp.RCS = Data_Input[i].RCS;
|
||
Data_buffer_temp.CPI_time = Data_Input[i].CPI_time;
|
||
Data_buffer_temp.GNSS_time = Data_Input[i].GNSS_time;
|
||
Data_buffer_temp.freq_index = Data_Input[i].Freq_index;
|
||
Data_buffer_temp.PRF_index = Data_Input[i].PRI;
|
||
Data_buffer_temp.range = Data_Input[i].Range;
|
||
Data_buffer_temp.height= Data_Input[i].Range*sin(Data_Input[i].Elevation/180*PI);
|
||
Data_buffer_temp.use_flag = 0;
|
||
Data_buffer_temp.velocity = Data_Input[i].Velocity;
|
||
Data_buffer_temp.pitch_beam_index = Data_Input[i].pitch_num;
|
||
Data_buffer_temp.encoder_value = Data_Input[i].Encoder_value;
|
||
Data_buffer_temp.azimuth_beam_index = Data_Input[i].Beam_index_aiz;
|
||
Data_buffer_temp.point_type = Data_Input[i].point_type;
|
||
Data_buffer_temp.amp_asv_point = Data_Input[i].amp_asv;
|
||
Data_buffer_temp.prn_fftn_point = Data_Input[i].prn_fftn;
|
||
|
||
std::memcpy(Data_buffer_temp.speed_dim, Data_Input[i].speed_dim, sizeof(Data_buffer_temp.speed_dim));
|
||
std::memcpy(Data_buffer_temp.range_dim, Data_Input[i].range_dim, sizeof(Data_buffer_temp.range_dim));
|
||
|
||
#ifdef RDP_RECORD_ENABLE
|
||
rec_points.push_back(Data_buffer_temp);
|
||
#endif
|
||
|
||
Data_buffer_tas.push_back(Data_buffer_temp);
|
||
|
||
// std::cout << "Azimuth:" << Data_Input[i].Azimuth
|
||
// << "Range:" << Data_Input[i].Range
|
||
// << "EL:" << Data_Input[i].Elevation
|
||
// << "CPI_Time"<< Data_Input[i].CPI_time
|
||
// << "data_num:" << data_num;
|
||
|
||
}
|
||
//circle_id++ 之前把本帧收集的点迹一次入队
|
||
#ifdef RDP_RECORD_ENABLE
|
||
RDP_RECORD_POINTS(rec_points);
|
||
#endif
|
||
circle_id++;
|
||
return 3;
|
||
}
|
||
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
}
|
||
|
||
//数据处理
|
||
int Data_Process::track_process(struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
|
||
int *Trust_track_num_Output,
|
||
int Track_die_Index_Output[],
|
||
int *Track_die_num_Output,
|
||
int model)
|
||
{
|
||
|
||
//TWS处理
|
||
if(model==2)
|
||
{
|
||
|
||
dot_coh.dot_coh_process_buff(&Data_buffer,&point_recv);
|
||
std::vector<PointRecv>().swap(Data_buffer);
|
||
}
|
||
|
||
if(model==1)
|
||
{
|
||
*Trust_track_num_Output=0; //航迹更新数
|
||
*Track_die_num_Output=0; //航迹消亡数目
|
||
|
||
track_asso.track_asso_process(&point_recv,
|
||
&trust_track,
|
||
Trust_Track_Output,
|
||
Trust_track_num_Output,
|
||
latest_timestamp
|
||
);
|
||
|
||
track_die.track_die_process( &trust_track,
|
||
Track_die_Index_Output,
|
||
Track_die_num_Output);
|
||
|
||
track_init.track_init_process_logic( &point_recv,
|
||
&trust_track,
|
||
&temp_track,
|
||
Trust_Track_Output,
|
||
Trust_track_num_Output);
|
||
|
||
}
|
||
|
||
// TAS处理
|
||
if(model==3)
|
||
{
|
||
*Trust_track_num_Output=0; //航迹更新数
|
||
*Track_die_num_Output=0; //航迹消亡数目
|
||
|
||
dot_coh_tas.dot_coh_tas_process(&Data_buffer_tas,&point_recv_tas); //点迹凝聚
|
||
|
||
std::vector<PointRecv>().swap(Data_buffer_tas);//凝聚完毕 清除Data_buffer
|
||
|
||
track_asso_tas.track_asso_process_tas(&point_recv_tas, //点迹文件
|
||
&trust_track, //航迹文件
|
||
Trust_Track_Output, //更新航迹信息
|
||
Trust_track_num_Output, //更新航迹数
|
||
TAS_track_idx,
|
||
latest_timestamp //最新时间戳
|
||
);
|
||
|
||
std::vector<PointRecv>().swap(point_recv_tas); //清除 point_recv_tas
|
||
|
||
track_die_tas.track_die_process_tas( &trust_track,
|
||
Track_die_Index_Output,
|
||
Track_die_num_Output);
|
||
|
||
//std::cout << "Extrapolate_round :" << trust_track[TAS_track_idx].Extrapolate_round;
|
||
}
|
||
|
||
return 1;
|
||
}
|
||
|
||
//波束控制
|
||
void Data_Process::Beam_Ctrl(struct TrackingBeam *Tracking_beam,
|
||
struct Track Trust_Track_Output[][10],
|
||
int *Trust_track_num_Output)
|
||
{
|
||
|
||
tas_ctrl.tas_ctrl_process(&trust_track,Tracking_beam, Trust_Track_Output, Trust_track_num_Output, latest_timestamp);
|
||
}
|
||
|
||
//数据处理参数加载（从 TOML 配置文件加载全部工作参数到全局 rdp_parameter）
|
||
int Data_Process::parameters_reload(const char* parameter_file_path)
|
||
{
|
||
std::string error;
|
||
// rdp_parameter_load 仅在完整校验通过后写入目标对象，失败时保持原参数不变
|
||
if (rdp_parameter_load(parameter_file_path, rdp_parameter, &error) != 0)
|
||
{
|
||
RDP_LOG << "parameters_reload failed: " << error << std::endl;
|
||
return -1;
|
||
}
|
||
|
||
RDP_LOG << "parameters_reload ok: " << parameter_file_path << std::endl;
|
||
return 0;
|
||
}
|
||
|
||
//航迹清空函数
|
||
int Data_Process::track_clear_all(void)
|
||
{
|
||
//清空可靠航迹
|
||
Data_buffer.clear();
|
||
Data_buffer_tas.clear();
|
||
point_recv.clear();
|
||
point_recv_tas.clear();
|
||
trust_track.clear();
|
||
temp_track.clear();
|
||
|
||
// 同步清理内部缓存和状态，避免重连后输出幽灵航迹/幽灵波束
|
||
dot_coh.reset();
|
||
track_asso.reset();
|
||
track_asso_tas.reset();
|
||
track_init.reset();
|
||
tas_ctrl.reset();
|
||
last_beam_num = INT_MAX;
|
||
|
||
return 0;
|
||
}
|
||
|
||
//手动航迹删除函数
|
||
int Data_Process:: track_delete(int delete_track_num, //手动删除的航迹数目
|
||
int delete_track_index[]) //手动删除的航迹号
|
||
{
|
||
for (int i=0;i<delete_track_num ;i++)
|
||
{
|
||
for (size_t j=0;j<trust_track.size();j++)
|
||
{
|
||
if(trust_track[j].Track_Index == delete_track_index[i])
|
||
{
|
||
|
||
trust_track[j].manual_delete_flag=1;
|
||
// RDP_LOG << "Delete :" << trust_track[j].Track_Index << std::endl;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
//手动添加重点目标
|
||
int Data_Process::primary_target_add(int track_index)
|
||
{
|
||
|
||
for ( size_t i=0; i<trust_track.size();i++){
|
||
if(trust_track[i].Track_Index==track_index)
|
||
{
|
||
trust_track[i].primary_target_flag = 1;
|
||
//std::cout<< "Add primary target :" << trust_track[i].Track_Index << std::endl;
|
||
break;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
//手动取消重点目标
|
||
int Data_Process::primary_target_cancel(int track_index)
|
||
{
|
||
|
||
for ( size_t i=0; i<trust_track.size();i++){
|
||
if(trust_track[i].Track_Index==track_index)
|
||
{
|
||
trust_track[i].primary_target_flag = 0;
|
||
//std::cout<< "Cancel primary target :" << trust_track[i].Track_Index << std::endl;
|
||
break;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
//手动转TAS跟踪函数
|
||
int Data_Process:: tracking_start(int track_index) //需要手动转入TAS跟踪的航迹号
|
||
{
|
||
|
||
for ( size_t i=0; i<trust_track.size();i++){
|
||
if(trust_track[i].Track_Index==track_index)
|
||
{
|
||
trust_track[i].manual_tracking_flag = 1;
|
||
break;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
//手动取消TAS跟踪函数
|
||
int Data_Process:: tracking_stop(int track_index) //需要手动转入TAS跟踪的航迹号
|
||
{
|
||
|
||
for ( size_t i=0; i<trust_track.size();i++){
|
||
if(trust_track[i].Track_Index==track_index)
|
||
{
|
||
trust_track[i].manual_tracking_flag = 0;
|
||
trust_track[i].Track_Mode = 0;
|
||
break;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
//手动打跟踪波束
|
||
int Data_Process::tracking_point(float Azimuth)//方位角（当前接口未启用，保留空实现）
|
||
{
|
||
return 0;
|
||
}
|
||
|