GdopHelper.cs 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438
  1. 
  2. using System;
  3. using System.Collections.Generic;
  4. using System.Linq;
  5. using System.Reflection.Emit;
  6. using System.Runtime.InteropServices;
  7. namespace XdCxRhDW.Api
  8. {
  9. public static class GdopHelper
  10. {
  11. static readonly DateTime dtZero = new DateTime(1970, 1, 1, 8, 0, 0, 0);
  12. /// <summary>
  13. /// 两星一地GDOP 无参时参考位置不赋值
  14. /// </summary>
  15. /// <param name="mainEph">主星星历 x y z vx vy vz</param>
  16. /// <param name="adajEph">邻星星历x y z vx vy vz</param>
  17. /// <param name="cdbPos">超短波位置 3</param>
  18. /// <param name="refPos">参考站位置 3</param>
  19. /// <param name="dtousErr">时差误差</param>
  20. /// <param name="ephLocErr">星历误差</param>
  21. /// <param name="refPos"></param>
  22. /// <returns></returns>
  23. public static List<ErrDistanceMapPoints> Gdop2Sat1DByXyz(double[] mainEph, double[] adajEph, double[] cdbPos, double dtousErr, double ephLocErr, double[] refPos = null)
  24. {
  25. int satCount = 2;
  26. //该值和points 一一对应
  27. double[] level = GdopParam.误差配置.误差距离m;
  28. double[] satllh = new double[satCount * 3];
  29. IntPtr res = IntPtr.Zero;
  30. int[] resCount = new int[level.Length];
  31. if (refPos == null || refPos.Length == 0)
  32. {
  33. GDOPApi.Gdop2Sat1DNoRefByXyz(mainEph, adajEph, cdbPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);
  34. }
  35. else
  36. {
  37. GDOPApi.Gdop2Sat1DRefByXyz(mainEph, adajEph, cdbPos, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);
  38. }
  39. IntPtr tmp = res;
  40. //用于绘制的数据
  41. List<double[]> points = new List<double[]>();
  42. for (int idx = 0; idx < level.Length; ++idx)
  43. {
  44. double[] levelval = new double[resCount[idx]];
  45. Marshal.Copy(tmp, levelval, 0, resCount[idx]);
  46. tmp += (resCount[idx] * sizeof(double));
  47. points.Add(levelval);
  48. }
  49. GDOPApi.FreeGDOPBuf(res);
  50. List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
  51. for (int i = 0; i < points.Count; i++)
  52. {
  53. if (!points[i].Any()) continue;
  54. ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
  55. errDistanceMap.ErrDistance = level[i];
  56. errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
  57. errs.Add(errDistanceMap);
  58. }
  59. return errs;
  60. }
  61. /// <summary>
  62. /// 两星一地GDOP 无参时参考位置不赋值
  63. /// </summary>
  64. /// <param name="mainEph">主星星历 x y z vx vy vz</param>
  65. /// <param name="adajEph">邻星星历x y z vx vy vz</param>
  66. /// <param name="cdbPos">超短波位置 3</param>
  67. /// <param name="refPos">参考站位置 3</param>
  68. /// <param name="dtousErr">时差误差</param>
  69. /// <param name="ephLocErr">星历误差</param>
  70. /// <param name="refPos"></param>
  71. /// <returns></returns>
  72. public static List<ErrDistanceMapPoints> Gdop2Sat1DByXyzNew(double[] mainEph, double[] adajEph, double[] cdbPos, double dtousErr, double ephLocErr, double[] refPos = null)
  73. {
  74. int satCount = 2;
  75. //该值和points 一一对应
  76. double[] level = GdopParam.误差配置.误差距离m;
  77. double[] satllh = new double[satCount * 3];
  78. IntPtr res = IntPtr.Zero;
  79. IntPtr lpoints = IntPtr.Zero;
  80. int[] resCount = new int[level.Length];
  81. if (refPos == null || refPos.Length == 0)
  82. {
  83. GDOPApi.Gdop2Sat1DNoRefByXyz_new(mainEph, adajEph, cdbPos, dtousErr, ephLocErr, level, level.Length, resCount, out lpoints, out res, satllh);
  84. }
  85. else
  86. {
  87. GDOPApi.Gdop2Sat1DRefByXyz_new(mainEph, adajEph, cdbPos, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out lpoints, out res, satllh);
  88. }
  89. var errs= ToErrDistanceMapPoints(level,resCount,lpoints,res);
  90. GDOPApi.FreeGDOPBuf(res);
  91. GDOPApi.FreeGDOPBuf(lpoints);
  92. return errs;
  93. }
  94. private static List<ErrDistanceMapPoints> ToErrDistanceMapPoints(double[] level,int[] resCount, IntPtr lpoints, IntPtr res)
  95. {
  96. int total = resCount.Sum(r => r);
  97. int[] Points_Value = new int[total];
  98. Marshal.Copy(lpoints, Points_Value, 0, Points_Value.Length);
  99. int totalPoint = Points_Value.Sum(p => p);
  100. double[] LOP_Value = new double[totalPoint * 2];
  101. Marshal.Copy(res, LOP_Value, 0, LOP_Value.Length);
  102. var points = ParseResult(LOP_Value);
  103. //用于绘制的数据
  104. List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
  105. int skippointcount = 0;
  106. int skipcount = 0;
  107. int count = 0;
  108. for (int idx = 0; idx < level.Length; ++idx)
  109. {
  110. int levelcount = resCount[idx];
  111. skipcount = idx == 0 ? 0 : skipcount + resCount[idx - 1];
  112. for (int i = skipcount; i < levelcount + skipcount; i++)
  113. {
  114. int pointcount = Points_Value[i];
  115. skippointcount = count == 0 ? 0 : skippointcount + Points_Value[i - 1];
  116. var mapDots = points.Skip(skippointcount).Take(pointcount);
  117. if (!mapDots.Any()) continue;
  118. ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
  119. errDistanceMap.ErrDistance = level[idx];
  120. errDistanceMap.MapDots.AddRange(mapDots);
  121. errs.Add(errDistanceMap);
  122. count++;
  123. }
  124. }
  125. return errs;
  126. }
  127. /// <summary>
  128. /// 一星一地GDOP
  129. /// </summary>
  130. /// <param name="mainLines">主星星历</param>
  131. /// <param name="captime">信号时间</param>
  132. /// <param name="cdbPos">超短波位置 3</param>
  133. /// <param name="cxPos">测向站位置 3</param>
  134. /// <param name="dtousErr">时差误差</param>
  135. /// <param name="doaErr">测向误差</param>
  136. /// <param name="ephLocErr">星历误差</param>
  137. /// <param name="refPos">参考站位置 3</param>
  138. /// <returns></returns>
  139. public static List<ErrDistanceMapPoints> Gdop1Sat1DByXyz(double[] mainEph, double[] cdbPos, double[] cxPos, double dtousErr, double doaErr, double ephLocErr, double[] refPos = null)
  140. {
  141. int satCount = 1;
  142. //该值和points 一一对应
  143. double[] level = GdopParam.误差配置.误差距离m;
  144. double[] satllh = new double[satCount * 3];
  145. IntPtr res = IntPtr.Zero;
  146. int[] resCount = new int[level.Length];
  147. if (refPos == null || refPos.Length == 0)
  148. {
  149. GDOPApi.GdopXDCXNoRefByXyz(mainEph, cdbPos, cxPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out res, satllh);
  150. }
  151. else
  152. {
  153. GDOPApi.GdopXDCXRefByXyz(mainEph, cdbPos, cxPos, refPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount, out res, satllh);
  154. }
  155. IntPtr tmp = res;
  156. //用于绘制的数据
  157. List<double[]> points = new List<double[]>();
  158. for (int idx = 0; idx < level.Length; ++idx)
  159. {
  160. double[] levelval = new double[resCount[idx]];
  161. Marshal.Copy(tmp, levelval, 0, resCount[idx]);
  162. tmp += (resCount[idx] * sizeof(double));
  163. points.Add(levelval);
  164. }
  165. GDOPApi.FreeGDOPBuf(res);
  166. List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
  167. for (int i = 0; i < points.Count; i++)
  168. {
  169. if (!points[i].Any()) continue;
  170. ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
  171. errDistanceMap.ErrDistance = level[i];
  172. errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
  173. errs.Add(errDistanceMap);
  174. }
  175. return errs;
  176. }
  177. public static List<ErrDistanceMapPoints> Gdop1Sat1DByXyzNew(double[] mainEph, double[] cdbPos, double[] cxPos, double dtousErr, double doaErr, double ephLocErr, double[] refPos = null)
  178. {
  179. int satCount = 1;
  180. //该值和points 一一对应
  181. double[] level = GdopParam.误差配置.误差距离m;
  182. double[] satllh = new double[satCount * 3];
  183. IntPtr res = IntPtr.Zero;
  184. IntPtr lpoints = IntPtr.Zero;
  185. int[] resCount = new int[level.Length];
  186. if (refPos == null || refPos.Length == 0)
  187. {
  188. GDOPApi.GdopXDCXNoRefByXyz_new(mainEph, cdbPos, cxPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount,out lpoints, out res, satllh);
  189. }
  190. else
  191. {
  192. GDOPApi.GdopXDCXRefByXyz_new(mainEph, cdbPos, cxPos, refPos, dtousErr, doaErr, ephLocErr, level, level.Length, resCount,out lpoints, out res, satllh);
  193. }
  194. var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
  195. GDOPApi.FreeGDOPBuf(res);
  196. GDOPApi.FreeGDOPBuf(lpoints);
  197. return errs;
  198. }
  199. public static List<ErrDistanceMapPoints> Gdop3SatByXyz(double[] mainEph, double[] adaj1Eph, double[] adaj2Eph,
  200. double dtousErr, double ephLocErr, double[] refPos = null)
  201. {
  202. int satCount = 3;
  203. //该值和points 一一对应
  204. double[] level = GdopParam.误差配置.误差距离m;
  205. double[] satllh = new double[satCount * 3];
  206. IntPtr res = IntPtr.Zero;
  207. int[] resCount = new int[level.Length];
  208. if (refPos == null || refPos.Length == 0)
  209. {
  210. GDOPApi.Gdop3SatNoRefByXyz(mainEph, adaj1Eph, adaj2Eph, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);
  211. }
  212. else
  213. {
  214. GDOPApi.Gdop3SatRefByXyz(mainEph, adaj1Eph, adaj2Eph, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out res, satllh);
  215. }
  216. IntPtr tmp = res;
  217. //用于绘制的数据
  218. List<double[]> points = new List<double[]>();
  219. for (int idx = 0; idx < level.Length; ++idx)
  220. {
  221. double[] levelval = new double[resCount[idx]];
  222. Marshal.Copy(tmp, levelval, 0, resCount[idx]);
  223. tmp += (resCount[idx] * sizeof(double));
  224. points.Add(levelval);
  225. }
  226. GDOPApi.FreeGDOPBuf(res);
  227. List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
  228. for (int i = 0; i < points.Count; i++)
  229. {
  230. if (!points[i].Any()) continue;
  231. ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
  232. errDistanceMap.ErrDistance = level[i];
  233. errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
  234. errs.Add(errDistanceMap);
  235. }
  236. return errs;
  237. }
  238. public static List<ErrDistanceMapPoints> Gdop3SatByXyzNew(double[] mainEph, double[] adaj1Eph, double[] adaj2Eph,
  239. double dtousErr, double ephLocErr, double[] refPos = null)
  240. {
  241. int satCount = 3;
  242. //该值和points 一一对应
  243. double[] level = GdopParam.误差配置.误差距离m;
  244. double[] satllh = new double[satCount * 3];
  245. IntPtr res = IntPtr.Zero;
  246. IntPtr lpoints = IntPtr.Zero;
  247. int[] resCount = new int[level.Length];
  248. if (refPos == null || refPos.Length == 0)
  249. {
  250. GDOPApi.Gdop3SatNoRefByXyz_new(mainEph, adaj1Eph, adaj2Eph, dtousErr, ephLocErr, level, level.Length, resCount,out lpoints, out res, satllh);
  251. }
  252. else
  253. {
  254. GDOPApi.Gdop3SatRefByXyz_new(mainEph, adaj1Eph, adaj2Eph, refPos, dtousErr, ephLocErr, level, level.Length, resCount, out lpoints, out res, satllh);
  255. }
  256. var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
  257. GDOPApi.FreeGDOPBuf(res);
  258. GDOPApi.FreeGDOPBuf(lpoints);
  259. return errs;
  260. }
  261. public static List<ErrDistanceMapPoints> Gdop3SatDFByXyz(double[] mainEph, double[] adaj1Eph, double[] adaj2Eph,
  262. double fuHz1, double fuHz2, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
  263. {
  264. int satCount = 3;
  265. //该值和points 一一对应
  266. double[] level = GdopParam.误差配置.误差距离m;
  267. double[] satllh = new double[satCount * 3];
  268. IntPtr res = IntPtr.Zero;
  269. int[] resCount = new int[level.Length];
  270. GDOPApi.Gdop3SatDFByXyz(mainEph, adaj1Eph, adaj2Eph, refPos, fuHz1, fuHz2, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out res, satllh);
  271. IntPtr tmp = res;
  272. //用于绘制的数据
  273. List<double[]> points = new List<double[]>();
  274. for (int idx = 0; idx < level.Length; ++idx)
  275. {
  276. double[] levelval = new double[resCount[idx]];
  277. Marshal.Copy(tmp, levelval, 0, resCount[idx]);
  278. tmp += (resCount[idx] * sizeof(double));
  279. points.Add(levelval);
  280. }
  281. GDOPApi.FreeGDOPBuf(res);
  282. List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
  283. for (int i = 0; i < points.Count; i++)
  284. {
  285. if (!points[i].Any()) continue;
  286. ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
  287. errDistanceMap.ErrDistance = level[i];
  288. errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
  289. errs.Add(errDistanceMap);
  290. }
  291. return errs;
  292. }
  293. public static List<ErrDistanceMapPoints> Gdop2SatDRefByXyz(double[] mainEph, double[] adajEph, double fuHz1, double fuHz2, double dtousErr, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
  294. {
  295. int satCount = 2;
  296. //该值和points 一一对应
  297. double[] level = GdopParam.误差配置.误差距离m;
  298. double[] satllh = new double[satCount * 3];
  299. IntPtr res = IntPtr.Zero;
  300. int[] resCount = new int[level.Length];
  301. GDOPApi.Gdop2SatDRefByXyz(mainEph, adajEph, refPos, fuHz1, fuHz2, dtousErr, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out res, satllh);
  302. IntPtr tmp = res;
  303. //用于绘制的数据
  304. List<double[]> points = new List<double[]>();
  305. for (int idx = 0; idx < level.Length; ++idx)
  306. {
  307. double[] levelval = new double[resCount[idx]];
  308. Marshal.Copy(tmp, levelval, 0, resCount[idx]);
  309. tmp += (resCount[idx] * sizeof(double));
  310. points.Add(levelval);
  311. }
  312. GDOPApi.FreeGDOPBuf(res);
  313. List<ErrDistanceMapPoints> errs = new List<ErrDistanceMapPoints>();
  314. for (int i = 0; i < points.Count; i++)
  315. {
  316. if (!points[i].Any()) continue;
  317. ErrDistanceMapPoints errDistanceMap = new ErrDistanceMapPoints();
  318. errDistanceMap.ErrDistance = level[i];
  319. errDistanceMap.MapDots.AddRange(ParseResult(points[i]));
  320. errs.Add(errDistanceMap);
  321. }
  322. return errs;
  323. }
  324. private static List<(double lon, double lat)> ParseResult(double[] ponits)
  325. {
  326. List<(double lon,double lat)> mapDots = new List<(double lon, double lat)>();
  327. int count = 2;
  328. for (int i = 0; i < ponits.Length / count; i++)
  329. {
  330. var Lon = ponits[count * i];
  331. var Lat = ponits[count * i + 1];//0 1 2 3 4 5 6 7
  332. mapDots.Add((Lon, Lat));
  333. }
  334. return mapDots;
  335. }
  336. public static List<ErrDistanceMapPoints> Gdop3SatDFByXyzNew(double[] mainEph, double[] adaj1Eph, double[] adaj2Eph,
  337. double fuHz1, double fuHz2, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
  338. {
  339. int satCount = 3;
  340. //该值和points 一一对应
  341. double[] level = GdopParam.误差配置.误差距离m;
  342. double[] satllh = new double[satCount * 3];
  343. IntPtr res = IntPtr.Zero;
  344. IntPtr lpoints = IntPtr.Zero;
  345. int[] resCount = new int[level.Length];
  346. GDOPApi.Gdop3SatDFByXyz_new(mainEph, adaj1Eph, adaj2Eph, refPos, fuHz1, fuHz2, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount,out lpoints, out res, satllh);
  347. var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
  348. GDOPApi.FreeGDOPBuf(res);
  349. GDOPApi.FreeGDOPBuf(lpoints);
  350. return errs;
  351. }
  352. public static List<ErrDistanceMapPoints> Gdop2SatDRefByXyzNew(double[] mainEph, double[] adajEph, double fuHz1, double fuHz2, double dtousErr, double dfoErr, double ephLocErr, double ephVErr, double[] refPos)
  353. {
  354. int satCount = 2;
  355. //该值和points 一一对应
  356. double[] level = GdopParam.误差配置.误差距离m;
  357. double[] satllh = new double[satCount * 3];
  358. IntPtr res = IntPtr.Zero;
  359. IntPtr lpoints = IntPtr.Zero;
  360. int[] resCount = new int[level.Length];
  361. GDOPApi.Gdop2SatDRefByXyz_new(mainEph, adajEph, refPos, fuHz1, fuHz2, dtousErr, dfoErr, ephLocErr, ephVErr, level, level.Length, resCount, out lpoints, out res, satllh);
  362. var errs = ToErrDistanceMapPoints(level, resCount, lpoints, res);
  363. GDOPApi.FreeGDOPBuf(res);
  364. GDOPApi.FreeGDOPBuf(lpoints);
  365. return errs;
  366. }
  367. }
  368. }