LeoPosApi.cs 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Configuration;
  4. using System.Linq;
  5. using System.Runtime.InteropServices;
  6. using System.Security.Policy;
  7. using System.Text;
  8. using System.Threading.Tasks;
  9. using XdCxRhDW.Entity;
  10. namespace XdCxRhDW.Api
  11. {
  12. public static class LeoPosApi
  13. {
  14. //低轨道单星和双星定位
  15. private const string leoPos = @"AddIns\低轨\DLL_XDXZ.dll";
  16. private static double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
  17. /// <summary>
  18. /// 地轨双星dw
  19. /// </summary>
  20. /// <param name="main_sat"></param>
  21. /// <param name="neigh_sat"></param>
  22. /// <param name="Ref_Station_LLH"></param>
  23. /// <param name="Zone"></param>
  24. /// <param name="target_dto"></param>
  25. /// <param name="target_dfo"></param>
  26. /// <param name="ref_dto"></param>
  27. /// <param name="ref_dfo"></param>
  28. /// <param name="fu1"></param>
  29. /// <param name="fu2"></param>
  30. /// <param name="target_llh"></param>
  31. [DllImport(leoPos, EntryPoint = "TwoStar_DTFO_DW", CallingConvention = CallingConvention.Cdecl)]
  32. public extern static void TwoStar_DTFO_DW(double[] main_sat, double[] neigh_sat, double[] Ref_Station_LLH
  33. , double[] Zone, double target_dto, double target_dfo,
  34. double ref_dto, double ref_dfo, double fu1, double fu2, double[] target_llh);
  35. /// <summary>
  36. /// 单星dw
  37. /// </summary>
  38. /// <param name="main_sat">第一时刻星历 长度6</param>
  39. /// <param name="neigh_sat1">第二时刻星历 长度6</param>
  40. /// <param name="neigh_sat2">第三时刻星历 长度6</param>
  41. /// <param name="Zone"></param>
  42. /// <param name="target_dfo1">频差1(Hz)</param>
  43. /// <param name="target_dfo2">频差2(Hz)</param>
  44. /// <param name="fu">上行频点(Hz)</param>
  45. /// <param name="target_llh"></param>
  46. [DllImport(leoPos, EntryPoint = "SingleStar_DFO_DW", CallingConvention = CallingConvention.Cdecl)]
  47. public extern static void SingleStar_DFO_DW(double[] main_sat, double[] neigh_sat1, double[] neigh_sat2
  48. , double[] Zone, double target_dfo1, double target_dfo2, double fu, double[] target_llh);
  49. /// <summary>
  50. /// 低轨双星时差线
  51. /// </summary>
  52. /// <param name="main_sat_pos">主星星历 长度6</param>
  53. /// <param name="neigh_sat_pos">邻星星历 长度6</param>
  54. /// <param name="ref_pos">参考站位置</param>
  55. /// <param name="Zone">-85, 85, -180, 180</param>
  56. /// <param name="target_dto">目标时差(s)</param>
  57. /// <param name="ref_dto">参考时差(s)</param>
  58. /// <param name="LOP_Value"></param>
  59. /// <param name="LOP_Len"></param>
  60. [DllImport(leoPos, EntryPoint = "TwoStar_SCX", CallingConvention = CallingConvention.Cdecl)]
  61. public extern static void TwoStar_SCX(double[] main_sat_pos, double[] neigh_sat_pos, double[] ref_pos, double[] Zone,
  62. double target_dto, double ref_dto, out IntPtr LOP_Value, ref int LOP_Len);
  63. /// <summary>
  64. /// 低轨双星频差线
  65. /// </summary>
  66. /// <param name="main_sat"></param>
  67. /// <param name="neigh_sat"></param>
  68. /// <param name="ref_pos"></param>
  69. /// <param name="Zone"></param>
  70. /// <param name="target_dfo">目标频差(Hz)</param>
  71. /// <param name="ref_dfo">参考频差(Hz)</param>
  72. /// <param name="fu1">上行频点1(Hz)</param>
  73. /// <param name="fu2">上行频点2(Hz)</param>
  74. /// <param name="LOP_Value"></param>
  75. /// <param name="LOP_Len"></param>
  76. [DllImport(leoPos, EntryPoint = "TwoStar_PCX", CallingConvention = CallingConvention.Cdecl)]
  77. public extern static void TwoStar_PCX(double[] main_sat, double[] neigh_sat, double[] ref_pos,
  78. double[] Zone, double target_dfo, double ref_dfo, double fu1, double fu2, out IntPtr LOP_Value, ref int LOP_Len);
  79. /// <summary>
  80. /// 单星频差线
  81. /// </summary>
  82. /// <param name="main_sat"></param>
  83. /// <param name="neigh_sat"></param>
  84. /// <param name="Zone"></param>
  85. /// <param name="target_dfo"></param>
  86. /// <param name="fu"></param>
  87. /// <param name="LOP_Value"></param>
  88. /// <param name="LOP_Len"></param>
  89. [DllImport(leoPos, EntryPoint = "SingleStar_PCX", CallingConvention = CallingConvention.Cdecl)]
  90. public extern static void SingleStar_PCX(double[] main_sat, double[] neigh_sat
  91. , double[] Zone, double target_dfo, double fu, out IntPtr LOP_Value, ref int LOP_Len);
  92. public static double[] X1_POS(CgRes cg, double upfreqHz)
  93. {
  94. double[] mainSat = new double[6] { cg.MainX.Value, cg.MainY.Value, cg.MainZ.Value, cg.MainVx.Value, cg.MainVy.Value, cg.MainVz.Value };
  95. double[] adjaSat1 = new double[6] { cg.Adja1X.Value, cg.Adja1Y.Value, cg.Adja1Z.Value, cg.Adja1Vx.Value, cg.Adja1Vy.Value, cg.Adja1Vz.Value };
  96. double[] adjaSat2 = new double[6] { cg.Adja2X.Value, cg.Adja2Y.Value, cg.Adja2Z.Value, cg.Adja2Vx.Value, cg.Adja2Vy.Value, cg.Adja2Vz.Value };
  97. double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
  98. double[] res = new double[6];
  99. SingleStar_DFO_DW(mainSat, adjaSat1, adjaSat2, zone, cg.Dfo1.Value, cg.Dfo2.Value, upfreqHz, res);
  100. return ConvertToGeoPoint(res);
  101. }
  102. /// <summary>
  103. ///
  104. /// </summary>
  105. /// <param name="cg"></param>
  106. /// <param name="refTx"></param>
  107. /// <param name="upfreqHz1">目标上行频点</param>
  108. /// <param name="upfreqHz2">参考上行频点</param>
  109. /// <returns></returns>
  110. public static double[] X2_POS(CgRes cg, StationRes sRes, double upfreqHz1, double upfreqHz2)
  111. {
  112. double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
  113. double[] mainSat = new double[6] { cg.MainX.Value, cg.MainY.Value, cg.MainZ.Value, cg.MainVx.Value, cg.MainVy.Value, cg.MainVz.Value };
  114. double[] adjaSat1 = new double[6] { cg.Adja1X.Value, cg.Adja1Y.Value, cg.Adja1Z.Value, cg.Adja1Vx.Value, cg.Adja1Vy.Value, cg.Adja1Vz.Value };
  115. double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
  116. double[] res = new double[6];
  117. var refDto = (cg.YbMainDto.Value - cg.YbAdja1Dto.Value) / 1e6;
  118. var refDfo = cg.YbMainDfo.Value - cg.YbAdja1Dfo.Value;
  119. TwoStar_DTFO_DW(mainSat, adjaSat1, refStation, zone, cg.Dto1.Value / 1e6, cg.Dfo1.Value, refDto, refDfo, upfreqHz1, upfreqHz2, res);
  120. return ConvertToGeoPoint(res);
  121. }
  122. private static double[] ConvertToGeoPoint(double[] res)
  123. {
  124. if (res == null || res.Length == 0)
  125. {
  126. return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
  127. }
  128. else if (res.Length != 3 && res.Length != 6)
  129. {
  130. throw new Exception("定位结果解析异常,长度不是3或6");
  131. }
  132. var list = res.Select(p => Math.Round(p, 4)).ToArray();
  133. if (list.Length == 3)
  134. {
  135. list = list.Concat(new double[4] { 999, 999, 0, -1 }).ToArray();
  136. }
  137. list[2] = list[5] = 0;//高度
  138. if (double.IsNaN(list[0]) || double.IsNaN(list[1]))
  139. {
  140. list[0] = list[1] = 999;
  141. }
  142. if (double.IsNaN(list[3]) || double.IsNaN(list[4]))
  143. {
  144. list[3] = list[4] = 999;
  145. }
  146. if (list[0] < -180 || list[0] > 180)
  147. {
  148. list[0] = list[1] = 999;
  149. }
  150. else if (list[1] < -90 || list[1] > 90)
  151. {
  152. list[0] = list[1] = 999;
  153. }
  154. if (list[3] < -180 || list[3] > 180)
  155. {
  156. list[3] = list[4] = 999;
  157. }
  158. else if (list[4] < -90 || list[4] > 90)
  159. {
  160. list[3] = list[4] = 999;
  161. }
  162. if (list.Length == 6)
  163. {
  164. var listNew = list.ToList();
  165. listNew.Add(-1);
  166. list = listNew.ToArray();
  167. }
  168. return list;
  169. }
  170. public static IEnumerable<(double lon, double lat)> DtoLineLeoX2(DtoLineTwoStartOption opt)
  171. {
  172. List<DtoLinePoint> list = new List<DtoLinePoint>();
  173. IntPtr LOP_ValuePtr;
  174. int LOP_Len = 0;
  175. TwoStar_SCX(
  176. opt.MsEph,
  177. opt.NsEph,
  178. opt.RefGeod,
  179. zone,
  180. opt.TargetDto * 1e-6,
  181. opt.RefDto * 1e-6, out LOP_ValuePtr, ref LOP_Len);
  182. double[] LOP_Value = new double[LOP_Len * 3];
  183. if (LOP_Len > 0)
  184. {
  185. Marshal.Copy(LOP_ValuePtr, LOP_Value, 0, LOP_Value.Length);
  186. list = OutputHelper.WriteDtoLine(LOP_Value, LOP_Len);
  187. }
  188. var Lines = list.Select(p => (p.Lon, p.Lat));
  189. return Lines;
  190. }
  191. public static IEnumerable<(double lon, double lat)> DfoLineLeo2(LeoDfoLineTwo opt)
  192. {
  193. IntPtr LOP_ValuePtr;
  194. int LOP_Len = 0;
  195. TwoStar_PCX(
  196. opt.MsEph,
  197. opt.NsEph,
  198. opt.RefGeod,
  199. zone,
  200. opt.TargetDfo,
  201. opt.RefDfo,
  202. opt.fu1,
  203. opt.fu2, out LOP_ValuePtr, ref LOP_Len);
  204. return ParseResult(LOP_ValuePtr, LOP_Len);
  205. }
  206. public static IEnumerable<(double lon, double lat)> DfoLineLeoX1(double[] main_sat, double[] neigh_sat, double target_dfo, double fu)
  207. {
  208. IntPtr LOP_ValuePtr;
  209. int LOP_Len = 0;
  210. SingleStar_PCX(main_sat, neigh_sat
  211. , zone, target_dfo, fu, out LOP_ValuePtr, ref LOP_Len);
  212. return ParseResult(LOP_ValuePtr, LOP_Len);
  213. }
  214. private static IEnumerable<(double lon, double lat)> ParseResult(IntPtr LOP_ValuePtr, int LOP_Len)
  215. {
  216. List<DtoLinePoint> list = new List<DtoLinePoint>();
  217. double[] LOP_Value = new double[LOP_Len * 3];
  218. if (LOP_Len > 0)
  219. {
  220. Marshal.Copy(LOP_ValuePtr, LOP_Value, 0, LOP_Value.Length);
  221. for (int idx = 0; idx < LOP_Len; ++idx)
  222. {
  223. if (LOP_Value[3 * idx + 1] != -1)
  224. {
  225. list.Add(new DtoLinePoint()
  226. {
  227. Lon = LOP_Value[3 * idx + 1],
  228. Lat = LOP_Value[3 * idx]
  229. });
  230. }
  231. }
  232. for (int idx = 0; idx < LOP_Len; ++idx)
  233. {
  234. if (LOP_Value[3 * idx + 2] != -1)
  235. {
  236. list.Add(new DtoLinePoint()
  237. {
  238. Lon = LOP_Value[3 * idx + 2],
  239. Lat = LOP_Value[3 * idx]
  240. });
  241. }
  242. }
  243. }
  244. var Lines = list.Select(p => (p.Lon, p.Lat));
  245. return Lines;
  246. }
  247. }
  248. }