From 7c10ae919c247483cdac789585d6e3e7f1706a70 Mon Sep 17 00:00:00 2001
From: waiwaylee <waiwaylee@foxmail.com>
Date: Thu, 25 Jun 2026 11:30:05 +0800
Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=9A1=E3=80=81TWS?=
 =?UTF-8?q?=E6=A8=A1=E5=BC=8F=E4=B8=8B=EF=BC=8C=E7=82=B9=E8=88=AA=E5=85=B3?=
 =?UTF-8?q?=E8=81=94=E6=97=B6=E5=A2=9E=E5=8A=A0=E4=BF=AF=E4=BB=B0=E9=97=A8?=
 =?UTF-8?q?=E9=99=90=EF=BC=8C=E9=99=90=E5=88=B6=E4=BF=AF=E4=BB=B0=E5=A4=A7?=
 =?UTF-8?q?=E5=B9=85=E5=BA=A6=E8=B7=B3=E5=8F=98=E7=9A=84=E7=82=B9=E8=BF=B9?=
 =?UTF-8?q?=E5=85=B3=E8=81=94?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
---
 data_process_class_dll/track_asso.cpp | 120 ++++++++++++++------------
 1 file changed, 65 insertions(+), 55 deletions(-)

diff --git a/data_process_class_dll/track_asso.cpp b/data_process_class_dll/track_asso.cpp
index 44d1d1d..dbe9db6 100644
--- a/data_process_class_dll/track_asso.cpp
+++ b/data_process_class_dll/track_asso.cpp
@@ -467,61 +467,71 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct  RadarP
                 IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
 
                 //计算高度门限
-                double d_h;
-                if (r_track<1000)
-                {
-                    if(abs(h_track-h_point)<=150)
-                    {
-                       d_h = 1;
-                    }
-                    else
-                    {
-                        d_h = 0;
-                    }
+//		double d_h;
+//                if (r_track<1000)
+//                {
+//                    if(abs(h_track-h_point)<=150)
+//                    {
+//                       d_h = 1;
+//                    }
+//                    else
+//                    {
+//                        d_h = 0;
+//                    }
 
-                }
-                else if (r_track<2000 && r_track>=1000)
-                {
-                    if(abs(h_track-h_point)<=200)
-                    {
-                       d_h = 1;
-                    }
-                    else
-                    {
-                        d_h = 0;
-                    }
+//                }
+//                else if (r_track<2000 && r_track>=1000)
+//                {
+//                    if(abs(h_track-h_point)<=200)
+//                    {
+//                       d_h = 1;
+//                    }
+//                    else
+//                    {
+//                        d_h = 0;
+//                    }
 
-                }
-                else if(r_track>=2000 && r_track<3000 )
-                {
-                    if(abs(h_track-h_point)<=200)
-                    {
-                       d_h = 1;
-                    }
-                    else
-                    {
-                        d_h = 0;
-                    }
-                }
-                else if(r_track>=3000 && r_track<5000)
-                {
-                    if(abs(h_track-h_point)<=300)
-                    {
-                       d_h = 1;
-                    }
-                    else
-                    {
-                        d_h = 0;
-                    }
-                }
-                else
-                {
-                   d_h = 1;
-                }
+//                }
+//                else if(r_track>=2000 && r_track<3000 )
+//                {
+//                    if(abs(h_track-h_point)<=200)
+//                    {
+//                       d_h = 1;
+//                    }
+//                    else
+//                    {
+//                        d_h = 0;
+//                    }
+//                }
+//                else if(r_track>=3000 && r_track<5000)
+//                {
+//                    if(abs(h_track-h_point)<=300)
+//                    {
+//                       d_h = 1;
+//                    }
+//                    else
+//                    {
+//                        d_h = 0;
+//                    }
+//                }
+//                else
+//                {
+//                   d_h = 1;
+//                }
 
-                qDebug() << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
-                qDebug() << h_track<<" "<<h_point<<" "<<d_h;
-                d_h = 1;
+//                qDebug() << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
+//                qDebug() << h_track<<" "<<h_point<<" "<<d_h;
+//                d_h = 1;
+
+		//计算俯仰门限
+		bool d_p = false;
+
+		double p_track = atan2(h_track, r_track);			//航迹俯仰
+		double p_point = asin(h_point/point_process[loop_of_point].Range);			//点迹俯仰
+
+		if(abs(p_track - p_point)*180/PI <= 5){
+		    d_p = true;
+		}
 
                 //计算距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
                 double& t_x0 = (*trust_track)[loop_of_track].X[0];
@@ -549,9 +559,9 @@ void Track_Asso:: model_filter(QVector <Trust_Track> *trust_track,struct  RadarP
 
                 //小于关联门限  保存关联信息
                 if(
-                    (  r_track<=500               && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000) && d_h == 1 && d_r)//
-                    ||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100) && d_h == 1 && d_r)//
-                    ||(  r_track>=1000              && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_h == 1 && d_r)//
+		    (  r_track<=500               && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000) && d_p && d_r)//
+		    ||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100) && d_p && d_r)//
+		    ||(  r_track>=1000              && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_p && d_r)//
                 )
                 {
                      qDebug() << "asooooooooooo";