两星一地定位算法使用最新的

XdCxRhDW.Api/AddIns/定位/PosApi.cs

@@ -332,79 +332,79 @@ namespace XdCxRhDW.Api
                     //目标-超短站的时间(us)
                     var dt3 = PhysicsHelper.Dto(posXYZ, cdbXYZ) * 1e6;
                     var dto2 = Math.Abs(dt1 - dt3 - cgRes.DtoCdb.Value);
-                    if (dto2 > 100)
+                    if (dto2 > 400)
                     {
                         lat += 1;
                         if (flag < 1)
                             flag = 1;
                         continue;
                     }
-                    else if (dto2 > 50)
+                    else if (dto2 > 200)
                     {
                         lat += 0.5;
                         if (flag < 2)
                             flag = 2;
                         continue;
                     }
-                    else if (dto2 > 20)
+                    else if (dto2 > 100)
                     {
                         lat += 0.2;
                         if (flag < 3)
                             flag = 3;
                         continue;
                     }
-                    else if (dto2 > 5)
+                    else if (dto2 > 10)
                     {
-                        lat += 0.1;
+                        lat += 0.01;
                         if (flag < 4)
                             flag = 4;
                         continue;
                     }
-                    else if (dto2 > 2)
-                    {
-                        lat += 0.02;
-                        if (flag < 5)
-                            flag = 5;
-                        continue;
-                    }
+                    //else if (dto2 > 2)
+                    //{
+                    //    lat += 0.001;
+                    //    if (flag < 5)
+                    //        flag = 5;
+                    //    continue;
+                    //}
                     //目标-邻星-接收站的时间(us)
                     var dt2 = PhysicsHelper.Dto(posXYZ, (cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value), recXYZ) * 1e6;
                     var dto1 = Math.Abs(dt1 - dt2 - cgRes.Dto1.Value);
-                    if (dto1 > 100)
+                    if (dto1 > 400)
                     {
                         lat += 1;
                         if (flag < 1)
                             flag = 1;
                         continue;
                     }
-                    if (dto1 > 50)
+                    if (dto1 > 200)
                     {
                         lat += 0.5;
                         if (flag < 2)
                             flag = 2;
                         continue;
                     }
-                    else if (dto1 > 20)
+                    else if (dto1 > 100)
                     {
                         lat += 0.2;
                         if (flag < 3)
                             flag = 3;
                         continue;
                     }
-                    else if (dto1 > 5)
+                    else if (dto1 > 10)
                     {
-                        lat += 0.1;
+                        lat += 0.01;
                         if (flag < 4)
                             flag = 4;
                         continue;
                     }
-                    else if (dto1 > 2)
-                    {
-                        lat += 0.02;
-                        if (flag < 5)
-                            flag = 5;
-                        continue;
-                    }
+                    //else if (dto1 > 2)
+                    //{
+                    //    lat += 0.001;
+                    //    if (flag < 5)
+                    //        flag = 5;
+                    //    continue;
+                    //}
                     list.Add((lon, lat, dto1, dto2));
                 }
                 if (flag == 1)
@@ -414,23 +414,36 @@ namespace XdCxRhDW.Api
                 else if (flag == 3)
                     lon += 0.2;
                 else if (flag == 4)
-                    lon += 0.1;
+                    lon += 0.01;
                 else if (flag == 5)
-                    lon += 0.02;
+                    lon += 0.001;
             }
             double[] res;
-            var p1 = list.OrderBy(p => p.Item3 + p.Item4).FirstOrDefault();
+            var p1 = list.OrderBy(p => p.Item4 * p.Item4 + p.Item3 * p.Item3).FirstOrDefault();
             if (p1 == default)
             {
                 res = new double[7] { 999, 999, 0, 999, 999, 0, -1 };
                 return res;
             }
-            var p2 = list.Where(p => PhysicsHelper.DistanceGeo((p1.Item1, p1.Item2, 0), (p.Item1, p.Item2, 0)) > 10000).OrderBy(p => p.Item3 + p.Item4).FirstOrDefault();
+            var p2 = list.Where(p => PhysicsHelper.DistanceGeo((p1.Item1, p1.Item2, 0), (p.Item1, p.Item2, 0)) > 10000).OrderBy(p => p.Item4 * p.Item4 + p.Item3 * p.Item3).FirstOrDefault();
             if (p2 == default)
                 res = new double[7] { p1.Item1, p1.Item2, 0, 999, 999, 0, -1 };
             else
                 res = new double[7] { p1.Item1, p1.Item2, 0, p2.Item1, p2.Item2, 0, -1 };
-
+            if (res[3] != 999)
+            {
+                var dis1 = PhysicsHelper.DistanceArcGeo((res[3], res[4]), (sRes.CdbTxLon.Value, sRes.CdbTxLat.Value));
+                var dis2 = PhysicsHelper.DistanceArcGeo((res[0], res[1]), (sRes.CdbTxLon.Value, sRes.CdbTxLat.Value));
+                if (dis1 < dis2)
+                {
+                    var tmp1 = res[3];
+                    var tmp2 = res[4];
+                    res[3] = res[0];
+                    res[4] = res[1];
+                    res[0] = tmp1;
+                    res[1] = tmp2;
+                }
+            }
             return res;
         }
 

XdCxRhDW.App/Controllers/PosController.cs

@@ -664,7 +664,7 @@ namespace XdCxRhDW.App.Controllers
                         TarFreqDown = dto.FreqDown,
                     });
                     double[] res;
-                    res = PosApi.X2D1_PosNoRef(cgRes, StationRes, dto.CalcConfidence);
+                    res = PosApi.X2D1_PosNoRef_ZL(cgRes, StationRes, dto.CalcConfidence);
                     PosRes posRes = new PosRes()
                     {
                         FreqUpHz = (long)cgRes.TarFreqUp.Value,
@@ -837,7 +837,7 @@ namespace XdCxRhDW.App.Controllers
                         TarFreqDown = dto.FreqDown,
                     });
                     double[] res;
-                    res = PosApi.X2D1_PosNoRef(cgRes, StationRes, dto.CalcConfidence);
+                    res = PosApi.X2D1_PosNoRef_ZL(cgRes, StationRes, dto.CalcConfidence);
                     PosRes posRes = new PosRes()
                     {
                         FreqUpHz = (long)cgRes.TarFreqUp.Value,

XdCxRhDW.App/EditForms/X2D1DTOParamEditor.cs

@@ -376,7 +376,9 @@ namespace XdCxRhDW.App.EditForms
                         Adja1Z = dto.AdjaZ,
                     };
                     double[] res;
-                    res = PosApi.X2D1_PosNoRef(cgRes, StationRes, dto.CalcConfidence);
+                    //res = PosApi.X2D1_PosNoRef(cgRes, StationRes, dto.CalcConfidence);
+                    res = PosApi.X2D1_PosNoRef_ZL(cgRes, StationRes, dto.CalcConfidence);
+
                     this.txtPosRes.Text = $"{info.PosResType.GetEnumDisplayName()}定位点:[{res[0]:f4},{res[1]:f4}] 镜像点:[{res[3]:f4},{res[4]:f4}]";
                 }
                 else

XdCxRhDW.App/UserControl/CtrlHome.cs

@@ -1173,7 +1173,7 @@ namespace XdCxRhDW.App.UserControl
                             }
                             else
                             {
-                                res = PosApi.X2D1_PosNoRef(CgRes, StationRes);
+                                res = PosApi.X2D1_PosNoRef_ZL(CgRes, StationRes);
                             }
                             item.PosLon = res[0];
                             item.PosLat = res[1];