TheoryDtoDfoApi.cs 5.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Linq;
  4. using System.Runtime.InteropServices;
  5. using System.Text;
  6. using System.Threading.Tasks;
  7. using XdCxRhDW.Entity;
  8. namespace XdCxRhDW.Api
  9. {
  10. /// <summary>
  11. /// 理论时频差API
  12. /// </summary>
  13. public static class TheoryDtoDfoApi
  14. {
  15. #region cpp dll Interop
  16. //一星一地测向带参定位
  17. private const string dll = @"AddIns\GDOP误差椭圆\GDOP_Draw_11.dll";
  18. //主接收站位置[3],
  19. //邻接收站位置[3]
  20. //主星历[6](x,y,z,Vx,Vy,Vz)
  21. //邻星历[6](x,y,z,Vx,Vy,Vz)
  22. //上行频(单位,Hz)
  23. //下行频(单位,Hz)
  24. [DllImport(dll, EntryPoint = "theryDfo", CallingConvention = CallingConvention.Cdecl)]//双星理论频差
  25. private extern static double TheoryDfo(double[] target_llh, double[] rec_llh1, double[] rec_llh2, double[] main_sat, double[] neigh_sat, double fu, double fd);
  26. #endregion
  27. public static EnumTargetState TheoryDfo(CgRes cRes, StationRes sRes, PosRes posRes, int YDPZThreshold)
  28. {
  29. cRes.MainVx = 1;
  30. cRes.MainVy = 2;
  31. cRes.MainVz = 3;
  32. cRes.Adja1Vx = 3;
  33. cRes.Adja1Vy = 3;
  34. cRes.Adja1Vz = 3;
  35. //张老板说理论频差如果和参估计算出来的频差差值比较大,就说明目标在运动(多路参估每一路都要比较,有一路判定运动则为运动)
  36. //?具体多少差异需要确定一下
  37. //主邻星本振不一样时该怎么计算
  38. if (cRes.TarFreqUp == null || cRes.TarFreqDown == null || YDPZThreshold <= 0)
  39. return EnumTargetState.Unknown;
  40. double[] target = new double[3] { posRes.PosLon, posRes.PosLat, 0 };
  41. double[] rec = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
  42. if (cRes.MainVx == null || cRes.MainVy == null || cRes.MainVz == null) return EnumTargetState.Unknown;
  43. double[] xlMain = new double[6] { cRes.MainX.Value, cRes.MainY.Value, cRes.MainZ.Value, cRes.MainVx.Value, cRes.MainVy.Value, cRes.MainVz.Value };
  44. if (posRes.PosResType == EnumPosResType.X2D1 || posRes.PosResType == EnumPosResType.X2D1NoRef || posRes.PosResType == EnumPosResType.RH)
  45. {
  46. if (cRes.Dfo1 == null) return EnumTargetState.Unknown;
  47. var xlAdja1 = new double[6] { cRes.Adja1X.Value, cRes.Adja1Y.Value, cRes.Adja1Z.Value, cRes.Adja1Vx.Value, cRes.Adja1Vy.Value, cRes.Adja1Vz.Value };
  48. double res = TheoryDfo(target, rec, rec, xlMain, xlAdja1, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
  49. var val = Math.Abs(res - cRes.Dfo1.Value);
  50. if (val < YDPZThreshold)
  51. return EnumTargetState.Stationary;
  52. else
  53. return EnumTargetState.Movement;
  54. }
  55. else if (posRes.PosResType == EnumPosResType.X2Dfo)
  56. {
  57. if (cRes.Dfo1 == null) return EnumTargetState.Unknown;
  58. var xlAdja1 = new double[6] { cRes.Adja1X.Value, cRes.Adja1Y.Value, cRes.Adja1Z.Value, cRes.Adja1Vx.Value, cRes.Adja1Vy.Value, cRes.Adja1Vz.Value };
  59. if (cRes.Adja1Vx == null || cRes.Adja1Vy == null || cRes.Adja1Vz == null) return EnumTargetState.Unknown;
  60. double res = TheoryDfo(target, rec, rec, xlMain, xlAdja1, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
  61. var val = Math.Abs(res - cRes.Dfo1.Value);
  62. if (val < YDPZThreshold)
  63. return EnumTargetState.Stationary;
  64. else
  65. return EnumTargetState.Movement;
  66. }
  67. else if (posRes.PosResType == EnumPosResType.X1D1CX)
  68. {
  69. //星地不知道接口是否支持
  70. return EnumTargetState.Unknown;
  71. }
  72. else if (posRes.PosResType == EnumPosResType.X3 || posRes.PosResType == EnumPosResType.X3NoRef || posRes.PosResType == EnumPosResType.X3TwoDfo)
  73. {
  74. if (cRes.Dfo1 == null && cRes.Dfo2 == null) return EnumTargetState.Unknown;
  75. if (cRes.Dfo1 != null)
  76. {
  77. var xlAdja1 = new double[6] { cRes.Adja1X.Value, cRes.Adja1Y.Value, cRes.Adja1Z.Value, cRes.Adja1Vx.Value, cRes.Adja1Vy.Value, cRes.Adja1Vz.Value };
  78. if (cRes.Adja1Vx == null || cRes.Adja1Vy == null || cRes.Adja1Vz == null) return EnumTargetState.Unknown;
  79. double res = TheoryDfo(target, rec, rec, xlMain, xlAdja1, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
  80. var val = Math.Abs(res - cRes.Dfo1.Value);
  81. if (val >= YDPZThreshold)
  82. return EnumTargetState.Movement;
  83. }
  84. if (cRes.Dfo2 != null)
  85. {
  86. var xlAdja2 = new double[6] { cRes.Adja2X.Value, cRes.Adja2Y.Value, cRes.Adja2Z.Value, cRes.Adja2Vx.Value, cRes.Adja2Vy.Value, cRes.Adja2Vz.Value };
  87. if (cRes.Adja2Vx == null || cRes.Adja2Vy == null || cRes.Adja2Vz == null) return EnumTargetState.Unknown;
  88. double res = TheoryDfo(target, rec, rec, xlMain, xlAdja2, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
  89. var val = Math.Abs(res - cRes.Dfo2.Value);
  90. if (val >= YDPZThreshold)
  91. return EnumTargetState.Movement;
  92. }
  93. return EnumTargetState.Stationary;
  94. }
  95. else
  96. {
  97. return EnumTargetState.Unknown;
  98. }
  99. }
  100. }
  101. }