368 lines
16 KiB
C++
368 lines
16 KiB
C++
#include "tas_ctrl.h"
|
|
#include "coor_trans.h"
|
|
#include "rdp_log.h"
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <vector>
|
|
#include <iomanip>
|
|
#include <sstream>
|
|
|
|
using namespace std;
|
|
|
|
TAS_Ctrl::TAS_Ctrl()
|
|
{
|
|
memset(tas_target_queue,0,RDP_TAS_QUEUE_CAPACITY*sizeof(Tracking_Target));
|
|
|
|
tas_target_num=0;
|
|
}
|
|
|
|
void TAS_Ctrl::reset()
|
|
{
|
|
memset(tas_target_queue,0,sizeof(tas_target_queue));
|
|
tas_target_num=0;
|
|
}
|
|
|
|
|
|
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,
|
|
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);
|
|
tas_target_del(trust_track, Trust_Track_Output, Trust_track_num_Output);
|
|
tas_beam_output(trust_track,Tracking_beam,latest_timestamp);
|
|
|
|
// //跟踪队列移位
|
|
// struct Tracking_Target tas_target_tmp;
|
|
// 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));
|
|
// }
|
|
// memcpy(&tas_target_queue[0], &tas_target_tmp, sizeof(Tracking_Target));
|
|
};
|
|
|
|
void TAS_Ctrl::tas_target_add(std::vector<Trust_Track> *trust_track,
|
|
struct Track Trust_Track_Output[][10],
|
|
int *Trust_track_num_Output)
|
|
{
|
|
|
|
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+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)==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) == 0)
|
|
{
|
|
for(int j=0;j<RDP_TAS_QUEUE_CAPACITY;j++)
|
|
{
|
|
if(tas_target_queue[j].empty_flag==0)// && j%2==0)
|
|
{
|
|
if (*Trust_track_num_Output >= MAX_TRACK_NUM)
|
|
break;
|
|
|
|
//插入跟踪队列
|
|
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;
|
|
|
|
//TAS跟踪标志置1
|
|
(*trust_track)[i].Track_Mode=1;
|
|
|
|
//目标跟踪状态发生改变 对外输出航迹更新信息
|
|
*Trust_track_num_Output= *Trust_track_num_Output+1;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
|
|
|
double x,y;
|
|
x=(*trust_track)[i].X[0]+(*trust_track)[i].X[1]*rdp_parameter.radar_system_delay;
|
|
y=(*trust_track)[i].X[3]+(*trust_track)[i].X[4]*rdp_parameter.radar_system_delay;
|
|
coor_trans Coor_trans;
|
|
double r, azi;
|
|
Coor_trans.cart2polar(x, y, &r, &azi);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Range=static_cast<float>(r);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azi/PI*180);
|
|
|
|
if((*trust_track)[i].Height<r)
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>(asin((*trust_track)[i].Height/r)/PI*180);
|
|
else
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Elevation = static_cast<float>((*trust_track)[i].elev_point);
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index;
|
|
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V = static_cast<float>(sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]));
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude = static_cast<float>((*trust_track)[i].Amplitude);
|
|
|
|
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].track_time=static_cast<float>((*trust_track)[i].T_track/1000.0);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].GNSS_time=(*trust_track)[i].GNSS_time;
|
|
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].z=static_cast<float>((*trust_track)[i].Height+rdp_parameter.radar_height);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * rdp_parameter.track_quality_max);
|
|
|
|
double Direction_Angle;
|
|
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
|
|
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].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].elev_point=static_cast<float>((*trust_track)[i].elev_point);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].vr_point=static_cast<float>((*trust_track)[i].vr_point);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].point_type=0;
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].prf_point = static_cast<float>((*trust_track)[i].prf_point);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].track_snr = static_cast<float>((*trust_track)[i].snr_point);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].amp_asv_point = static_cast<float>((*trust_track)[i].amp_asv_point);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].prn_fftn_point = static_cast<float>((*trust_track)[i].prn_fftn_point);
|
|
|
|
//输出平滑后的高度
|
|
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].x=static_cast<float>((*trust_track)[i].X[0]);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].y=static_cast<float>((*trust_track)[i].X[3]);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].v_x=static_cast<float>((*trust_track)[i].X[1]);
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].v_y=static_cast<float>((*trust_track)[i].X[4]);
|
|
|
|
if(acos((-x * cos(Direction_Angle) - y * sin(Direction_Angle)) / r ) < PI / 2)
|
|
{
|
|
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=1;
|
|
// if(r*abs(sin(azi - Direction_Angle))<500)
|
|
// {
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=1;
|
|
// }
|
|
// else
|
|
// {
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Direction=0;
|
|
// }
|
|
}
|
|
else
|
|
{
|
|
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].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].range_dim, (*trust_track)[i].range_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][0].range_dim));
|
|
|
|
break;
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
// 说明：TAS 自动开始（tas_auto_start）的原实现依赖改造前雷达参数结构体中的
|
|
// TAS 距离/速度/高度门限，这些参数未纳入新的 RDP_parameter，故原实现保持停用；
|
|
// 具体逻辑不再以注释代码形式保留（tas_ctrl.h 中保留声明，后续如需启用再按新参数补齐）。
|
|
|
|
int TAS_Ctrl::tas_prohibite_area(double r,double azi, double v, double h)
|
|
{
|
|
|
|
for (int i=0;i<rdp_parameter.tas_prohibite_area_num;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;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
void TAS_Ctrl::tas_target_del(std::vector<Trust_Track> *trust_track,
|
|
struct Track Trust_Track_Output[][10],
|
|
int *Trust_track_num_Output)
|
|
{
|
|
|
|
//查找TAS目标是否已经消批 若消批则移出TAS队列
|
|
for (int i=0;i<RDP_TAS_QUEUE_CAPACITY;i++)
|
|
{
|
|
if(tas_target_queue[i].empty_flag == 1)
|
|
{
|
|
//查找TAS队列里的目标是否存在
|
|
int flag=0;
|
|
for (size_t j=0;j<trust_track->size();j++){
|
|
{
|
|
if(tas_target_queue[i].Index==(*trust_track)[j].Track_Index && (*trust_track)[j].Track_Mode == 1 && (*trust_track)[j].manual_tracking_flag == 1)
|
|
{
|
|
flag=1;
|
|
}
|
|
}
|
|
}
|
|
//目标不存在 说明已消批 从队列里删除
|
|
if(flag == 0)
|
|
{
|
|
memset(&tas_target_queue[i],0,sizeof(Tracking_Target));
|
|
tas_target_num=tas_target_num-1;
|
|
}
|
|
}
|
|
}
|
|
|
|
//判断TAS目标是否满足自动跟踪条件 若不满足移除TAS队列 通知界面改变目标状态
|
|
// for ( int 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+rdp_parameter.radar_height;
|
|
|
|
// if((*trust_track)[i].manual_tracking_flag == 0 && (*trust_track)[i].Track_Mode == 1 && tas_auto_end(v,r,h) == 1) //满足结束跟踪条件
|
|
// {
|
|
|
|
// int track_index=(*trust_track)[i].Track_Index;
|
|
|
|
// for (int ii=0;ii<RDP_TAS_QUEUE_CAPACITY;ii++)
|
|
// {
|
|
// if(tas_target_queue[ii].Index == track_index)
|
|
// {
|
|
// //从队列中删除
|
|
// memset(&tas_target_queue[ii],0,sizeof(Tracking_Target));
|
|
// tas_target_num=tas_target_num-1;
|
|
|
|
// //TAS跟踪标志置零
|
|
// (*trust_track)[i].Track_Mode=0;
|
|
|
|
// //目标类型设为地面目标
|
|
// (*trust_track)[i].Target_Type = 0;
|
|
|
|
// //航迹状态发生改变 对外输出航迹更新信息
|
|
// *Trust_track_num_Output= *Trust_track_num_Output+1;
|
|
// double r_output,azmi_output;
|
|
// coor_trans Coor_trans;
|
|
// Coor_trans.cart2polar((*trust_track)[i].X[0],(*trust_track)[i].X[3],&r_output,&azmi_output);
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Point_Sum=1;
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range=static_cast<float>(r_output);
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Azimuth=static_cast<float>(azmi_output/PI*180);
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Track_Index=(*trust_track)[i].Track_Index; //航迹号
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Range_V=static_cast<float>(sqrt((*trust_track)[i].X[1]*(*trust_track)[i].X[1]+(*trust_track)[i].X[4]*(*trust_track)[i].X[4]));
|
|
// Trust_Track_Output[*Trust_track_num_Output-1][0].Amplitude=static_cast<float>((*trust_track)[i].Amplitude);
|
|
// 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+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);
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
// }
|
|
|
|
};
|
|
|
|
// 说明：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,
|
|
long long latest_timestamp)
|
|
{
|
|
|
|
long long CPI_time = 0;
|
|
long long earliest_CPI_time = LLONG_MAX;
|
|
float delta_T = 0;
|
|
float delta_T_last_track = 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<RDP_TAS_QUEUE_CAPACITY;tas_idx++)
|
|
{
|
|
if(tas_target_queue[tas_idx].empty_flag==0)
|
|
{
|
|
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_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 > rdp_parameter.data_rate_tas))
|
|
{
|
|
earliest_CPI_time = CPI_time;
|
|
H_track = (*trust_track)[track_idx].Height;
|
|
delta_T = (latest_timestamp - CPI_time) / 1000.0f;
|
|
|
|
earliest_track_idx = static_cast<int>(track_idx);
|
|
earliest_tas_queue_idx = tas_idx;
|
|
|
|
for(int ii=0;ii<6;ii++){
|
|
X_now[ii]=(*trust_track)[track_idx].X[ii];
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
//没有满足条件的TAS目标，关闭跟踪波束
|
|
if(earliest_track_idx == -1)
|
|
{
|
|
Tracking_beam->open_flag = 0;
|
|
return;
|
|
}
|
|
|
|
//更新目标最后跟踪时间
|
|
tas_target_queue[earliest_tas_queue_idx].last_track_time = latest_timestamp;
|
|
|
|
//预测目标位置 计算跟踪波束波位号 俯仰角
|
|
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=(float)range;
|
|
//目标方位
|
|
Tracking_beam->Azi=(float)amzi/PI*180;
|
|
// Tracking_beam->Azi+=6;
|
|
//目标俯仰角
|
|
double elev=asin(H_track/range)/PI*180;
|
|
|
|
// if(elev<=0)
|
|
// elev=0;
|
|
// else if(elev>=40)
|
|
// elev=40;
|
|
// else
|
|
// elev=elev;
|
|
|
|
Tracking_beam->Elev=(float)elev;
|
|
|
|
//跟踪波束类型
|
|
Tracking_beam->type=rdp_parameter.default_track_beam_type;
|
|
|
|
//跟踪目标批号
|
|
Tracking_beam->TAS_track_index = (*trust_track)[earliest_track_idx].Track_fingerprint;
|
|
|
|
//跟踪波束开关开启
|
|
Tracking_beam->open_flag=1;
|
|
|
|
// RDP_LOG_INFO("TAS <{}> output: range: {} azi: {} pit: {}", Tracking_beam->TAS_track_index, Tracking_beam->Range, Tracking_beam->Azi, Tracking_beam->Elev);
|
|
}
|